Dealing with armature in crba, computeAllTerms and other dynamics algos

[proyan]

Currently we can load the rotor parameters inside Pinocchio, but they are not used anywhere in the algorithms. As a result, forwardDynamics, and ImpulseDynamics are useless if you need the armature, since both internally computes the mass during computeAllTerms.

I can take care of adding the armature values to crba and computeAllTerms by either: 1) Modify the current algos and add a (by default armature is zero) value to the mass diagonals() 2) create a separate algo.

Would (1) be okay with you?

[jcarpent]

The problem of the armature is for multi-dof joints. Currently, I do not see how to handle them correctly in this meaning.

[jcarpent]

The correct way to deal with armature information would be by implementing #440 features, but no one is available to implement it.

[proyan]

440 would require plenty of work. It has been open for more than a year, and I don't see that it will be done anytime soon.

However, just adding the available aramature to the mass matrix would provide an acceptable solution for the time being. This would be beneficial for anyone doing control.

[proyan]

Plus, we already save the information in model class.

[jcarpent]

Indeed, we save it as it is now because we have not implemented a convenient and generic solution, as suggested in #440.

[jcarpent]

To conclude, I think you can proceed as 1 but this is not the most elegant and generic solution. By the way, you should assert that the armature is indeed a positive vector.

[proyan]

I agree it isn't generic. The solution will just be a placeholder untill #440 is solved. I'll work on it.

[proyan]

Extending Armature to ABA and RNEA:

I understood how to add the armature to the aba (and by extension, abaderivatives). It requires me to: 1) call calc_aba in AbaForwardPass2 2) define a constraintAxisSelector in the constraintBase class (akin to the jointConfigSelector). It takes as input a 6D vector, and returns the segment where jdata.S() is active.

Proposals: 1) (1) seems more intrusive than simply adding a diagonal term (as in the case of crba and computeAllTerms). I propose to add a function abaWithArmature, which uses (1) as the ForwardPass2. 2) I will simply add the armature terms to RNEA, instead of creating a new function.It is again only one extra addition operation. Unfortunately, this means that we have two aba (aba and abaWithArmature), but one RNEA.

Would you be okay with that?

[nmansard]

About ABA: adding a new aba-with-armature method seems interesting to me. I think we can make the assumption that armature is always 0 for free flyers (as i) it is not mechanically relevant to have armature on a free flyer and ii) I believe it is equivalent to add armature to a free joint or modifying the inertia matrix).

About RNEA: I don't think you should consider armature at all in RNEA and CRBA. It is very easy for the user to modify the output of the classical RNEA-CRBA to take into account the armature. At maximum, you could add rnea-with-armature and crba-with-armature that post process the rnea and crba outputs.

Side remarks:

r1) you have originally chosen to record rotor inertia and gear ratio in the pinocchio::Model. I think we should rather only record armature. This is more generic and easier to use. The SRDF parser can compute the armature on the flight and discart gear/rotor infos.

r2) Are we sure that there is not equivalent 6d inertias that would correspond to an armatured system, ie given a set of body inertias Y_i and joint armature a_i, is it not possible to find a set of Ya_i so that the RNEA on Ya_i is equivalent to the RNEA on Y_i plus the armature (and equivalently on the ABA).

On 2/14/19 3:55 AM, Rohan Budhiraja wrote:

Extending Armature to ABA and RNEA:

I understood how to add the armature to the aba (and by extension, abaderivatives). It requires me to:

1. call calc_aba in AbaForwardPass2
2. define a constraintAxisSelector in the constraintBase class (akin to the jointConfigSelector). It takes as input a 6D vector, and returns the segment where jdata.S() is active.

Proposals:

1. (1) seems more intrusive than simply adding a diagonal term (as in the case of crba and computeAllTerms). I propose to add a function abaWithArmature, which uses (1) as the ForwardPass2.
2. I will simply add the armature terms to RNEA, instead of creating a new function.It is again only one extra addition operation. Unfortunately, this means that we have two aba (aba and abaWithArmature), but one RNEA.

