Update pilz tutorials for POLYLINE planner

doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst

@@ -39,7 +39,7 @@ strictest combination of all limits as a common limit for all joints.
 Cartesian Limits
 ----------------
 
-For Cartesian trajectory generation (LIN/CIRC), the planner needs
+For Cartesian trajectory generation (LIN/CIRC/POLYLINE), the planner needs
 information about the maximum speed in 3D Cartesian space. Namely,
 translational/rotational velocity/acceleration/deceleration need to be
 set in the node parameters like this:
@@ -60,6 +60,9 @@ rotational trapezoidal shapes. The rotational acceleration is
 calculated as ``max_trans_acc / max_trans_vel * max_rot_vel``
 (and for deceleration accordingly).
 
+You can set different max_trans_vel using ``MotionPlanRequest`` by setting
+the field ``max_cartesian_speed`` and the field ``cartesian_speed_limited_link``.
+
 Planning Interface
 ------------------
 
@@ -70,7 +73,7 @@ are explained below.
 For a general introduction on how to fill a ``MotionPlanRequest``, see
 :ref:`move_group_interface-planning-to-pose-goal`.
 
-You can specify ``"PTP"``, ``"LIN"`` or ``"CIRC"`` as the ``planner_id`` of the ``MotionPlanRequest``.
+You can specify ``"PTP"``, ``"LIN"``, ``"CIRC"`` or ``"POLYLINE"`` as the ``planner_id`` of the ``MotionPlanRequest``.
 
 The PTP motion command
 ----------------------
@@ -144,6 +147,7 @@ LIN Input Parameters in ``moveit_msgs::MotionPlanRequest``
    translational/rotational velocity
 -  ``max_acceleration_scaling_factor``: scaling factor of maximal
    Cartesian translational/rotational acceleration/deceleration
