Memory allocation in impulseDynamics

[cmastalli]

This code detects memory allocation on the impulseDynamics. It's used the current signature in v2.7.1, I didn't try the latest changes, however, I believe the problem could be still there:

#define EIGEN_RUNTIME_NO_MALLOC

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include <pinocchio/algorithm/contact-dynamics.hpp>
#include "pinocchio/parsers/sample-models.hpp"

using namespace Eigen;
using namespace pinocchio;

using std::cout;
using std::endl;

int main() {
  Model model;
  buildModels::humanoidRandom(model);

  Data data(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);

  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));

  pinocchio::computeJointJacobians(model, data, q);

  const std::string RF = "rleg6_joint";
  const std::string LF = "lleg6_joint";

  Data::Matrix6x J_RF (6, model.nv);
  J_RF.setZero();
  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
  Data::Matrix6x J_LF (6, model.nv);
  J_LF.setZero();
  getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);

  Eigen::MatrixXd J (12, model.nv);
  J.setZero();
  J.topRows<6> () = J_RF;
  J.bottomRows<6> () = J_LF;

  Eigen::internal::set_is_malloc_allowed(false);
  pinocchio::impulseDynamics(model, data, q, v, J, 0., true);
  Eigen::internal::set_is_malloc_allowed(true);
}

Remember that you need to compile in debug mode. If you name this file as test_impulse_derivatives.cpp, then this is a suggested compilation command:

g++ -I $(pkg-config --cflags pinocchio) -g test_impulse_dynamics.cpp -o test_impulse_dynamics

[jcarpent]

This is expected. As J and Gamma do not have fixed sizes.

[jcarpent]

The next release with new contact solver won't have this trouble.

[jcarpent]

I will close this issue. Feel free to reopen it if needed.

[cmastalli]

Ok, thanks for the support.

[cmastalli]

Could you elaborate the explanation about Jacobian and gamma? Cause I don't see any difference between the forward dynamics, where Pinocchio doesn't allocate memory.

For Crocoddyl team, it is quite important this issue. It is blocking point for multi-threading deployment (see. https://gepgitlab.laas.fr/loco-3d/crocoddyl/issues/286).

I know you're very busy, however, I would be extremely grateful if this issue can be solved in very short-term since MEMMO deadlines. Please do not close the issue before the solution will be propagated to Pinocchio

[jcarpent]

It is just allocating memory at the first call. Not during the second normally. Can you check it?

[cmastalli]

Yes, I confirm that it only allocates memory during the first call of impulseDynamics

[cmastalli]

@jcarpent do you know when it could be available the new impulse function?

[gabrielebndn]

There is no way to avoid this. Matrix J has a variable number of rows, it is of type Matrix<Scalar,Dynamic,Dynamic>. This implies that the size of all related quantities such as JMinvJt cannot be known a priori and is also of type Matrix<Scalar,Dynamic,Dynamic>. Therefore

1. When Data is created, these matrices have size 0
2. When the method is first called, the memory is allocated through the assignment operator
3. When the method is called a second time, the memory is already allocated from the first call, therefore it is not reallocated

This is the same for forwardDynamics

You will only incur into a problem if you repeatedly call impulseDynamics and/or forwardDynamics with J being of different size. If you find yourself in this situation, and you really want to avoid the allocations, my suggestion is to use different Data objects for the different cases.

[jcarpent]

Thanks @gabrielebndn for these explanations.

Reading again @cmastalli's comment:

Cause I don't see any difference between the forward dynamics, where Pinocchio doesn't allocate memory. it is worth mentioning that both impulseDynamics and forwardDynamics do memory allocation at the first call. You have not seen one this allocation because you were certainly using the same Data. To avoid memory allocation, and contrary to what @gabrielebndn mentioned, there are two ways:

• fix a maximum size for the contact matrices (J, JMinvJt, etc) using Eigen features but it will be limiting the total number of possible contacts
• add a function initForwardDyanmics which will perform the allocation before any use of forwardDynamics. This is the strategy I've chosen to implement in the new code for contact dynamics solvers.

@cmastalli Be patient, it will come soon. Before the end of the month, I hope so. But for sure, as the allocation is done only once in your program, it should not be limiting.

[cmastalli]

@jcarpent thanks a lot for the effort!

I understand that it shouldn't limit myself, however I have wrong behavior when I use the impulses in a multithreading app. In this case I am using the new API of the impulse dynamics.

[cmastalli]

@gabrielebndn thanks for the explanation.

[jcarpent]

@cmastalli Can you provide a link for the multi-threading in your code? I will copy past it to make sure evrything goes fine ;)

[cmastalli]

@cmastalli Can you provide a link for the multi-threading in your code? I will copy past it to make sure evrything goes fine ;)

I developed the impulse dynamics here: https://github.com/loco-3d/crocoddyl/blob/devel/src/multibody/actions/impulse-fwddyn.cpp.

Is this what you need?

[nmansard]

I believe you want to point to the memory alloc here: https://github.com/loco-3d/crocoddyl/blob/devel/src/multibody/actions/impulse-fwddyn.cpp#L68 Yet @jcarpent was mentioning multi-threading.

[nmansard]

Sorry, I close my answered too early. Multi-threading is done in the shooting calc-diff: https://github.com/loco-3d/crocoddyl/blob/devel/src/core/optctrl/shooting.cpp#L65

[cmastalli]

I am coming again to this topic since it's preventing the deployment of multi-threading applications in Crocoddyl.

I would like to have a better idea of why we encounter problems only for the impulse dynamics. Note that this doesn't happen in the case of forwardDynamics. Any idea?

[jcarpent]

impulse and forward dynamics are based on the same routines. There is no reason. Again, please provide self content examples to highlight your issues.

[cmastalli]

@jcarpent thank you for your response. I checked again my implementation, and the issue could be in my side. Let me copy the part of my code for computing the impulse derivatives:

  pinocchio_.gravity.setZero();
  pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, d->vnone, d->pinocchio.dq_after - v,
                                    d->multibody.impulses->fext);
  pinocchio_.gravity = gravity_;
  pinocchio::getKKTContactDynamicMatrixInverse(pinocchio_, d->pinocchio, d->multibody.impulses->Jc.topRows(ni),
                                               d->Kinv);

As you can see, I change the gravity which might create problems in a multithreading app. Note that my goal in this piece of code is to compute dtau_dq.

So my question to you is "how can I cancel dgrav_dq without modifying the gravity vector?

Please let me know if you have a more efficient solution than removing dgrav_dq.

---

PD: I could create another Pinocchio model to solve this issue, but it is not a nice solution.

[jcarpent]

there is this feature already implemented in Pinocchio and called computeGeneralizedGravityDerivatives.

[cmastalli]

there is this feature already implemented in Pinocchio and called computeGeneralizedGravityDerivatives.

Awesome, thank you so much for a prompt response. I will try this implementation, and keep you updated.

[cmastalli]

@jcarpent I implemented this solution. I confirm you that we don't have any issue in Crocoddyl.

For more details see: https://github.com/loco-3d/crocoddyl/pull/765.

This issue is still important but not relevant for Crocoddyl. Feel free to keep open or close it!

[jcarpent]

Done in pinocchio-3x which should be released soon.