Would you be okay with that?

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/stack-of-tasks/pinocchio/issues/671#issuecomment-463466822, or mute the thread https://github.com/notifications/unsubscribe-auth/AAb-CFCzmoXE9dcRAR4YbIKh8Wc8cOxUks5vNNAdgaJpZM4a5NMY.

[gabrielebndn]

Side note: if armature data are taken into account, then they should also be considered in a lot of other places. In particular, in the corresponding derivative algorithms, in the kinetic energy algorithm, in the centroidal momentum algorithm, and probably somewhere else

[proyan]

About RNEA: I don't think you should consider armature at all in RNEA and CRBA. It is very easy for the user to modify the output of the classical RNEA-CRBA to take into account the armature. I do need to add the armature to the compute-all-terms, since it is used internally for computations of dynamics and mass is not an output. For RNEA and CRBA, I am happy with either option. It would not be much of a difference computationally whether the armature is dealt within Pinocchio, or outside by the user in case of RNEA and CRBA. The only reason I would have added is to maintain consistency with compute-all-terms. At maximum, you could add rnea-with-armature and crba-with-armature that post process the rnea and crba outputs.

Sounds good to me.

Side remarks:

r1) you have originally chosen to record rotor inertia and gear ratio in the pinocchio::Model. I think we should rather only record armature.

It was my mistake to save rotorInertia and gearRatio :(. It should have directly been armature the first time. I am now saving and doing the computations only with armature. I'll put rotorInertias and gearRatio as deprecated, if @jcarpent agrees.

r2) Are we sure that there is not equivalent 6d inertias that would correspond to an armatured system, ie given a set of body inertias Y_i and joint armature a_i, is it not possible to find a set of Ya_i so that the RNEA on Ya_i is equivalent to the RNEA on Y_i plus the armature (and equivalently on the ABA).

It is indeed. For ABA however, since we are iterating over the inertia elements, we can't update the Y_i by default with armature (if we do that, the armature of a joint will propagate to all its parents.).

So based on the above two comments, I am implementing: abaWithArmature, crbaWithArmature, rneaWithArmature, the derivativesWithArmature and I am updating computeAllTerms to include armature. I will keep the new functions in the same file as the original ones (i.e., no new armature.hpp file.).

Sounds good?

[jcarpent]

So, currently and for your purpose, I think adding armature to CRBA is sufficient and I would prefer to think twice before adding it to other places. Indeed, by just adding the armature to the mass matrix only, you are not taking into account the Coriolis and centrifugal effects I guess.

Concerning the modification of ABA to take account the armure, I already implemented an elegant solution which does not require additional operators as you suggest. This was done one year ago, but not merged in the main repository. I will focus on it next week if it is fine for you.

[jcarpent]

So, in conclusion, only implements the account for armature for the computeAllTerms and add a vector to define the generic armature, make suppress the other ones because I think you were the only to use it.

[jcarpent]

From my side, I will implement the efficient ABA version with armature.

[jcarpent]

@traversaro @DanielePucci How do you take into account the armature from your side?

[traversaro]

@traversaro @DanielePucci How do you take into account the armature from your side?

I think @gabrielenava is the right person to point to the relevant code and papers, as he is the one dealing with this. In particular, he also developed a reflected inertia controller that accounts also on the coupled joints of iCub.

However, from the technical point all those modifications are done on the Matlab/Simulink side, i.e. none of the terms that a they are not contained in the floating base RNEA/CRBA implementation in iDynTree/WB-Toolbox. The main reason why we never integrated reflected inertia, transmissions and basic friction models in iDynTree/WB-Toolbox is that we never properly spent some time in trying to understand how to expose them in a way that is sufficient transparent and clear to the C++/Matlab/Simulink user. In particular, I think there are several applications for which the C++/Matlab/Simulink user wants to use the "usual" Mass Matrix/Inverse Dynamics that does not take into account the transmission at all, and I am not sure what is the correct interface to expose to enable that. I fear that storing these "transmission" parameters in the Model data structure and requiring the user to set them to zero to ignore any transmission effect is not enough transparent to the library user, and risk of being error prone.

