Getting Kinematic Hessians in Python API

[EpicDuckPotato]

Hi, Is there a way to use getJointKinematicHessian in the Python API?

[jcarpent]

Not yet, we should first expose Eigen::Tensor into Python (interfacing Eigen::Tensor and np.array) as explained here https://github.com/stack-of-tasks/eigenpy/issues/247 What is your need? Which computation do you want to perform?

[EpicDuckPotato]

I'm writing a custom actuation model to use with crocoddyl, and one of the derivatives requires a joint kinematic hessian.

[jcarpent]

Could you write the equations here? I do think you don't really need them ;).

[EpicDuckPotato]

The robot is described by a URDF. It has a floating base, revolute joints, and thrusters. The dynamics are M\dot{v} + Cv + g = Bu

v consists of body twist and joint velocities.

u consists of thruster forces and joint torques.

To map thruster forces to generalized forces, I use the B matrix, where the ith column corresponds to the ith thruster. The column itself is the transposed row of the Jacobian corresponding to the thruster's z axis (axis along which it exerts thrust).

In crocoddyl, I need to calculate d\tau dx, i.e. the derivative of the generalized force with respect to state. B consists of Jacobians, so to differentiate it with respect to the state, I need a Hessian.

Just a note, I have fixed joints on the URDF corresponding to the thrusters, with dummy links attached to them, so I usually construct the B matrix by getting the frame Jacobians of these links

[jcarpent]

The B matrix seems to be constant, no?

[jcarpent]

Is it depending on q?

[EpicDuckPotato]

The B matrix isn't constant. The thrusters are sitting on links which can be moved by the revolute joints.

[cmastalli]

Hey @EpicDuckPotato!

Not sure if you're doing the same, but we have a multicopter actuation defined in Crocoddyl. For more details see: https://github.com/loco-3d/crocoddyl/blob/master/include/crocoddyl/multibody/actuations/multicopter-base.hpp#L21-L37

[cmastalli]

Of course, this assumes that the thruster positions are fixed, and therefore B is constant. However, thinking twice, it seems you have thrusters which their locations might change w.r.t. the robot joints, right?

[EpicDuckPotato]

Yeah In the multicopter, tau_f is fixed, but in my case, tau_f would depend on the joint angles

[cmastalli]

Following your needs about implementing an actuation model in Crocoddyl, then I wonder why you're looking for the kinematic Hessians. In Crocoddyl, for the actuation models, we use the Jacobians only.

[EpicDuckPotato]

Right, but in the calcDiff function for an action model, I need to calculate dtau_dx, right? This requires differentiating B with respect to q. And the columns of B are constructed using Jacobians. So differentiating the Jacobians wrt q requires Hessians.

[cmastalli]

And the columns of B are constructed using Jacobians. So differentiating the Jacobians wrt q requires Hessians.

This means that your B matrix depends on the joint velocities, not joint positions. Is this what you need to do? In any case, it would be easier to help you if you describe precisely your model (as @jcarpent suggested as well)

[EpicDuckPotato]

Sorry, B depends only on joint positions. Each column of B corresponds to a row of a Jacobian (and the Jacobian depends on joint positions)

[EpicDuckPotato]

[EpicDuckPotato]

The first n_t columns of B correspond to thrusters, the last nj columns correspond to revolute joints. Each thruster acts along its z axis, so J{i, z} corresponds to the linear z row of the ith thruster's Jacobian. The Jacobian I'm talking about maps velocities of generalized coordinates (i.e. twist of the base link and revolute joint velocities) to twist of the thruster. Therefore I'm using the transpose to map z force along the thruster z axis to generalized force.

[jcarpent]

In this case, you don’t need really to compute the Hessian you mention. Your trust forces are constant in the local frame of the propeller. You can then use the signature of the forward dynamics derivatives which also account for the external forces.

[EpicDuckPotato]

Is this true if my thruster forces are control inputs?

[jcarpent]

Could you send the model of your system? A raw picture of it just to understand the way it is organized. Is there any articulation between your truster and the base of your system?

[EpicDuckPotato]

It's an underwater snake robot with propellers.

[EpicDuckPotato]

Hopefully it's clear from the above image, but there is articulation between the base link and the thrusters.

[jcarpent]