+-  ``max_cartesian_speed``: maximal Cartesian speed for this motion (replaces the max_trans_vel parameter)
 -  ``start_state/joint_state/(name, position and velocity``: joint
    name/position of the start state.
 -  ``goal_constraints`` (goal can be given in joint space or Cartesian
@@ -215,6 +219,7 @@ CIRC Input Parameters in ``moveit_msgs::MotionPlanRequest``
    translational/rotational velocity
 -  ``max_acceleration_scaling_factor``: scaling factor of maximal
    Cartesian translational/rotational acceleration/deceleration
+-  ``max_cartesian_speed``: maximal Cartesian speed for this motion (replaces the max_trans_vel parameter)
 -  ``start_state/joint_state/(name, position and velocity``: joint
    name/position of the start state.
 -  ``goal_constraints`` (goal can be given in joint space or Cartesian
@@ -260,6 +265,46 @@ CIRC Planning Result in ``moveit_msg::MotionPlanResponse``
 -  ``group_name``: the name of the planning group
 -  ``error_code/val``: error code of the motion planning
 
+The POLYLINE motion command
+---------------------------------
+
+This planner generates a continuous Cartesian trajectory passing through a sequence of waypoints.
+The generated path is a combination of linear segments connected by
+circular arcs to smooth the transitions between consecutive lines. A smoothness level scaling factor is used to
+determine smoothness by scaling the max possible rounding radius.
+The planner automatically filters waypoints that are positioned too closely;
+however, the user must ensure the angle between consecutive segments is
+sufficiently large to avoid violating minimum rounding constraints.
+The planner uses Cartesian limits to generate a trapezoidal 
+velocity profile in Cartesian space. This planner only accepts a 
+start state with zero velocity. The planning result is a joint trajectory. The user needs to adapt 
+the Cartesian velocity/acceleration scaling factor if the motion plan fails due to violation of cartesian limits.
+the planner will fail if three or more consecutive waypoints are collinear.
+
+POLYLINE Input Parameters in ``moveit_msgs::MotionPlanRequest``
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+-  ``planner_id``: ``"POLYLINE"``
+-  ``group_name``: the name of the planning group
+-  ``max_velocity_scaling_factor``: scaling factor of maximal Cartesian
+   translational/rotational velocity
+-  ``max_acceleration_scaling_factor``: scaling factor of maximal
+   Cartesian translational/rotational acceleration/deceleration
+-  ``max_cartesian_speed``: maximal Cartesian speed for this motion (replaces the max_trans_vel parameter)
+-  ``start_state/joint_state/(name, position and velocity``: joint
+   name/position of the start state.
+-  ``path_constraints``: a list of position constraints to be followed in
+   Cartesian space. Each waypoint is defined as a ``moveit_msgs::msg::PositionConstraint``
+
+      -  ``path_constraints/position_constraints/constraint_region/primitive_poses/point``:
+         pose of the point
+-  ``goal_constraints`` (the last goal point)
+      -  ``goal_constraints/position_constraints/header/frame_id``:
+         frame this data is associated with
+      -  ``goal_constraints/position_constraints/link_name``: target
+         link name
+-  ``smoothness_level``: scaling factor for the maximum possible rounding radius
+
 Examples
 --------
 
@@ -425,7 +470,7 @@ is used instead:
 
 The
 :codedir:`pilz_sequence.cpp file <how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.launch.py>`
-creates two target poses that will be reached sequentially.
+creates a sequence of three commands.
 
 ::
 
@@ -435,6 +480,8 @@ creates two target poses that will be reached sequentially.
 
     // Set pose blend radius
     item1.blend_radius = 0.1;
+    // Set max_cartesian_speed (overwrite the max_trans_vel)
+    item1.req.max_cartesian_speed = 0.5;
 
     // MotionSequenceItem configuration
     item1.req.group_name = PLANNING_GROUP;
@@ -461,7 +508,7 @@ creates two target poses that will be reached sequentially.
        msg.pose.orientation.w = 0.0;
        return msg;
     } ();
-    item1.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item1));
+    item1.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_hand", target_pose_item1));
 
 The service client needs to be initialized:
 
@@ -485,6 +532,7 @@ Then, the request is created:
     auto service_request = std::make_shared<GetMotionSequence::Request>();
     service_request->request.items.push_back(item1);
     service_request->request.items.push_back(item2);
+    service_request->request.items.push_back(item3);
 
 Once the service call is completed, the method ``future.wait_for(timeout_duration)`` blocks until
 a specified ``timeout_duration`` has elapsed or the result becomes available, whichever comes
@@ -546,9 +594,9 @@ The future response is read with the ``future.get()`` method.
        return 0;
     }
 
-In this case, the planned trajectory is drawn. Here is a comparison of a blend radius of 0 and 0.1 for the first and second trajectory, respectively.
+In this case, the planned trajectory is drawn. Here is a comparison of a blend radius of 0, 0.1, 0.05 for the first, second, and third trajectory (line, line, ellipse), respectively.
 
-.. figure:: trajectory_comparison.jpeg
+.. figure:: trajectory_comparison.png
    :alt: trajectory comparison
 
 Action interface
@@ -588,6 +636,7 @@ Then, the request is created:
     moveit_msgs::msg::MotionSequenceRequest sequence_request;
     sequence_request.items.push_back(item1);
     sequence_request.items.push_back(item2);
+    sequence_request.items.push_back(item3);
 
 The goal and planning options are configured. A goal response callback and result callback can be included as well.
 

doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_move_group.cpp

@@ -170,6 +170,74 @@ int main(int argc, char* argv[])
     plan_and_execute("[CIRC] Turn");
   }
 
