PlanningPipeline::generatePlan generates discontinuous trajectories

edit

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 ...