Does it support trees (multiple arms) ?

[firesurfer]

Hi,

I am currently looking into planning for multiple arms for our setup. A while ago I stumbled upon a post which said that BioIK does support tree like kinematic structures, is this also true for pick-ik ?

My tests so far where ambiguous. During startup the robot_model_loader complains that the plugin does only support chains:

[moveit_ros.robot_model_loader]: Kinematics solver N7pick_ik12PickIKPluginE does not support joint group both.  Error: This plugin only supports joint groups which are chains

But while planning for a jointspace goal with OMPL it just plans a trajectory for both of the arms.

On the other hand when trying to plan two linear motions with the Pilz Planner it complains about missing an IK solver instance:

[moveit.pilz_industrial_motion_planner]: Cannot service planning request because group 'both' does have an IK solver instance.

So does it support trees or not ? And if not, why does it work with OMPL ?

If it does support trees it would probably make sense to implement the KinematicsBase::supportsGroup method and have it return true. (At the moment I think the method from the base class is called: https://github.com/moveit/moveit2/blob/358ec63d9a8fee3d2e29ee37413029ff4eff3426/moveit_core/kinematics_base/src/kinematics_base.cpp#L142 which checks if the JointModelGroup is a chain)

[sea-bass]

Planning a joint space goal with OMPL doesn't require an IK solver at all (it just samples joint states), but Pilz linear planner does (solves IK along Cartesian path), so I guess that part is expected.

But as you said, I think a benefit of pick_ik's "brute force" finite differencing/evolutionary algorithm approach is that it should support trees.

Could you try implementing that supportsGroup function and see how it works?

Might be good to also add some unit tests for a simple tree to validate this as well. The unit tests already have a simple 2 or 3DOF robot as well as the Panda, which are both chains.

[firesurfer]

Planning a joint space goal with OMPL doesn't require an IK solver at all (it just samples joint states), but Pilz linear planner does (solves IK along Cartesian path), so I guess that part is expected.

You are right about that. The forward kinematics is enough for that. Nevertheless I think I couldn't make it make it work with KDL if I remember correctly. Therefore I assumed that at some point it also calls the IK solver.

Could you try implementing that supportsGroup function and see how it works?

I just did that. Unfortunately Pilz does not support goal constraints for multiple tips. But while trying to get linear planning to work with multiple arms somehow, I tried to call .setFromIK on a RobotState with cartesian goals for both arms and this seemed to work. (Directly adding the cartesian goal constraints did not work because the ConstraintsSampler does not support multiple tips).

So the conclusion from my side is: The IK solver supports trees but the rest of moveit struggles with it. (If you have some recommendations how to achieve linear planning for multiple arms I would be happy to hear it)

[sea-bass]

I can't think of anything too straightforward... except one thing.

Besides the Pilz LIN planner, there is also a "cartesian interpolator" feature in MoveIt that may call these functions in a way that works.

https://github.com/moveit/moveit2/blob/c72ac1bfc4fad1288bc0e5361c16edaaf8b90a01/moveit_core/robot_state/src/cartesian_interpolator.cpp#L174

So maybe give that a look to see if that is a quick workaround?

[firesurfer]

Thanks for you answer. Just from the method signature I do not think there is a way to get this to calculate a motion for multiple tips at once:

CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath(
    RobotState* start_state, const JointModelGroup* group, std::vector<RobotStatePtr>& path, const LinkModel* link,
    const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step,
    const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback,
    const kinematics::KinematicsQueryOptions& options, const kinematics::KinematicsBase::IKCostFn& cost_function,
    const Eigen::Isometry3d& link_offset)

But that is not the topic of this issue.