Moveit can't execute a plan with MoveGroupInterface
Hello,
I am trying to move a group from my own robot with a joint goal, with the MoveGroupInterface class. When I use move() the group moves perfectly to the joint goal in rviz.
Now I want to add constraints and time parameterization, so I generated a plan with the planning pipeline :
planning_pipeline->generatePlan(planning_scene, req, res);
The trajectory created is correct, but when I try to execute it in Rviz the robot doesn't move, even though I get a "SUCCEED" message for the execution.
How can I execute and visualize a trajectory created with the planning pipeline ? I tried several methods from the moveit tutorials, and never got a result. Thanks
I use :
Ubuntu 18.04 ROS melodic (from binaries) Moveit (from binaries)
I use the demo.launch file.
My code :
planning_pipeline->generatePlan(planning_scene, req, res); robot_trajectory::RobotTrajectory trajectory = *res.trajectory_; vector <string> joint_names = {"J1_right", "J2_right", "J3_right", "J4_right"};
moveit_msgs::RobotTrajectory trajectory_msg;
trajectory.getRobotTrajectoryMsg(trajectory_msg);
moveit::planning_interface::MoveGroupInterface move_group(group);
moveit::planning_interface::MoveGroupInterface::Plan plan;
plan.trajectory_ = trajectory_msg;
plan.trajectory_.joint_trajectory.header.stamp = ros::Time::now();
plan.trajectory_.joint_trajectory.header.frame_id = trajectory_msg.joint_trajectory.header.frame_id;
plan.trajectory_.joint_trajectory.joint_names = joint_names;
cout<<"EXECUTE PLAN"<<endl;
move_group.plan(plan); // I tried with .plan(), without .plan(), and with .move()
move_group.execute(plan);
Console output :
[ INFO] [1587753968.681163408]: Looking around: no [ INFO] [1587753968.681232961]: Replanning: no [ INFO] [1587754003.859250207]: Execution request received [ INFO] [1587754003.930022457]: Fake execution of trajectory [ INFO] [1587754004.229404023]: Completed trajectory execution with status SUCCEEDED ... [ INFO] [1587754004.229801013]: Execution completed: SUCCEEDED