[gabrielebndn]

There are use cases where you do not want the rotor moment of inertia in your inertia matrix. Forcefully embedding them in the dynamic computations assumes a strong rigid link between motor and joint which might not be there. For instance, for Series Elastic Actuators, the equations are

M ddq + n = K(theta-q) B ddtheta + K(theta-q) = tau

Where M does not include the rotor inertia moments.

[jcarpent]

That motivates the need for transmission and actuation models.

[proyan]

@gabrielebndn @traversaro wrt the issue you raise about adding armature being error prone, this is what @nmansard is proposing in #676 : a separate file armature.hpp which wraps the original algorithms with a suffix withArmature (or something else). This way, the set of algorithms which deal directly with the robot dynamics, and the set of algorithms which include in some way the armature are easily identifiable.

[gabrielenava]

I think @gabrielenava is the right person to point to the relevant code and papers, as he is the one dealing with this. In particular, he also developed a reflected inertia controller that accounts also on the coupled joints of iCub.

The main documentation and theory concerning the reflected inertia controller is inside a private organization in github, therefore I copied the relevant info, link to the paper and other important links inside another issue that can be found at the following link:

https://github.com/robotology/whole-body-controllers/issues/61

In particular, adding the motors reflected inertia to the system's mass matrix helped to improve the numerical stability of a Simulink-based momentum-based controller for humanoid robot balancing. It is worth noticing that we assumed the robot to have rigid coupling between motors and joints. As @traversaro and @gabrielebndn already mentioned, our approach does not scale as it is for example in case of a robot with Series Elastic Actuators.

cc @DanielePucci

[jcarpent]

@gabrielenava Thank you very much for taking these and providing very nice and useful explanations.

[DanielePucci]

In addition to what @traversaro and @gabrielenava said, let me add that exploiting friction and motor reflected inertia in the case of iCub increased the balancing performances basically from

https://www.youtube.com/watch?v=9XRI4BeXN78&

to

https://www.youtube.com/watch?v=33F2G5mYiHY

@gabrielebndn Just for your eventual information, if you were interested in some extensions of control algorithms to the Serial Elastic Actuators case, you may take a look at the paper

Momentum control of humanoid robots with series elastic actuators

We worked on that when porting the balancing controllers from iCub to Walkman - in this particular simulation, however, Walkman was still assumed to have rigid transmissions

[proyan]

How do you propose to replicate a new namespace in python?

[gabrielebndn]

@proyan a submodule?

[jcarpent]

@proyan You can get a look to this new Python helper https://github.com/stack-of-tasks/pinocchio/blob/bbe7d0d0c949100ec909d6eb30ee4ce8aa024773/bindings/python/utils/namespace.hpp#L23 to implement the namespace in Python.

[duburcqa]

I'm trying myself to modify aba to take into account the armature. It does not have to be generic and clean, I just want something working and easy to implement. @proyan mentioned that one must "call calc_aba in AbaForwardPass2", but it is not clear for me what to do exactly. Someone can give be more insight about it ? Thanks !

[proyan]

Hi Alexis, If you look at the algorithm for ABA, there is a forrward pass, a backward pass, and then a second forward pass. The simplest way to add armature, is to add it as a diagonal term in the M matrix (M+rI), and let everything else intact. However, ABA uses an iterative algorithm to compute M_inv inverse, and this means that if we want to compute (M+rI)_inv, we lose the iterations given by ABA.

During the backward pass, ABA updates the element D_i, which stores the transformed inertial parameters. Then the backward pass does the inverse of D_i. The proposal in this issue was to change the second forward pass, so that it instead uses the inverse of an updated Dnew_i (which contains the diagonal elements updated by the armature). I have it in my notes from February, but I'll have to dig it up :)

