From 4b9b25dc03784f3b9bacd772ff0a69c68c38b636 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Mon, 24 Oct 2022 18:42:30 +0200
Subject: [PATCH] fix betweenStatesGlobalAcceleration vec composite
---
.../math/force_torque_inertial_delta_tools.h | 16 +++++++++++++++-
1 file changed, 15 insertions(+), 1 deletion(-)
diff --git a/include/bodydynamics/math/force_torque_inertial_delta_tools.h b/include/bodydynamics/math/force_torque_inertial_delta_tools.h
index 4f771ce..2d3f813 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());
--
GitLab