ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How do you generate a motion plan from moveit_msgs::MotionPlanRequest and moveit_msgs::MotionPlanResponse?

asked 2022-05-06 14:20:42 -0500

ASPHassan gravatar image

I am trying to use the Pilz Industrial Motion Planner in a C++ node and the tutorials claim that it uses moveit_msgs::MotionPlanRequest (and Response) but I haven't found a way to actually generate the plan to populate the response. The Motion Planning Pipeline tutorial uses planning_interface::MotionPlanRequest (and Response) and generates the plans using planning_pipeline::PlanningPipeline (name_of_pipeline) and name_of_pipeline->generatePlan. Is there something similar I could use for the moveit_msgs::MotionPlanRequest? I tried using the planning pipeline method with Pilz and it worked for a single trajectory, but I don't think there is a way I can use moveit_msgs::MotionSequenceRequest this way which is ultimately the tool I want to use.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2022-05-12 12:41:37 -0500

ASPHassan gravatar image

updated 2022-05-13 08:05:15 -0500

So I found the answer and I'll share it here in case anyone finds it helpful. The Pilz Tutorial gives you two options: The MoveGroup Action and the /plan_kinematic_path service. Both of these should start up when you start the movegroup node. I used the /plan_kinematic_path to generate a PILZ trajectory and the movegroup interface to execute that trajectory. It seems that this should work with other planning plugins, or atleast ones that develop kinematic paths. i.e


moveit::planning_interface::MoveGroupInterface::Plan plan; 
moveit_msgs::MotionPlanRequest req;
moveit_msgs::MotionPlanResponse res;

Fill out the request as shown in the tutorial. I formed the goal constraints using

moveit_msgs::Constraints pose_start_goal = kinematic_constraints::constructGoalConstraints(end_link, pose_stamped, tolerance_pos, tolerance_ang);

and then I instantiated the service client, filled the request and called the service:

ros::ServiceClient motionplanclient = nh.serviceClient<moveit_msgs::GetMotionPlan>("/plan_kinematic_path");
plan_path.waitForExistence();
moveit_msgs::GetMotionPlan motionplansrv;
motionplansrv.request.motion_plan_request = req;
motionplanclient.call(motionplansrv);

Then I pulled the response from the service and added the trajectory to the plan

res=motionplansrv.response.motion_plan_response;
plan.trajectory_=res.trajectory;

I used rviz_visual_tools to publish the trajectory visual_tools.publishTrajectoryLine(plan.trajectory_, joint_model_group) and move_group.execute(plan) to move the robot.

My current issue is now with MotionSequenceRequest, namely visualizing the path in RVIZ and executing the plan, but I'll post that as another question and link it here.

edit flag offensive delete link more

Question Tools

Stats

Asked: 2022-05-06 14:20:42 -0500

Seen: 191 times

Last updated: May 13 '22