Best, Rohan

[jcarpent]

The second option is to use compute all terms + adding information on the diagonal of the mass matrix and then perform the Cholesky on the mass matrix. We will add soon (when? I don't know exactly but this is the in the pipe) the notion of actuator and transmission which will do that automatically. More manpower on this task would be helpful.

[jcarpent]

Just to mention that the solution proposed by @proyan does not take into account the Coriolis effects due to the spin of the motor and the motion of supporting joint!

[duburcqa]

@jcarpent Yes I know it does not take into account the Coriolis effects but I cannot afford to increase significantly the computational time. I'm more interested in fast computation rather than accurate modelling. I will have a look to the proposal of @proyan but I only have a vague idea of what to do.

[proyan]

@jcarpent, @duburcqa Just for info, after this issue was raised, I found that the update of ABA to take into account the rotor parameters (without taking into account the Coriolis effects, as Justin mentioned) has already been done in Featherstone's book at equation 9.28. It is simply: D_i = S_i^{T} * U_i + I_rot

This, as Featherstone and Justin note, is an easy but dirty way that only takes into account the larger inertial effects, and discounts the smaller coriolis effects.

[duburcqa]

@proyan Great, It seems easy ! I implemented it, however I did some experiment to check the results and it does not look right. If I set f_ext to 0 and compute

crba(model, data, q);
data.M.triangularView<Eigen::StrictlyLower>() =
            data.M.transpose().triangularView<Eigen::StrictlyLower>();

vectorN_t aZero = vectorN_t::Zero(model.nv);
auto b = rnea(model, data, q, v, aZero);

container::aligned_vector<Force> fextZero(model.joints.size(), Force::Zero());
vectorN_t a = aba(model, data, q, v, u, fextZero);
vectorN_t aWithMot = abaWithMot(model, data, q, v, u, fextZero); // Custom aba implementation taking into account the armature

then I do

(data.M * a + b - u).array().abs().maxCoeff() < 0

It evaluates to True. But if do

matrixN_t Imot(model.rotorInertia.asDiagonal());
((data.M + Imot) * aWithMot + b - u).array().abs().maxCoeff() < 0

It evaluates to False, although the maximum residual error is not very large.

[jcarpent]

@jcarpent Yes I know it does not take into account the Coriolis effects but I cannot afford to increase significantly the computational time. I'm more interested in fast computation rather than accurate modelling. I will have a look to the proposal of @proyan but I only have a vague idea of what to do.

It won't cost so much. Performs the bench and you will see ;)

[jcarpent]

@duburcqa Could you provide me a link to abaWithMotimplementation?

[duburcqa]

@jcarpent Yes no problem. I only replaced AbaBackwardStep. It is a sketch, I only implemented the armature for joints of type pinocchio::JointModelRevoluteTpl<Scalar, 0, 0>. For course the results I mentioned above were obtained for model.rotorInertia non-zero only for this specific type of joint. abaWithMot.txt

[duburcqa]

It won't cost so much. Performs the bench and you will see ;)

Ok I will benchmark it and let you know !

[jcarpent]

although the maximum residual error is not very large.

Could you provide the real value that you obtained?

[duburcqa]

I did the benchmark, aba is between 20% and 25% faster than using vectorN_t a = data.M.llt().solve(u - b); to compute the accelerations. The dimension of the configuration vector and velocity vectors are 37 and 32, respectively. That's quite a huge difference for me.

[duburcqa]

About the residual errors, I have this for a random state:

========= [(data.M + Imot) * aWithMot + b, u] =========
-0.00103341           0
   0.453027           0
  0.0119376           0
   0.762085           0
  0.0211216           0
 -0.0300496           0
    6.25927      6.2723
  -0.163269   -0.159311
   -2.40261    -2.40057
    5.24287     5.24403
 -0.0161583  -0.0160569
   0.973494    0.973494
2.13276e-05 2.13276e-05
    2.64313     2.63951
   -2.12795    -2.12903
   -1.62686    -1.62604
     3.5301     3.53054
    2.32349     2.32343
    2.00074     2.00074
2.31749e-05 2.31749e-05

[duburcqa]

@jcarpent @proyan OK I found the bug in my implementation. Now it's working perfectly. thank you for your help!

[jcarpent]

What was your error finally?

[jcarpent]

By the way, you should use the Sparse Cholesky decompostion of pinocchio, located in pinocchio::cholesky namespace. It will be faster than the Eigen::LLT

[duburcqa]

What was your error finally?

Line 61 in the attached file in my previous message. I forgot to replace data from calc_aba of revolute joints by jdata.derived() in AbaBackwardStep.

By the way, you should use the Sparse Cholesky decompostion of pinocchio, located in pinocchio::cholesky namespace. It will be faster than the Eigen::LLT

I didn't know about this function. I will benchmark it later if I have time just to get an idea of the speed up wrt Eigen::LLT.

[jcarpent]

You can run:

make timings-cholesky
./benchmark/timings-cholesky

[jcarpent]

Even if I'm not totally convinced by the fact to not update the Coriolis and centrifugal effects with the rotor inertia, we may investigate the fact of at least adding the term to the diagonal elements of the mass matrix or its inverse. It simple enough to do that before adding the notion of transmission + actuators.

@proyan @cmastalli @duburcqa Are you still interested in this feature?

[jcarpent]

@duburcqa By the way, I think in your code you have forgotten the influence of the gear ratio ;)

