JMinvJt_damping results in pinocchio quaternion assertion

[wxmerkt]

In GitLab by @wxmerkt on Oct 10, 2019, 14:57

I've added 1e-12 damping to every action model as follows (in quadruped.py):

dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(self.state, self.actuation, contactModel, costModel)
dmodel.JMinvJt_damping = 1e-12
model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep)

When solving, I now receive the following assertion crash:

python: /opt/openrobots/include/pinocchio/math/quaternion.hpp:89: void pinocchio::quaternion::firstOrderNormalize(const Eigen::QuaternionBase<Derived>&) [with D = Eigen::Map<Eigen::Quaternion<double>, 0>]: Assertion `static_leq::op(math::fabs(N2-1.), epsilon)' failed.

Without damping, the assertion in the DifferentialActionModelContactFwdDynamics triggers.

[wxmerkt]

In GitLab by @wxmerkt on Oct 10, 2019, 14:58

changed the description

[wxmerkt]

In GitLab by @cmastalli on Oct 10, 2019, 15:13

This assertion occurs when your quaternion inside the state is wrong, e.g. all values are infinity or NaN. Pinocchio detected in Debug mode compilation (i.e. Debug mode compilation of Crocoddyl).

You need to tell me more to understand why you have wrong values in the quaternions. This could happen with the divergence in the rollout of the dynamics.

[wxmerkt]

In GitLab by @cmastalli on Oct 10, 2019, 15:15

But we should expect our solver to leave the rollout loop before this happens

[wxmerkt]

In GitLab by @wxmerkt on Oct 10, 2019, 15:15

I compiled both Pinocchio and Crocoddyl in Debug mode to trace down another bug. This happens when you run the quadrupedal_gaits.py example, i.e., the one using FDDP. All the changes I made is to (a) set pseudoImpulse=False and (b) to add the 1e-12 damping as described above in quadruped.py.

[wxmerkt]

In GitLab by @cmastalli on Oct 10, 2019, 20:28

Thanks for reporting it.
Please note that the impulse dynamics are still in experimental phase. I didn't have time to check myself them. And we solve the impulse dynamics differently compared with the legacy Python code (i.e. through Cholesky decomposition)

As soon as possible, I'll investigate this issue if this is still unsolved.