Add example documentation for trajectory_point_interface

Author: urfeex

doc/examples.rst

@@ -24,4 +24,5 @@ may be running forever until manually stopped.
    examples/script_sender
    examples/spline_example
    examples/tool_contact_example
+   examples/trajectory_point_interface
    examples/ur_driver

doc/examples/trajectory_point_interface.rst

@@ -0,0 +1,103 @@
+:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/trajecotry_joint_interface_example.rst
+
+.. _trajecotry_joint_interface_example:
+
+Trajectory Joint Interface example
+==================================
+
+
+
+At first, we create a ``UrDriver`` object as usual:
+
+.. literalinclude:: ../../examples/trajectory_point_interface.cpp
+   :language: c++
+   :caption: examples/trajectory_point_interface.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: std::unique_ptr<urcl::ToolCommSetup> tool_comm_setup;
+   :end-before: // --------------- INITIALIZATION END -------------------
+
+We use a small helper function to make sure that the reverse interface is active and connected
+before proceeding.
+
+
+Initialization
+--------------
+
+As trajectory execution will be triggered asynchronously, we define a callback function to handle a
+finished trajectory. A trajectory is considered finished when the robot is no longer executing it,
+independent of whether it successfully reached its final point. The trajectory result will reflect
+whether it was executed successfully, was canceled upon request or failed for some reason.
+
+.. literalinclude:: ../../examples/trajectory_point_interface.cpp
+   :language: c++
+   :caption: examples/trajectory_point_interface.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: void trajDoneCallback(const urcl::control::TrajectoryResult& result)
+   :end-at: }
+
+That callback can be registered at the ``UrDriver`` object:
+
+
+.. literalinclude:: ../../examples/trajectory_point_interface.cpp
+   :language: c++
+   :caption: examples/trajectory_point_interface.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: g_my_driver->registerTrajectoryDoneCallback(&trajDoneCallback);
+   :end-at: g_my_driver->registerTrajectoryDoneCallback(&trajDoneCallback);
+
+
+MoveJ Trajectory
+----------------
+
+Then, in order to execute a trajectory, we need to define a trajectory as a sequence of points and
+parameters. The following example shows execution of a 2-point trajectory using URScript's
+``movej`` function:
+
+.. literalinclude:: ../../examples/trajectory_point_interface.cpp
+   :language: c++
+   :caption: examples/trajectory_point_interface.cpp
+   :linenos:
+   :lineno-match:
+   :start-after: // --------------- MOVEJ TRAJECTORY -------------------
+   :end-before: // --------------- END MOVEJ TRAJECTORY -------------------
+
+In fact, the path is followed twice, once parametrized by a segment duration and once using maximum
+velocity / acceleration settings. If a duration > 0 is given for a segment, the velocity and
+acceleration settings will be ignored as in the underlying URScript functions. In the example
+above, each of the ``g_my_driver->writeTrajectoryPoint()`` calls will be translated into a
+``movej`` command in URScript.
+
+While the trajectory is running, we need to tell the robot program that connection is still active
+and we expect the trajectory to be running. This is being done by the
+``g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);``
+call.
+
+MoveL Trajectory
+----------------
+
+Similar to the ``movej``-based trajectory, execution can be done interpolating in joint space:
+
+.. literalinclude:: ../../examples/trajectory_point_interface.cpp
+   :language: c++
+   :caption: examples/trajectory_point_interface.cpp
+   :linenos:
+   :lineno-match:
+   :start-after: // --------------- MOVEL TRAJECTORY -------------------
+   :end-before: // --------------- END MOVEL TRAJECTORY -------------------
+
+Spline based interpolation
+--------------------------
+
+Similar to the :ref:`spline_example`, the trajectory point interface can be used to execute motions
+using the spline interpolation:
+
+.. literalinclude:: ../../examples/trajectory_point_interface.cpp
+   :language: c++
+   :caption: examples/trajectory_point_interface.cpp
+   :linenos:
+   :lineno-match:
+   :start-after: // --------------- SPLINE TRAJECTORY -------------------
+   :end-before: // --------------- END SPLINE TRAJECTORY -------------------

examples/trajectory_point_interface.cpp

@@ -42,18 +42,38 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101";
 const std::string SCRIPT_FILE = "resources/external_control.urscript";
 const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt";
 const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt";
-const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542";
 
 std::unique_ptr<urcl::DashboardClient> g_my_dashboard;
 std::unique_ptr<urcl::UrDriver> g_my_driver;
 
 std::atomic<bool> g_trajectory_done = false;
 
+std::atomic<bool> g_program_running;
+std::condition_variable g_program_running_cv;
+std::mutex g_program_running_mutex;
+
+bool waitForProgramRunning(int milliseconds = 100)
+{
+  std::unique_lock<std::mutex> lk(g_program_running_mutex);
+  if (g_program_running_cv.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout ||
+      g_program_running == true)
+  {
+    return true;
+  }
+  return false;
+}
+
 // We need a callback function to register. See UrDriver's parameters for details.
 void handleRobotProgramState(bool program_running)
 {
   // Print the text in green so we see it better
   std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl;
+  g_program_running = program_running;
+  if (program_running)
+  {
+    std::lock_guard<std::mutex> lk(g_program_running_mutex);
+    g_program_running_cv.notify_one();
+  }
 }
 
 void trajDoneCallback(const urcl::control::TrajectoryResult& result)
@@ -113,7 +133,12 @@ int main(int argc, char* argv[])
   std::unique_ptr<urcl::ToolCommSetup> tool_comm_setup;
   const bool headless = true;
   g_my_driver.reset(new urcl::UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState,
-                                       headless, std::move(tool_comm_setup), CALIBRATION_CHECKSUM));
+                                       headless, std::move(tool_comm_setup)));
+  if (!waitForProgramRunning(1000))
+  {
+    URCL_LOG_ERROR("Program did not start running. Is the robot in remote control?");
+    return 1;
+  }
   // --------------- INITIALIZATION END -------------------
 
   g_my_driver->registerTrajectoryDoneCallback(&trajDoneCallback);
@@ -218,7 +243,7 @@ int main(int argc, char* argv[])
       g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);
     }
   }
-  // --------------- END MOVEJ TRAJECTORY -------------------
+  // --------------- END SPLINE TRAJECTORY -------------------
 
   g_my_driver->stopControl();
   return 0;