Documentation about MotionSequenceRequest (Pilz Industrial Motion Planner)

[MikelBueno]

Hi all,

A quick and simple doubt: I am trying to implement sequence executions using the Pilz_Industrial_Motion_Planner. I have followed your documentation here, and I have successfully implemented the motion planner but I am missing documentation about the MotionSequenceRequest.

I have checked your C++ code on Moveit Task Constructor, but unfortunately, that doesn't work for me since I am interested in:

• Planning the whole sequence as a single trajectory to avoid delay between "plan" actions.
• Implementing blending.

I have tried to implement this on my own (even with subsequent plans changing the RobotState manually), but with no success.
Some documentation (+ source code) on the ROS2 (C++) version of this would be amazing!

I am happy to contribute to this, but I would need some help at the beginning.

Thank you very much in advance,
Mikel Bueno
Cranfield University

[hoedwin]

@MikelBueno

Hi there, is there any progress on C++ usage of Pilz Industrial Motion Planner's Sequence?
I am pretty stuck too.

In my case, Action Server for moveit_msgs/action/MoveGroupSequence is not active.

Also, all the functions within #include <pilz_industrial_motion_planner/move_group_sequence_action.h> are all private and there is no documentation how to kick start using it.

Some guides or sample codes are truly appreciated. Thanks!

[Hantao-Ye]

@MikelBueno @hoedwin

Hi there, I have tried with the service call and it worked well.

#include <moveit_msgs/GetMotionSequence.h>

// after launched the node handler
sequence_path_client_ = nh_ptr_->serviceClient<moveit_msgs::GetMotionSequence>("plan_sequence_path");

// initialized the service call
moveit_msgs::GetMotionSequenceRequest motion_sequence_req;
moveit_msgs::GetMotionSequenceResponse motion_sequence_res;

moveit::core::RobotStatePtr robot_state_ptr(robot_move_group_ptr->getCurrentState());
robot_move_group_ptr->setStartState(*robot_state_ptr);

for (size_t i = 0; i < target_poses.size(); i++)
{
     moveit_msgs::MotionSequenceItem motion_sequence_item;

     motion_sequence_item.blend_radius = 0.1;

     if (i == target_poses.size() - 1)
          motion_sequence_item.blend_radius = 0.0;

     robot_state_ptr->setFromIK(robot_joint_group_ptr, target_poses[i]);

     robot_move_group_ptr->setJointValueTarget(*robot_state_ptr);
     robot_move_group_ptr->constructMotionPlanRequest(motion_sequence_item.req);

     if (i != target_poses.size() - 1)
          motion_sequence_item.req.start_state = moveit_msgs::RobotState();

     motion_sequence_req.request.items.push_back(motion_sequence_item);
}

// call the service
sequence_path_client_.call(motion_sequence_req, motion_sequence_res);

[sea-bass]

@Hantao-Ye thanks for this update, but that is ROS 1 code, the original question pertains to ROS 2

[Hantao-Ye]

@sea-bass Sorry for the mistake:cry:

[MikelBueno]

Hi @hoedwin,

Unfortunately, I couldn't find a way of implementing sequence execution using <pilz_industrial_motion_planner/move_group_sequence_action.h>. However, I have designed and implemented my own way of executing static programs (sequences) using the Pilz Industrial Motion Planner. You can find it here, in our IFRA-Cranfield/ros2_SimRealRobotControl repository.

Regards,
Mikel

[peterdavidfagan]

+1 also interested in this feature for data collection purposes. Would be nice to incorporate planning sequences in moveit_cpp.

[alejomancinelli]

Hi, I've playing around with the MoveGroupSequenceAction as explain in the Pilz Tutorial and I think I got it to work. For the record, I'm using ROS2 Humble.

First, I start the action server with the moveit config

 moveit_config = (
        MoveItConfigsBuilder("moveit_resources_panda")
        .robot_description(file_path="config/panda.urdf.xacro")
        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
        .planning_scene_monitor(
            publish_robot_description=True, publish_robot_description_semantic=True
        )
        .planning_pipelines(
            pipelines=["pilz_industrial_motion_planner"]
        )
        .to_moveit_configs()
    )

    move_group_capabilities = {
        "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService"
    }

