diff --git a/include/bodydynamics/math/force_torque_inertial_delta_tools.h b/include/bodydynamics/math/force_torque_inertial_delta_tools.h index 4f771ceb0d39ca5f1b299b4d887bba9603775091..2d3f813e5ed50aa06e669add40968caa63abf95a 100644 --- a/include/bodydynamics/math/force_torque_inertial_delta_tools.h +++ b/include/bodydynamics/math/force_torque_inertial_delta_tools.h @@ -728,8 +728,22 @@ inline Matrix<typename D1::Scalar, 19, 1> betweenStatesGlobalAcceleration(const inline void betweenStatesGlobalAcceleration(const VectorComposite& x1, const VectorComposite& x2, const Vector3d& g, - double dt, VectorComposite& ret) + double dt, + VectorComposite& ret) { + if (not ret.count('P')) + ret['P'] = Vector3d(); + if (not ret.count('O')) + ret['O'] = Vector4d(); + if (not ret.count('V')) + ret['V'] = Vector3d(); + if (not ret.count('p')) + ret['p'] = Vector3d(); + if (not ret.count('v')) + ret['v'] = Vector3d(); + if (not ret.count('L')) + ret['L'] = Vector3d(); + Map<const Quaterniond> q1(x1.at('O').data()); Map<const Quaterniond> q2(x2.at('O').data()); Map<Quaterniond> dq(ret.at('O').data());