Moveit can't execute a plan with MoveGroupInterface

edit

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