This is the first part on limiting joint windup for single IK solutions by setting a "soft" joint limit that still allows exceeding it intentionally. It's based on setting a joint value modifier in range [0,1] that is multiplied with the IK seed state to "pull" desired joints towards the center. This works best in combination with minimal-distance IK solvers (like currently LMA). To eliminate outliers completely the consistency limit of specified joints is set to the lower value PI.
This method allows to preserve the full freedom of the joint since only the seed state is modified.
This allows for consecutive IK solutions to still move the full way if the steps are sufficiently small (i.e. for cartesian planning). For example a value of 0.5 would allow the joint value to converge to 2PI but only if sequential IK calls produce solutions within closest proximity. The closer the the value is to the limit the lower the distance needs to be to still move towards it. In practice this means that the actually reachable limit is considerably below that one. Lower fractions let the value converge to lower limits (I could make a table for that). A good value to limit wrist windup would be 0.3 or maybe even lower (~1.4PI limit).
The modifiers are stored in a map ik_seed_statefractions and can only accessed by getter() and setter() functions that also test the validity of the values. The feature is enabled by calling robot.limitJointWindup(true) and is active for all getJointSolution() calls including waypoint conversions.
The test shows how this is applied to limit wrist joint windup by setting the corresponding fraction to 0.3.
I suggest specifying desired values either inside the kinematics.yaml or inside a new moveit_simple.yaml since I think this should be needed anyway.
This solution is meant to be used for all IK calls but does not support end effector symmetries.
For this a separate function will be needed anyway just as for separate pick/place state optimization.
I'm planning to do follow-up PRs for that so this one doesn't get bloated.
henningkayser
commented
5 years ago
This is the first part on limiting joint windup for single IK solutions by setting a "soft" joint limit that still allows exceeding it intentionally. It's based on setting a joint value modifier in range [0,1] that is multiplied with the IK seed state to "pull" desired joints towards the center. This works best in combination with minimal-distance IK solvers (like currently LMA). To eliminate outliers completely the consistency limit of specified joints is set to the lower value PI.
This method allows to preserve the full freedom of the joint since only the seed state is modified. This allows for consecutive IK solutions to still move the full way if the steps are sufficiently small (i.e. for cartesian planning). For example a value of 0.5 would allow the joint value to converge to 2PI but only if sequential IK calls produce solutions within closest proximity. The closer the the value is to the limit the lower the distance needs to be to still move towards it. In practice this means that the actually reachable limit is considerably below that one. Lower fractions let the value converge to lower limits (I could make a table for that). A good value to limit wrist windup would be 0.3 or maybe even lower (~1.4PI limit).
The modifiers are stored in a map ik_seed_statefractions and can only accessed by getter() and setter() functions that also test the validity of the values. The feature is enabled by calling robot.limitJointWindup(true) and is active for all getJointSolution() calls including waypoint conversions.
The test shows how this is applied to limit wrist joint windup by setting the corresponding fraction to 0.3. I suggest specifying desired values either inside the kinematics.yaml or inside a new moveit_simple.yaml since I think this should be needed anyway.
This solution is meant to be used for all IK calls but does not support end effector symmetries. For this a separate function will be needed anyway just as for separate pick/place state optimization. I'm planning to do follow-up PRs for that so this one doesn't get bloated.