+  {
+    auto const goal_pose = [] {
+      geometry_msgs::msg::PoseStamped msg;
+      msg.header.frame_id = "world";
+      msg.pose.orientation.x = 1.0;
+      msg.pose.orientation.y = 0.0;
+      msg.pose.orientation.z = 0.0;
+      msg.pose.orientation.w = 0.0;
+      msg.pose.position.x = 0.3;
+      msg.pose.position.y = 0.0;
+      msg.pose.position.z = 0.6;
+      return msg;
+    }();
+    // Move back home using the PTP planner.
+    move_group_interface.setPlannerId("PTP");
+    move_group_interface.setPoseTarget(goal_pose, "panda_hand");
+    plan_and_execute("[PTP] Return");
+  }
+  // Clear Marker to show polyline path clearly
+  moveit_visual_tools.deleteAllMarkers();
+  moveit_visual_tools.trigger();
+  {
+    // Move in a heart-shaped Cartesian path using the POLYLINE.
+    move_group_interface.setPlannerId("POLYLINE");
+    // To prevent failure due to exceeding cartesian limits.
+    move_group_interface.setMaxVelocityScalingFactor(0.05);
+    move_group_interface.setMaxAccelerationScalingFactor(0.05);
+    // Define Heart Shape Parametric Equation to collect waypoints
+    auto heart_eq = [](double t) -> Eigen::Vector3d {
+      Eigen::Vector3d p;
+      p.x() = 0.3 + 0.0068 * (17 + 13 * std::cos(t) - 5 * std::cos(2 * t) - 2 * std::cos(3 * t) - cos(4 * t));
+      p.y() = 0.0068 * (16.0 * std::pow(std::sin(t), 3));
+      p.z() = 0.6;
+      return p;
+    };
+
+    geometry_msgs::msg::PoseStamped msg;
+    msg.header.frame_id = "world";
+    moveit_msgs::msg::Constraints path_constraints;
+    const int num_points = 40;
+    for (int i = 0; i <= num_points; ++i)
+    {
+      // Get waypoint from parametric equation of heart shape
+      double t = M_PI + 2.0 * i * M_PI / float(num_points);
+      Eigen::Vector3d p = heart_eq(t);
+      msg.pose.position.x = p.x();
+      msg.pose.position.y = p.y();
+      msg.pose.position.z = p.z();
+      msg.pose.orientation.x = 1.0;
+      msg.pose.orientation.y = 0.0;
+      msg.pose.orientation.z = 0.0;
+      msg.pose.orientation.w = 0.0;
+      // Add waypoint as position constraint
+      moveit_msgs::msg::PositionConstraint pos_constraint;
+      pos_constraint.header.frame_id = "world";
+      pos_constraint.link_name = "panda_hand";
+      pos_constraint.constraint_region.primitive_poses.resize(1);
+      pos_constraint.constraint_region.primitive_poses[0] = msg.pose;
+      pos_constraint.weight = 1.0;
+      path_constraints.position_constraints.push_back(pos_constraint);
+    }
+    // Set all position constraints as path constraints
+    move_group_interface.setPathConstraints(path_constraints);
+    // Set the last pose as goal
+    move_group_interface.setPoseTarget(msg, "panda_hand");
+    plan_and_execute("[POLYLINE] Heart");
+  }
+
   {
     // Move back home using the PTP planner.
     move_group_interface.setPlannerId("PTP");

doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp

@@ -71,7 +71,9 @@ int main(int argc, char** argv)
   moveit_msgs::msg::MotionSequenceItem item1;
 
   // Set pose blend radius
-  item1.blend_radius = 0.1;
+  item1.blend_radius = 0.05;
+  // Set max_cartesian_speed (overwrite the max_trans_vel)
+  item1.req.max_cartesian_speed = 0.5;
 
   // MotionSequenceItem configuration
   item1.req.group_name = PLANNING_GROUP;
@@ -92,22 +94,22 @@ int main(int argc, char** argv)
     msg.pose.position.x = 0.3;
     msg.pose.position.y = -0.2;
     msg.pose.position.z = 0.6;
-    msg.pose.orientation.x = 1.0;
-    msg.pose.orientation.y = 0.0;
+    msg.pose.orientation.x = 0.924;
+    msg.pose.orientation.y = -0.380;
     msg.pose.orientation.z = 0.0;
     msg.pose.orientation.w = 0.0;
     return msg;
   }();
-  item1.req.goal_constraints.push_back(
-      kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item1));
+  item1.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_hand", target_pose_item1));
 
   // ----- Motion Sequence Item 2
   // Create a MotionSequenceItem
   moveit_msgs::msg::MotionSequenceItem item2;
 
   // Set pose blend radius
-  // For the last pose, it must be 0!
-  item2.blend_radius = 0.0;
+  item2.blend_radius = 0.05;
+  // set max_cartesian_speed (overwrite the max_trans_vel)
+  item2.req.max_cartesian_speed = 1.2;
 
   // MotionSequenceItem configuration
   item2.req.group_name = PLANNING_GROUP;
