Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="robot_description_xml_path" default="$(find-pkg-share moveit_resources_panda_moveit_config)/config/panda.urdf.xacro" />
<arg name="srdf_path" default="$(find-pkg-share moveit_resources_panda_moveit_config)/config/panda.srdf" />
<let name="moveitconfig_share_dir"
value="$(find-pkg-share moveit_resources_panda_moveit_config)" />
<let name="tutorials_share" value="$(find-pkg-share moveit2_tutorials)" />
<let name="child_frame_statictf" value="panda_link0" />
<let name="arm_controller" value="panda_arm_controller" />
<let name="gripper_controller" value="panda_hand_controller" />
<let name="moveit_configs_yaml"
value="$(command 'python3 -c &quot;import tempfile, yaml; from moveit_configs_utils import MoveItConfigsBuilder; mc = MoveItConfigsBuilder(\x27moveit_resources_panda\x27).robot_description(file_path=\x27config/panda.urdf.xacro\x27).trajectory_execution(file_path=\x27config/gripper_moveit_controllers.yaml\x27).planning_pipelines(pipelines=[\x27pilz_industrial_motion_planner\x27]).to_moveit_configs(); f = tempfile.NamedTemporaryFile(mode=\x27w\x27, suffix=\x27.yaml\x27, delete=False); yaml.safe_dump({\x27/**\x27: {\x27ros__parameters\x27: mc.to_dict()}}, f); print(f.name)&quot;')" />

<node pkg="rviz2" exec="rviz2" output="log"
args="-d $(var tutorials_share)/launch/move_group.rviz">
<param from="$(var moveit_configs_yaml)" />
</node>

<node pkg="tf2_ros" exec="static_transform_publisher" name="static_transform_publisher"
output="log" args="0.0 0.0 0.0 0.0 0.0 0.0 world $(var child_frame_statictf)" />

<node pkg="robot_state_publisher"
exec="robot_state_publisher" name="robot_state_publisher" output="both">
<param name="robot_description" value="$(command 'xacro $(var robot_description_xml_path)')" />
</node>

<node pkg="moveit_ros_move_group" exec="move_group" output="screen">
<param name="capabilities" value="pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService" />
<param name="publish_robot_description" value="True" />
<param name="publish_robot_description_semantic" value="True" />
<param from="$(var moveit_configs_yaml)" />
</node>

<node pkg="controller_manager" exec="ros2_control_node" output="both">
<param name="robot_description" value="$(command 'xacro $(var robot_description_xml_path)')" />
<param from="$(var moveitconfig_share_dir)/config/ros2_controllers.yaml" />
</node>

<executable cmd="ros2 run controller_manager spawner joint_state_broadcaster" output="screen" shell="true" />
<executable cmd="ros2 run controller_manager spawner $(var arm_controller)" output="screen" shell="true" />
<executable cmd="ros2 run controller_manager spawner $(var gripper_controller)" output="screen" shell="true" />

</launch>
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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
------------------

Expand All @@ -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
----------------------
Expand Down Expand Up @@ -144,6 +147,8 @@ 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; requires ``cartesian_speed_limited_link`` to be set)
- ``cartesian_speed_limited_link``: name of a link (required to be non-empty for ``max_cartesian_speed`` to take effect)
- ``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
Expand Down Expand Up @@ -215,6 +220,8 @@ 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; requires ``cartesian_speed_limited_link`` to be set)
- ``cartesian_speed_limited_link``: name of a link (required to be non-empty for ``max_cartesian_speed`` to take effect)
- ``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
Expand Down Expand Up @@ -260,6 +267,57 @@ 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
---------------------------

.. note::

The ``POLYLINE`` planner and its parameters (such as ``smoothness_level`` in :moveit_msgs_codedir:`MotionPlanRequest.msg <msg/MotionPlanRequest.msg>`, introduced in `moveit2#3610 <https://github.com/moveit/moveit2/pull/3610>`_) are not planned to be released into ROS 2 Jazzy or earlier. To run the ``POLYLINE`` portion of this tutorial, use ROS 2 Lyrical, Rolling, or newer.

This planner generates a continuous Cartesian trajectory along a sequence of waypoints, rounding non-collinear corners with circular arcs when a nonzero blend radius is used.
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; requires ``cartesian_speed_limited_link`` to be set)
- ``cartesian_speed_limited_link``: name of a link (required to be non-empty for ``max_cartesian_speed`` to take effect)
- ``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`` ([(link to jazzy)](https://docs.ros.org/en/jazzy/p/moveit_msgs/msg/PositionConstraint.html))

- ``path_constraints/position_constraints/constraint_region/primitive_poses/point``:
pose of the point
- ``goal_constraints`` (checked only with inverse kinematics; the trajectory ends at the last waypoint in ``path_constraints``, so include the goal pose as the final path waypoint)
- ``goal_constraints/position_constraints/header/frame_id``:
frame this data is associated with
- ``goal_constraints/position_constraints/link_name``: target
link name
- ``goal_constraints/orientation_constraints/header/frame_id``:
frame for the required goal orientation constraint
- ``goal_constraints/orientation_constraints/link_name``: target
link name for the orientation constraint
- ``goal_constraints/orientation_constraints/orientation``: target
quaternion orientation (or construct the full pose goal using ``kinematic_constraints::constructGoalConstraints``)
- ``smoothness_level``: scaling factor for the maximum possible rounding radius

Examples
--------

Expand Down Expand Up @@ -390,6 +448,10 @@ The service ``plan_sequence_path`` allows the user to generate a joint
trajectory for a ``moveit_msgs::msg::MotionSequenceRequest``.
The trajectory is returned and not executed.

.. note::

The ``pilz_sequence`` example includes a ``POLYLINE`` segment, which requires ROS 2 Lyrical, Rolling, or newer (this feature is not planned to be released into ROS 2 Jazzy or earlier; see `moveit2#3610 <https://github.com/moveit/moveit2/pull/3610>`_).

To use the ``MoveGroupSequenceService`` and the ``MoveGroupSequenceAction``, refer to the :codedir:`Pilz Motion Planner sequence example <how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp>`.
To run this, execute the following commands in separate terminals:

Expand Down Expand Up @@ -424,8 +486,8 @@ 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.
:codedir:`pilz_sequence.cpp file <how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp>`
creates a sequence of three commands.

::

Expand All @@ -435,6 +497,9 @@ 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;
item1.req.cartesian_speed_limited_link = "panda_hand";

// MotionSequenceItem configuration
item1.req.group_name = PLANNING_GROUP;
Expand All @@ -461,7 +526,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:

Expand All @@ -485,6 +550,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
Expand Down Expand Up @@ -546,9 +612,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 runs with intermediate blend radii of 0, 0.1, and 0.05 (the final sequence item always uses ``blend_radius = 0.0``).

.. figure:: trajectory_comparison.jpeg
.. figure:: trajectory_comparison.png
:alt: trajectory comparison

Action interface
Expand Down Expand Up @@ -588,6 +654,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.

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down
Loading