search

stack-of-tasks / pinocchio

A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
http://stack-of-tasks.github.io/pinocchio/
BSD 2-Clause "Simplified" License
1.78k stars 375 forks source link

Modifications of Joint Configuration #1363

Closed agutte closed 3 years ago

agutte commented 3 years ago

Hi,

Is there a method to get the current joint configuration from data? At least I did not find any obvious method in the data interface. In addition, I'm wondering if the current joint configuration in data is modified by the call of crba. Basically I want to get the subtree inertia matrices of different configurations without modifying the current joint configuration in data.

Regards, Alex

jcarpent commented 3 years ago

We do not save the current configuration vector value in Data. Calling CRBA changes the state of Data. You can retrieve the inertia of a given subtree starting at the ith joint, by simply calling:

data.Ycrb[i]
agutte commented 3 years ago

Thanks for your quick reply! So is there a way to get the sub tree inertia (specifically data.Ycrb[1]) without modifying the state of the robot?

jcarpent commented 3 years ago

Maybe, I don't get your point. But data.Ycrb[1] is the output of an algo (CRBA, etc.), so it depends on the state of the system.

What do you want to do exactly?

agutte commented 3 years ago

Okay, true, that was a bit unclear.

From a higher perspective, we want to write an interface that wraps the most relevant functions of pinocchio for our application (e.g. for specific configurations calculate forward kinematics, return the CoM and inertia, etc.). However, we still want to make sure that no unnecessary calculations are made.

Currently we are using framesForwardKinematics to calculate the forward kinematics and update the frame positions. When we want to calculate the inertia of a specific joint configuration, we have to call CRBA which computes the forward dynamics but also calculates the forward kinematics as "pre-step" so to say, right? As a consequence, subsequently calling framesForwardKinematics for other calculations with the same joint configuration would make our interface inefficient since we calculate the forward kinematics twice, isn't it? Because apparently Pinocchio is not able to check if the forward kinematics are already calculated for the given configuration by a previous function call (i.e. crba).

So one solution to our problem would be to store the configuration of the last forward kinematics call in our interface class and check if the forward kinematics are already calculated for the given config by a previous function call. Or is there may be a better approach?

I hope this makes things a bit clearer.

jcarpent commented 3 years ago

Many algorithms might change quantities contained in Data. It would be then very hard to track this change by simply recording the value of the state vector, etc.

We let the user manage these details. Depending on the application, it might be often easier.

In your case, you should better use updateFramePlacements after calling CRBA for instance. This will take the values contained in data and just update the fields data.oMf.

At some point, it should be simple for the end-users to copy-past some Pinocchio algos in order to write their own optimized version. This is for instance what is behind computeAllTerms which computes all the necessary quantities for controlling a robot for instance.

If you want further information @agutte, do not hesitate.

agutte commented 3 years ago

Thank you @jcarpent , that was very helpful! I think that's all I need so far.