[cmastalli]

@jcarpent I am not convinced neither if we need to model this element in the optimal control problem. I would expect that a low-level controller handle it.

My point of view is you can avoid this task, and if it is needed, then we come back to you

[jcarpent]

@cmastalli This term should help the stabilisation of the robots dynamics inside DDP ;)

[duburcqa]

@duburcqa By the way, I think in your code you have forgotten the influence of the gear ratio ;)

Good point. But its ok in my case because in practice I know the rotor inertia including gear ratio.

@cmastalli This term should help the stabilisation of the robots dynamics inside DDP ;)

I agree with you. Plus personally I'm using Pinocchio as the core component of a physics engine (mainly dedicated to reinforcement learning), and I really need to be able to take in account the armature.

Even if I'm not totally convinced by the fact to not update the Coriolis and centrifugal effects with the rotor inertia, we may investigate the fact of at least adding the term to the diagonal elements of the mass matrix or its inverse. It simple enough to do that before adding the notion of transmission + actuators. @proyan @cmastalli @duburcqa Are you still interested in this feature?

Surely it would be useful, but I don't think it is worth the effort since it is only a quick-and-dirty fix simple enough to implement for anyone that really needs it. However I think it would be very relevant to implement a proper way to define a generic armature that also updates the Coriolis and centrifugal effects.

[cmastalli]

Yes, It helps but it couldn't be necessary an important factor (centroidal component) for most motion and robots. Note that currently, we have implemented the armature model inside inertia matrix as proposed in 1. However I wouldn't pursue point 1 in Pinocchio, only 2.

On the other hand, I have listened for some time the option to extend Pinocchio with mechanism model. I am not sure if this is good design, I see Pinocchio as rigid body algorithm library and I will keep in this level.

There are many actuation/mechanism, not only armature, e.g. four-bar link, hydraulic cylinders, etc. Do you want Pinocchio to support them? If you ask me this question I will reply "no" this is control design business. And if you believe yes, then it has to be generic solution.

In Crocoddyl, we have developed an actuation model which allows us to model a custom actuation model, physical or artificial (that depends on q,v and u), with its derivatives. Indeed we have developed recently a simple actuation model for quadrotor propellers. At the time being, I don't think we need this solution because creating action models for tailored mechanism doesn't seem to be a good idea.