PlanningPipeline::generatePlan generates discontinuous trajectories
joint_limits.yaml
seems correctly loaded, as indicated by the params
* /robot_description_planning/joint_limits/elbow_joint/has_acceleration_limits: True
* /robot_description_planning/joint_limits/elbow_joint/has_velocity_limits: True
* /robot_description_planning/joint_limits/elbow_joint/max_acceleration: 3.15
* /robot_description_planning/joint_limits/elbow_joint/max_velocity: 3.15
* /robot_description_planning/joint_limits/shoulder_lift_joint/has_acceleration_limits: True
* /robot_description_planning/joint_limits/shoulder_lift_joint/has_velocity_limits: True
* /robot_description_planning/joint_limits/shoulder_lift_joint/max_acceleration: 2.16
* /robot_description_planning/joint_limits/shoulder_lift_joint/max_velocity: 2.16
* /robot_description_planning/joint_limits/shoulder_pan_joint/has_acceleration_limits: True
* /robot_description_planning/joint_limits/shoulder_pan_joint/has_velocity_limits: True
* /robot_description_planning/joint_limits/shoulder_pan_joint/max_acceleration: 2.16
* /robot_description_planning/joint_limits/shoulder_pan_joint/max_velocity: 2.16
* /robot_description_planning/joint_limits/wrist_1_joint/has_acceleration_limits: True
* /robot_description_planning/joint_limits/wrist_1_joint/has_velocity_limits: True
* /robot_description_planning/joint_limits/wrist_1_joint/max_acceleration: 3.2
* /robot_description_planning/joint_limits/wrist_1_joint/max_velocity: 3.2
* /robot_description_planning/joint_limits/wrist_2_joint/has_acceleration_limits: True
* /robot_description_planning/joint_limits/wrist_2_joint/has_velocity_limits: True
* /robot_description_planning/joint_limits/wrist_2_joint/max_acceleration: 3.2
* /robot_description_planning/joint_limits/wrist_2_joint/max_velocity: 3.2
* /robot_description_planning/joint_limits/wrist_3_joint/has_acceleration_limits: True
* /robot_description_planning/joint_limits/wrist_3_joint/has_velocity_limits: True
* /robot_description_planning/joint_limits/wrist_3_joint/max_acceleration: 3.2
* /robot_description_planning/joint_limits/wrist_3_joint/max_velocity: 3.2
Joint acceleration and velocity scaling was applied as follows:
motion_request.max_velocity_scaling_factor = 0.2; motion_request.max_acceleration_scaling_factor = 0.2;
I create my planning pipeline as follows
ur10_planning_pipeline = std::make_shared<planning_pipeline::PlanningPipeline>
(planning_pipeline::PlanningPipeline(ur10_kinematic_model, nodeHandle, "planning_plugin", "request_adapters"));
And generate the plan as follows
planning_scene_monitor::LockedPlanningSceneRO lscene(ur10_planning_scene_monitor);
ur10_planning_pipeline -> generatePlan(lscene, motion_request, motion_response);
The resultant plan is inspected as follows
control_msgs::FollowJointTrajectoryGoal joint_tjt_goal;
moveit_msgs::RobotTrajectory moveit_trajectory;
motion_response.trajectory_ -> getRobotTrajectoryMsg (moveit_trajectory);
joint_tjt_goal.trajectory = moveit_trajectory.joint_trajectory;
for (int j = 0; j < joint_tjt_goal.trajectory.points.size(); j++)
{
ROS_INFO("----- Point[%d] -----", j);
auto point = joint_tjt_goal.trajectory.points[j];
for (int i = 0; i < point.positions.size(); i++)
{
ROS_INFO("positions[%d] = %f", i, point.positions[i]);
}
}
Which shows large changes in position values (some seem like simple angle wrapping, but others seem entirely wrong)
e.g. Joint 1:
positions[1] = 0.009000
positions[1] = 2.933868
positions[1] = -5.707793
positions[1] = 2.596154
positions[1] = 2.542579
positions[1] = 2.492396
positions[1] = 2.444871
positions[1] = 2.399476
positions[1] = 2.355814
positions[1] = 2.313621
positions[1] = 2.272631
positions[1] = 2.232677
positions[1] = -3.901456
positions[1] = 3.732475
positions[1] = 2.308633
positions[1] = 2.273000
positions[1] = 2.237841
positions[1] = 2.203072
positions[1] = 2.168618
Joint 2:
positions[2] = 1.115631
positions[2] = 1.171292
positions[2] = 1.223828
positions[2] = 1.393478
positions[2] = -1.446829
positions[2] = 1.497745
positions[2] = 1.546420
positions[2] = 1.593008
positions[2] = 1.637637
Joint 3:
positions[3] = 0.000433
positions[3] = 2.902240
positions[3] = -2.494673
positions[3] = 6.060256
positions[3] = 6.040091
positions[3] = 6.021166
positions[3] = 6.003767
positions[3] = 5.987991
positions[3] = 5.973858
positions[3] = 5.961364
positions[3] = 5.950454
positions[3] = 5.941130
positions[3] = -3.799519
positions[3] = 3.975718
positions[3] = 2.457196
positions[3] = 2.446128
positions[3] = 2.436475
positions[3] = 2.428223
positions[3] = 2.421367
positions[3] = 2.415900
positions[3] = 2.411839
positions[3] = -0.355148
positions[3] = -0.348957
positions[3] = -0.341480
positions[3] = -0.332774
Joint 4:
positions[4] = -0.000017
positions[4] = -2 ...