moveit planner problem with J1 and J2

[toliver]

The planner complains about these joints being out of bounds. That happens quite often, as the position controllers only control the sum of both J1+J2=J0, but not each one of them separately.

[ugocupcic]

We need to investigate if we can replace our j0s with mimic joints. It's available in urdf. It would also help with #287

[guihomework]

@ugocupcic I have been working with a mekabot m3 hand, which has a lot of coupled joints per fingers (highly underactuated). mimic joints work great if the driver provides a value that is the "motor" joint that should be distributed among several joints.
On the shadow hand this means if j0 was provided by the motor (like measured on tendon side as on the mekabot), mimic would do j1=j0/2 and j2=j0/2
But the problem is that the shadow hands neither have a fixed coupling, nor a motor side sensor, BUT have sensors on both j1 and j2 joints. It is really nice to know the real current value of the joints J1 and J2, in the real world, but this blocks us from using mimic. (Rare shadow hands have a fixed coupling where j1,j2 separation is worthless)
Inverse kinematics from KDL in moveit also handles mimic from what I have read. That could help get rid of special hand_kinematics for coupled joints.

At some point I was thinking of having two robot_description. One for simulation with mimic and one for the real world without mimic. Deactivating mimic in the real world but using all the power of mimic (with a virtual joint 0 present in the urdf directly that would be attached to a simple j0 controller). That would however not let you get rid of all the special code for has j2 due to real world having to deal with that in the controllers.

I like the fact that the host side controllers that run the real hand are tested in simulation as is (and not have a special version in sim).

I think the problem is that the way the joints j1 and j2 are moved in sim is the problem. Sending the same force on both joints (or half I am not sure anymore) does not ensure the joints move the same angle (because there are serial links and the j2 has different inertia and mass to move than j1 hence, will move differently)

This latest issue should be investigated I think or discussed with gazebo people. I think something enforcing the coupling (probably what is done by mimic) in the transmission or in the sr_gazebo_plugin should be done. Then the initial problem goes away and we have a better simulation. This would not simulated the spring coupling but that is not as important as keeping the same controller for real and sim imho.