You can do as I mentioned previously, the only remaining part of the missing derivative is due to the joint propeller. For that, you will need to add M^{-1}J_i^t where J_i is the Jacobian of the Frame related to the propeller i expressed in the LOCAL coordinate system of the frame.

If you need more help on the math explanations, we can plan a short discussion on Monday.

[EpicDuckPotato]

But crocoddyl requires the derivative of the generalized force wrt the state. Does this not require differentiating J_i^t with respect to the configuration variables?

[jcarpent]

This is what J_i provides in fact

[jcarpent]

We will add soon the notion of Actuator in Pinocchio. All these computations will become easier with this new feature.

[EpicDuckPotato]

J_i is the derivative of the twist of the thruster frame with respect to the configuration variables. The generalized force contributed by thruster i is J_i^T multiplied by the force of thruster i. The derivative of the generalized force wrt the thruster force is easy, as I just use the corresponding columns of B. However, the derivative of the generalized force wrt the configuration variables requires differentiating B, which requires differentiating J_i with respect to configuration variables.

[jcarpent]

@EpicDuckPotato You are missing the fact that your propeller force is a Force constant in the local frame of your tip joints. In other words, it acts on the joint as jMp.act(f_propeller) where jMp is the relative placement between the propeller and the supporting joint.

[EpicDuckPotato]

Isn't the generalized force exerted on the free-floating base state-dependent?

[jcarpent]

Yes, you're correct. But you have a simpler way of dealing with your setting by considering the forces in their local frame directly acting their supporting joint. This is where you will be both sparse and efficient.

[EpicDuckPotato]

How should I reformulate my dynamics to do it this way?

[jcarpent]

We already explained everything above.

[EpicDuckPotato]

Same set of generalized coordinates? B matrix doesn't change?

[jcarpent]

Yes.

[EpicDuckPotato]

What changes, then? Or are you suggesting to have generalized coordinates for each link?

[jcarpent]

Please look above. You should consider your propeller force as external forces.

[jcarpent]

I leave you with this conclusive answer.

[EpicDuckPotato]

Sorry, I did not understand until I looked at the signature for computeABADerivatives() involving fext. I didn't realize pinocchio had this feature. Thanks!

[cmastalli]

@EpicDuckPotato --- Let's make a bit of notation. Your forward dynamics are a function of q, v, tau, fext, i.e., a = ABA(q, v, tau, fext).

In Crocoddyl, the actuator model maps control inputs into generalized joint torques (I.e., tau). Instead, what it was suggested is to map the thrust forces into exerted forces (I.e., fext).

As you can see there is a current limitation in scope of Crocoddyl; however, it could be handled, for instance, by extending the actuation-model concept. In any case, this requires to study first different possibilities.

What I suggest you if to become an active key for enabling this in Crocoddyl. But first, you need to write an issue to discuss different possibilities.

[cmastalli]

If so, please do it in Crocoddyl as Pinocchio is not the right place.

[jcarpent]

Sorry, I did not understand until I looked at the signature for computeABADerivatives() involving fext. I didn't realize pinocchio had this feature. Thanks!

OK. Thanks for this ack. If you use Pinocchio/Crocoddyl in your research or development, we will appreciate you cite them in your academic papers or software. Thanks in advance.

[EpicDuckPotato]

Yep, will do.

[jcarpent]

If so, please do it in Crocoddyl as Pinocchio is not the right place.

@cmastalli We already discussed several times this point, but the notion of Actuator should be implemented within Pinocchio rather than in Crocoddyl. This will provide more generic types of actuators, with better handling of the sparsity, in addition, to be available to a broader class of users.

[cmastalli]

If so, please do it in Crocoddyl as Pinocchio is not the right place.

@cmastalli We already discussed several times this point, but the notion of Actuator should be implemented within Pinocchio rather than in Crocoddyl. This will provide more generic types of actuators, with better handling of the sparsity, in addition, to be available to a broader class of users.

Hey @jcarpent

We need to come out with strict deadlines of delivering and potential ways of team work on that topic first. This will help us to define transition plan. However, at the moment, the obvious solution for this case is to stick with the current API in Crocoddyl.

Let's discuss about it in private. Please also know that my greatest effort is on having inverse-dynamics and solvers that handle arbitrary constraints. I am afraid of having many parallel works and how they can be integrated smoothly in Crocoddyl.