Constrained IK (CLIK) for dual arms

[davetcoleman]

What do you suppose it would take to adapt the constraint_ik to dual arm capabilities, with overlapping toros joints? I have past experience with this kind of project but am not sure if there are any mathematical limitations to the way constraints are currently being applied to the null space. I understand that this solver originally did support dual arms but it no longer does? @drchrislewis

[drchrislewis]

@davetcoleman The original CLIK code simply constructed a dual arm jacobian using the DH parameters from both arms that related the joint motions to the velocities of the second tool point in the first robot's tool frame. This Jacobian is straight forward to build by walking the DH parameters in reverse down from the first robots tool to its base then up to the first robot's tool in forward order. Two 6DOF robots provide 6 redundant DOF. Now if you want to have zero motion between the tools while they grasp an object, it must be in the null-space of this jacobian. Obstacle and joint limit avoidance are also projected into the null-space. Routing edges of a part held by one robot only requires 5DOF int he task space, so this provides one more or 7 redundant DOF. Its been a while since I've looked at CLIK, so I don't have much memory of its implementation. It evolved considerably since others took it over.

[Levi-Armstrong]

To accomplish the above comment you will need to implement your own constraint. I am on travel but I can provide links and additional information this evening.

[davetcoleman]

Thanks @Levi-Armstrong

[Levi-Armstrong]

@davetcoleman
I have been looking through the code and I believe it should work as @drchrislewis described assuming KDL supports the scenario where the base of the chain is not a fixed frame. If it does not I know another way to achieve what you want but it may require some code changes to handle multiple chains to solve them simultaneously. If you don't want to make changes to the code everything will work but you will have to manage every thing within your constraint and ignore the kin object. Your constraint would get initialized by two groups where you store two kin objects internally one for each move_group. Then for the constraint methods evaluateConstraint you would calculate the Jacobian and error for each chains and then combine the Jacobian from each into a single matrix and do the same with the error. Then the solver will solve for both simultaneously. Ideally it would be nice to update the base_kin and solver_state to support multiple kinematic chains.

Here is a cpp implementation.
Cpp Example

You will need to implement your constraint for the dual arm shown below.
Constraint Base Class