From 24ff567cfd1ff6b222cdbee23b85568ceb35e655 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sat, 6 Aug 2022 10:49:25 +0200
Subject: [PATCH] Cosmetics
---
.../math/force_torque_inertial_delta_tools.h | 49 +++++++------------
1 file changed, 19 insertions(+), 30 deletions(-)
diff --git a/include/bodydynamics/math/force_torque_inertial_delta_tools.h b/include/bodydynamics/math/force_torque_inertial_delta_tools.h
index 470f300..ef226f7 100644
--- a/include/bodydynamics/math/force_torque_inertial_delta_tools.h
+++ b/include/bodydynamics/math/force_torque_inertial_delta_tools.h
@@ -1205,41 +1205,30 @@ void forces2acc(const MatrixBase<D1>& _force,
const Matrix3t& I_inv = ((typename D5::Scalar)(1.0) / _inertia.array()).matrix().asDiagonal();
// variables
- const Vector3t& angacc = I_inv * (_torque - _angvel.cross(I * _angvel)); // (36)
-
+ const Vector3t& angacc = I_inv * (_torque - _angvel.cross(I * _angvel)); // (36)
const Matrix3t& angvel_x = skew(_angvel);
const Matrix3t& com_x = skew(_com);
const Matrix3t& angacc_x = skew(angacc);
_acc = _force / _mass - angacc.cross(_com) - _angvel.cross(_angvel.cross(_com)); // (35)
- // Jacobians
- const Matrix3t& J_Iw_i = _angvel.asDiagonal(); // (58)
-
- const Matrix3t& tmp = (_angvel.cross(I * _angvel) - _torque).asDiagonal();
-
- const Matrix3t& J_angacc_inertia = I_inv * (I_inv * tmp - angvel_x * J_Iw_i); // (57)
-
- const Matrix3t& J_acc_angacc = com_x; // (56)
-
- const Matrix3t& J_angacc_torque = I_inv; // (53)
-
- const Matrix3t& J_angvelcom_angvel = -com_x; // (39)
-
- const Matrix3t& J_angacc_angvel = I_inv * (skew(I * _angvel) - angvel_x * I); // (38)
-
- _J_acc_force = Matrix<S, 3, 3>::Identity() / _mass; // (59)
-
- _J_acc_torque = J_acc_angacc * J_angacc_torque; // (?)
-
+ // Jacobian blocks
+ const Matrix3t& tmp = (_angvel.cross(I * _angvel) - _torque).asDiagonal();
+ const Matrix3t& J_Iw_i = _angvel.asDiagonal(); // (58)
+ const Matrix3t& J_angacc_inertia = I_inv * (I_inv * tmp - angvel_x * J_Iw_i); // (57)
+ const Matrix3t& J_acc_angacc = com_x; // (56)
+ const Matrix3t& J_angacc_torque = I_inv; // (53)
+ const Matrix3t& J_angvelcom_angvel = -com_x; // (39)
+ const Matrix3t& J_angacc_angvel = I_inv * (skew(I * _angvel) - angvel_x * I); // (38)
+
+ // Output Jacobians
+ _J_acc_force = Matrix<S, 3, 3>::Identity() / _mass; // (59)
+ _J_acc_torque = J_acc_angacc * J_angacc_torque; // (?)
_J_acc_angvel =
J_acc_angacc * J_angacc_angvel - angvel_x * J_angvelcom_angvel + skew(_angvel.cross(_com)); // (37)
-
- _J_acc_com = -angacc_x - angvel_x * angvel_x; // (51)
-
- _J_acc_inertia = J_acc_angacc * J_angacc_inertia; // (55)
-
- _J_acc_mass = -_force / (_mass * _mass); // (50)
+ _J_acc_com = -angacc_x - angvel_x * angvel_x; // (51)
+ _J_acc_inertia = J_acc_angacc * J_angacc_inertia; // (55)
+ _J_acc_mass = -_force / (_mass * _mass); // (50)
}
template <typename D1,
@@ -1493,9 +1482,9 @@ void data2delta(const MatrixBase<D1>& _data,
tangent2delta(tangent, _model, _dt, _delta, J_delta_tangent, _J_delta_model);
- _J_delta_data = J_delta_tangent * J_tangent_data;
- _J_delta_bias = J_delta_tangent * J_tangent_bias;
- _J_delta_model += J_delta_tangent * J_tangent_model;
+ _J_delta_data = J_delta_tangent * J_tangent_data;
+ _J_delta_bias = J_delta_tangent * J_tangent_bias;
+ _J_delta_model = _J_delta_model + J_delta_tangent * J_tangent_model;
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
--
GitLab