Then, following the moveit_msgs Documentation I create and send to the MoveGroupSequenceAction the corresponding goal msg:

// Create MotionSequenceRequest
moveit_msgs::msg::MotionSequenceRequest sequence_request;

// Create un MotionSequenceItem
moveit_msgs::msg::MotionSequenceItem item;
item.blend_radius = 0.1;

item.req.group_name = PLANNING_GROUP;
item.req.planner_id = "PTP";
item.req.allowed_planning_time = 5;
item.req.max_velocity_scaling_factor = 1.0;
item.req.max_acceleration_scaling_factor = 0.5;

// First point target position
geometry_msgs::msg::PoseStamped target_pose;
target_pose.header.frame_id = "panda_link0";
target_pose.pose.position.x = 0.28;
target_pose.pose.position.y = 0;
target_pose.pose.position.z = 0.5;
target_pose.pose.orientation.w = 1.0;
  
item.req.goal_constraints.resize(1);
item.req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints("panda_link7", target_pose);

// First MotionSequenceItem 
sequence_request.items.push_back(item);

// --- Do the same with the other points in the trajectory
// Last item blend_radius must be 0!

// Create MoveGroupSequence action client
using MoveGroupSequence = moveit_msgs::action::MoveGroupSequence;
auto client = rclcpp_action::create_client<MoveGroupSequence>(node, "/sequence_move_group");

// Wait for the MoveGroupSequence action server
if (!client->wait_for_action_server(std::chrono::seconds(10))) {
  RCLCPP_ERROR(node->get_logger(), "Error al esperar al servidor de acción /sequence_move_group");
  return -1;
}

// Create action goal
auto goal_msg = MoveGroupSequence::Goal();
goal_msg.request = sequence_request;

// Configure planning options
goal_msg.planning_options.planning_scene_diff.is_diff = true;
goal_msg.planning_options.planning_scene_diff.robot_state.is_diff = true;
// goal_msg.planning_options.plan_only = true;

// Send action goal
auto send_goal_options = rclcpp_action::Client<MoveGroupSequence>::SendGoalOptions();
send_goal_options.goal_response_callback = [](std::shared_ptr<GoalHandleMoveGroupSequence> goal_handle) {
  try {
    if (!goal_handle) {
      RCLCPP_ERROR(rclcpp::get_logger("rclcpp_action_example"), "Goal was rejected by server");
    } else {
      RCLCPP_INFO(rclcpp::get_logger("rclcpp_action_example"), "Goal accepted by server, waiting for result");
    }
  }
  catch(const std::exception &e) {
    RCLCPP_ERROR(rclcpp::get_logger("motion_sequence_node"), "Exception while waiting for goal response: %s", e.what());
  }
};

// --- Result callback

auto goal_handle_future = client->async_send_goal(goal_msg, send_goal_options);

I'm still doing some testing and the code could be improved, but this allows to configure the blend radius and get a smoother robot motion. Any feedback will be greatly appreciated!

[sea-bass]

This is great @alejomancinelli!

I ported the current Pilz tutorial to ROS 2 but didn't do the sequence part, hence this issue. If you want to put up a PR adding your work, please @ me and I will happily review and test.

[alejomancinelli]

Sure! I will do a couple of tests, clean up the code and add this as another tutorial. Maybe I will have to ask for some help since I've never done a PR in a public repo.

I've also would like to implement the stop() and pause() commands. Reading through the pilz_robot_programming the stop() commands should be simple, just a client.async_cancel_goal( ... ), while the pause() seems a little more complicated since it uses threads to stop the execution and then continue it. I will leave these things for later since it's more of a API thing to control the movement.

[sea-bass]

Happy to help with that PR. You'll just need to fork this repository, make your changes, and then push your branch. That will then show up as a PR on the main repo.

Once that draft PR is up, tag me on it and we'll get through any issues!

[sea-bass]

Closed by #917 -- thanks again @alejomancinelli!