How can I get the partial derivative of the transformation matrix w.r.t. the joint angles

[BolunDai0216]

Hi,

I was wondering, for a robot manipulator, is there an easy way to get the derivative of a transformation matrix of a link with respect to the joint angles, i.e., $\frac{\partial T}{\partial q}$, in pinocchio?

Thanks in advance!

[fabinsch]

Hi @BolunDai0216,

what you want to compute is J(q) = dT(q) / dq which is the regular jacobian. In Pinocchio, you can use computeForwardKinematicsDerivatives for this. This function is similar to doing a forwardKinematics(model,data,q,v) followed by a computeJointJacobians(model,data,q). Afterward, you can get the Jacobian for the specific joint by calling the getJointJacobian function.

[fabinsch]

Ok, I understand what you are looking for. This Euclidian way of describing the Jacobian is not the default one, Pinocchio has no built-in function to directly give you the mapping from 6x9 to 4x4x9. But there are all the functions, that you can assemble this quantity by yourself. You can create pin.Motion objects from the cols of the jacobian matrix (twist = pin.Motion(jacobian[:, i])) and then call methods on it to obtain different representations (twist.toActionMatrix()). Then using the transformation for each joint you can obtain the quantity you're looking for.

[BolunDai0216]

Thanks for the response. One question I have is with twist.toActionMatrix(), after running twist = pin.Motion(jacobian[:, i]) I don't think twist has the member function .toActionMatrix()

>>> dir(twist)
['Random', 'Zero', '__add__', '__array__', '__class__', '__delattr__', '__dict__', '__dir__', '__doc__', '__eq__', '__format__', '__ge__', '__getattribute__', '__getinitargs__', '__gt__', '__hash__', '__iadd__', '__init__', '__init_subclass__', '__isub__', '__le__', '__lt__', '__module__', '__mul__', '__ne__', '__neg__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__rmul__', '__safe_for_unpickling__', '__setattr__', '__sizeof__', '__str__', '__sub__', '__subclasshook__', '__truediv__', '__weakref__', '__xor__', 'action', 'angular', 'copy', 'cross', 'dualAction', 'isApprox', 'isZero', 'linear', 'np', 'se3Action', 'se3ActionInverse', 'setRandom', 'setZero', 'vector']

If you are referring to twist.action, I think the output is $[[R, [p]R], [0, R]]$ which 6x6. From my derivation, I think I would need to get $[[[\omega], v], [0, 0]]$ from twist and multiply it with the transformation matrix, is there a way to get the $[[[\omega], v], [0, 0]]$ matrix (which is 4x4) using any built-in function. This is not a difficult thing to write by myself, but using a built-in function makes the codebase cleaner.

[fabinsch]

you are right. I think the function you are looking for is toHomogeneousMatrix which is binded with homogeneous. Please check out the se3-implementation of Pinocchio to see all available functions.

[BolunDai0216]

I think homogeneous matrices are the same thing as transformation matrices. I took a look at the se3 implementation and I don't think such a functionality exists. I believe $[[[\omega], v], [0, 0]]$ is a fairly common value that might be used, would it be possible to add a function to compute this in future releases?

[stephane-caron]

I think homogeneous matrices are the same thing as transformation matrices.

Homogeneous representations are more general: transformation matrices are the homogeneous representation for $SE(3)$, and there is a homogeneous representation for $se(3)$ as well. You can find more about this in e.g. Chapter 2 Rigid Body Motion of A Mathematical Introduction to Robotic Manipulation. Here is a relevant equation from p. 39:

I took a look at the se3 implementation and I don't think such a functionality exists.

True, toHomogeneousMatrix is a function in SE3, while here we are in $se(3)$, which is called Motion in Pinocchio 😅 I couldn't find such a function either, and I agree it would be a consistent feature to add. Let's open a feature request.

In the meantime we can do it in Python:

def to_homogeneous(twist: pin.Motion) -> np.ndarray: 
     homogeneous = np.zeros((4, 4)) 
     homogeneous[:3, :3] = twist.action[:3, :3] 
     homogeneous[:3, 3] = twist.linear 
     return homogeneous