moveit
moveit copied to clipboard
ros-planning
Pilz PTP planner: stationary joint limits can effect on moving joints, unnecesary reducing the speed of motion
Problem Description
In the pilz planner PTP computation here the velocity profile is computed using the conservative limit most_strict_limit_.
The value of most_strict_limit_ is computed here as the minimum value of speed, accelerations and decelerations for all the joints. Event those joints that do not move.
As a consequence motions that just concern few joints are computing considering the limits of the other joints that do not move. If we desire to move just a fast joint, the planner will impose on it the constraint of slow joints.
On case where this can be problematic, is when one desire to move the end-effect first using the first two/three joints (shoulder and elbow, etc., generally the must powerfull motors) for rough but fast positioning, and then using all joints for slow fine tuning.
Desired behavior
The PTP planner should only consider the limits of the joints involved in the actual motion.
Possible solution
One possible solution could modify active_joints here to
std::vector<std::string> active_joints;
const std::vector<std::string>& all_joints_in_group = jmg->getActiveJointModelNames();
std::copy_if(all_joints_in_group.begin(),all_joints_in_group.end(),
std::back_inserter(active_joints),
[&start_pos, &goal](const auto& _in){
return fabs(start_pos.at(goal.first) - goal.second) >= MIN_MOVEMENT;
} );
I can do the pull request if this solution is ok.
I would expect that if you have logic that uses different joint subsets, you do that through different (though subsets) joint model groups. Would that not solve your case as well?
Otherwise this is indeed a special case in the generator that could be optimized separately, but it would be a special case so it would need a rather elaborate test case.
Also a good value for MIN_MOVEMENT is difficult to give because it depends on the level of noise on your robot.
That is unless you only want to optimize the case where start and goal state are passed to the planner explicity (instead of reading the current robot state).
If we want to support only this case you could pretty much pass FLT_EPSILON.
I agree in several points. In fact, if an application intents to decouple the motion of the joints (e.g. first three joints and wrist) the correct solution is to go on separating the joint in different groups. This decouples the logic of motions from the joints.
I believe that the movement of single joints is not a very special case. For example, in developing phase this can be very common. I can think in situations like teaching waypoints or moving the base to catch out the robot from an area. Place the velocity constrains of the slowest joint can make it very slow, and it can be difficult to justify the introduction of a new joint group just for few motions in the developing phase or even in the application.
I don't see yet the difficulty in implement a test for this case, we only have to check that the general tests also work after introducing the lines I have proposed and that the correct limits are respected. These are returned by `getCommonLimit´, and seems easy to get. Unless I'm missing something.
The value MIN_MOVEMENT is already defined and used in the pilz library.
Feel free to submit a pull-request if you provide a test with it. I'm still not convinced it's a very common case, but it can help in some situations.
Just wanted to chime in that Rafael isn't the only person affected by this. My robot has a linear joint that is quite slow, and it's causing all PTP moves to be slow as a result even though that joint often doesn't need to move. Another move-group would work, but this solution is far more elegant and what a new user would expect.
