Enable setting joint_distance factorin JointModel with ROS 2 parameter

[sjahr]

The JointModel class has a member called distance_factor which can be used to weight specific joints in distance calculations:

https://github.com/ros-planning/moveit2/blob/0f47f28d4e527362da421a51fe73a8abc1f716d1/moveit_core/robot_model/src/robot_model.cpp#L1481-L1491

It would be nice to make this factor configurable via ROS 2 parameter that is passed to the joint_model via a constructor argument (similar to name) since the robot_model is usually treated as const after construction

[henningkayser]

To elaborate on https://github.com/ros-planning/moveit2/pull/2027#pullrequestreview-1354296984

The purpose of the distance factor is to normalize state position distances for a set of joints, where currently each joint is assigned a factor for each variable dimension (1.0 in case of revolute and prismatic joints, x for multi-variable joints) so that JMG::distance() is computing the L1 norm of all dimensions. This does not only have an affect on how a distance is computed between state a and b, but also what the distance scaling is if you want to sample within a nearby position range, or what it means if you want to sample from a gaussian distribution. Take https://moveit.picknik.ai/humble/api/html/classmoveit_1_1core_1_1JointModel.html#adaf152251e15131a66e48f5e1de98dbb as example. If we change the distance factor we suddenly have a scaling mismatch between a distance reported by distance() and the distance that we pass in for local sampling. In other words, if the factor doesn't exactly match the number of dimensions, non-uniform random sampling will be skewed as a side-effect (please correct me if you think I'm wrong here).

So what to do? I think we should define the exact use cases of weighting joints differently for distances. My take is that it would be useful for prioritizing joints depending on dynamic limits or effort when planning and solving IK. What other use cases do you see?

Taking dynamic limits into account basically changes the state space that we are considering for planning. Instead of the plain position value dimension, we are scaling the space by a cost factor that relates from our search space (in OMPL the ModelBasedStateSpace) to the absolute joint values. Currently, the JointModelGroup is representing the mapping between search space and joint space (joints are only representing the mapping for variable dimensions). JointModelGroup is also responsible for normalizing the joint distances, it's not possible to do that on the joint level. If we want to factor in different joint weights, we should try to find a solution that works with JMG (or RobotModel) and that explicitly supports the weights that we need. Something like JointModelGroup::distance(from, to, weights) and JointModelGroup::sampleRandomPositionNearBy(..., weights) (odd, just noticed something like this seems to be already supported). If we have that, we can think about adding convenience functions for generating those weights (may even be ROS params) and add support to actually use them in OMPL and IK functions. I bet @v4hn or @rhaschke may have some interesting insights to this problem.

[github-actions[bot]]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[github-actions[bot]]

This issue was closed because it has been stalled for 45 days with no activity.