IK with rotational constraint

[arjung128]

Hello,

I have been able to build off of the following code for IK with positional constraints: https://github.com/stack-of-tasks/pinocchio/blob/master/examples/inverse-kinematics.py

Note that I am doing IK with a model that has a pin.JointModelFreeFlyer(), so I can simply add something like:

q[0] = base_x
q[1] = base_y
q[2] = 0.

inside the end of the optimization loop, and the positional constraint is met.

My issue is that while the positional constraint is met, the base of the robot is rotating freely in 3D. I would like to restrict it to only rotate in 2D (i.e. on the ground). My attempt was to add:

from scipy.spatial.transform import Rotation as R
z_angle = R.from_quat(q[3:7]).as_euler('xyz')[2] # extract the 2D rotational angle about the z-axis
q[3:7] = R.from_euler('xyz', [np.pi/2, 0., z_angle]).as_quat() # keep this, and set everything else 

inside the end of the optimization loop, however, this IK call never seems to converge. I am certain that an IK solution exists, as I have verified this by doing the IK on a model without the pin.JointModelFreeFlyer() (but the issue with doing IK without the pin.JointModelFreeFlyer() is that I believe I can only find solutions for fixed rotations).

Any ideas about how else I could try to enforce this base rotational constraint without having IK diverge? Any help will be much appreciated, thanks in advance!

[stephane-caron]

You can formulate your IK as having two tasks:

1. one for the base, with higher priority (which is what you are aiming for by overriding the values of the configuration vector), and
2. one for the end effector, with lower priority.

When there is more than one task, the Jacobian matrices and vectors can be stacked, see e.g. this tutorial on weighted inverse kinematics. Be careful that the Jacobian for your rotational task should include a $log_6$ (pin.log6 function) as explained in this note on pose task Jacobians. (In case of doubt, you can always take a look at the FrameTask in Pink, it handles these caveats.)

[arjung128]

Thanks so much for your suggestion. I was able to achieve what I wanted to with the use of pin.JointModelPlanar(), but glad to know about weighted inverse kinematics, too, as this will likely be very useful for me. Thanks!