Add more information about acceleration/velocity parametrization in trajectory examples (#251)

The examples were mainly using either time parametrization or implicit
acc/vel parametrization using the default values.

This adds more explicit acc/vel parametrized motions to the examples
with some explaining comments.

---------

Co-authored-by: Rune Søe-Knudsen <[email protected]>

Author: urfeex

examples/instruction_executor.cpp

@@ -115,8 +115,9 @@ int main(int argc, char* argv[])
   std::vector<std::shared_ptr<urcl::control::MotionPrimitive>> motion_sequence{
     std::make_shared<urcl::control::MoveJPrimitive>(urcl::vector6d_t{ -1.57, -1.57, 0, 0, 0, 0 }, 0.1,
                                                     std::chrono::seconds(5)),
+    // This point uses acceleration / velocity parametrization
     std::make_shared<urcl::control::MoveJPrimitive>(urcl::vector6d_t{ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1,
-                                                    std::chrono::seconds(5)),
+                                                    std::chrono::seconds(0), 1.4, 2.0),
 
     std::make_shared<urcl::control::MoveLPrimitive>(urcl::Pose(-0.203, 0.263, 0.559, 0.68, -1.083, -2.076), 0.1,
                                                     std::chrono::seconds(2)),
@@ -125,10 +126,16 @@ int main(int argc, char* argv[])
   };
   instruction_executor->executeMotion(motion_sequence);
 
-  instruction_executor->moveJ({ -1.57, -1.57, 0, 0, 0, 0 });
-  instruction_executor->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 });
-  instruction_executor->moveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 });
-  instruction_executor->moveL({ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 });
+  double goal_time_sec = 2.0;
+
+  // acceleration / velocity parametrization
+  instruction_executor->moveJ({ -1.57, -1.57, 0, 0, 0, 0 }, 2.0, 2.0);
+  // goal time parametrization -- acceleration and velocity will be ignored
+  instruction_executor->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1, 0.1, goal_time_sec);
+  // acceleration / velocity parametrization
+  instruction_executor->moveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 1.5, 1.5);
+  // goal time parametrization -- acceleration and velocity will be ignored
+  instruction_executor->moveL({ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec);
 
   g_my_driver->stopControl();
   return 0;

examples/trajectory_point_interface.cpp

@@ -58,8 +58,7 @@ void handleRobotProgramState(bool program_running)
 
 void trajDoneCallback(const urcl::control::TrajectoryResult& result)
 {
-  URCL_LOG_INFO("Trajectory done with result %d", result);
-  ;
+  URCL_LOG_INFO("Trajectory done with result %s", urcl::control::trajectoryResultToString(result).c_str());
   g_trajectory_done = true;
 }
 
@@ -126,6 +125,8 @@ int main(int argc, char* argv[])
     // Trajectory definition
     std::vector<urcl::vector6d_t> points{ { -1.57, -1.57, 0, 0, 0, 0 }, { -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 } };
     std::vector<double> motion_durations{ 5.0, 2.0 };
+    std::vector<double> velocities{ 2.0, 2.3 };
+    std::vector<double> accelerations{ 2.5, 2.5 };
     std::vector<double> blend_radii{ 0.1, 0.1 };
 
     // Trajectory execution
@@ -140,7 +141,8 @@ int main(int argc, char* argv[])
     motion_durations = { 0.0, 0.0 };
     for (size_t i = 0; i < points.size(); i++)
     {
-      g_my_driver->writeTrajectoryPoint(points[i], 2.0, 2.0, false, motion_durations[i], blend_radii[i]);
+      g_my_driver->writeTrajectoryPoint(points[i], accelerations[i], velocities[i], false, motion_durations[i],
+                                        blend_radii[i]);
     }
 
     while (!g_trajectory_done)
@@ -159,17 +161,27 @@ int main(int argc, char* argv[])
     std::vector<urcl::vector6d_t> points{ { -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 },
                                           { -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 } };
     std::vector<double> motion_durations{ 5.0, 5.0 };
+    std::vector<double> velocities{ 2.0, 2.3 };
+    std::vector<double> accelerations{ 2.5, 2.5 };
     std::vector<double> blend_radii{ 0.0, 0.0 };
 
-    // Trajectory execution
+    // Trajectory execution of the path that goes through the points twice.
     g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START,
-                                               points.size());
+                                               points.size() * 2);
     for (size_t i = 0; i < points.size(); i++)
     {
       // setting the cartesian parameter makes it interpret the 6d vector as a pose and use movel
       g_my_driver->writeTrajectoryPoint(points[i], true, motion_durations[i], blend_radii[i]);
     }
 
+    // Same motion, but parametrized with acceleration and velocity
+    motion_durations = { 0.0, 0.0 };
+    for (size_t i = 0; i < points.size(); i++)
+    {
+      g_my_driver->writeTrajectoryPoint(points[i], accelerations[i], velocities[i], true, motion_durations[i],
+                                        blend_radii[i]);
+    }
+
     while (!g_trajectory_done)
     {
       std::this_thread::sleep_for(std::chrono::milliseconds(100));