@@ -123,14 +125,65 @@ int main(int argc, char** argv)
     msg.pose.position.x = 0.3;
     msg.pose.position.y = -0.2;
     msg.pose.position.z = 0.8;
-    msg.pose.orientation.x = 1.0;
-    msg.pose.orientation.y = 0.0;
+    msg.pose.orientation.x = 0.924;
+    msg.pose.orientation.y = -0.380;
     msg.pose.orientation.z = 0.0;
     msg.pose.orientation.w = 0.0;
     return msg;
   }();
-  item2.req.goal_constraints.push_back(
-      kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item2));
+  item2.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_hand", target_pose_item2));
+
+  // -------- Motion Sequence Items 3
+  moveit_msgs::msg::MotionSequenceItem item3;
+
+  // MotionSequenceItem configuration
+  item3.req.group_name = PLANNING_GROUP;
+  item3.req.planner_id = "POLYLINE";
+  item3.req.allowed_planning_time = 5.0;
+  item3.req.max_velocity_scaling_factor = 0.1;
+  item3.req.max_acceleration_scaling_factor = 0.1;
+
+  // For the item, it must be 0!
+  item3.blend_radius = 0.0;
+  // Set max_cartesian_speed (overwrite the max_trans_vel)
+  item3.req.max_cartesian_speed = 0.1;
+
+  // Extract waypoints along an ellipse
+  auto ellipse = [](double t) -> Eigen::Vector3d {
+    Eigen::Vector3d p;
+    p.x() = 0.3;
+    p.y() = -0.2 + 0.2 * std::sin(t);
+    p.z() = 0.6 + 0.2 * std::cos(t);
+    return p;
+  };
+
+  geometry_msgs::msg::PoseStamped msg;
+  msg.header.frame_id = "world";
+  moveit_msgs::msg::Constraints path_constraints;
+  const int num_points = 20;
+  for (int i = 0; i <= num_points; ++i)
+  {
+    // Get waypoint from parametric equation of ellipse
+    double t = i * (M_PI / 2.0) / float(num_points);
+    Eigen::Vector3d p = ellipse(t);
+    msg.pose.position.x = p.x();
+    msg.pose.position.y = p.y();
+    msg.pose.position.z = p.z();
+    msg.pose.orientation.x = std::cos(M_PI / 8.0 - t / 4.0);
+    msg.pose.orientation.y = -std::sin(M_PI / 8.0 - t / 4.0);
+    msg.pose.orientation.z = 0.0;
+    msg.pose.orientation.w = 0.0;
+    // Add waypoint as position constraint
+    moveit_msgs::msg::PositionConstraint pos_constraint;
+    pos_constraint.header.frame_id = "world";
+    pos_constraint.link_name = "panda_hand";
+    pos_constraint.constraint_region.primitive_poses.resize(1);
+    pos_constraint.constraint_region.primitive_poses[0] = msg.pose;
+    pos_constraint.weight = 1.0;
+    item3.req.path_constraints.position_constraints.push_back(pos_constraint);
+  }
+  item3.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_hand", msg));
+  item3.req.smoothness_level = 0.3;
 
   // [ --------------------------------------------------------------- ]
   // [ ------------------ MoveGroupSequence Service ------------------ ]
@@ -151,6 +204,7 @@ int main(int argc, char** argv)
   auto service_request = std::make_shared<GetMotionSequence::Request>();
   service_request->request.items.push_back(item1);
   service_request->request.items.push_back(item2);
+  service_request->request.items.push_back(item3);
 
   // Call the service and process the result
   auto service_future = service_client->async_send_request(service_request);
@@ -223,6 +277,7 @@ int main(int argc, char** argv)
   moveit_msgs::msg::MotionSequenceRequest sequence_request;
   sequence_request.items.push_back(item1);
   sequence_request.items.push_back(item2);
+  sequence_request.items.push_back(item3);
 
   // Create action goal
   auto goal_msg = MoveGroupSequence::Goal();