From b17dec32308e0f66af4c4cef960b7d1e1e40c302 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sat, 6 Aug 2022 21:52:58 +0200
Subject: [PATCH] format
---
.../math/force_torque_inertial_delta_tools.h | 18 ++++++++++++------
1 file changed, 12 insertions(+), 6 deletions(-)
diff --git a/include/bodydynamics/math/force_torque_inertial_delta_tools.h b/include/bodydynamics/math/force_torque_inertial_delta_tools.h
index 05041c0..430be43 100644
--- a/include/bodydynamics/math/force_torque_inertial_delta_tools.h
+++ b/include/bodydynamics/math/force_torque_inertial_delta_tools.h
@@ -1388,7 +1388,9 @@ void tangent2delta(const MatrixBase<D1>& _tangent,
Matrix<typename D1::Scalar, 3, 3> J_accd_inertia;
Matrix<typename D1::Scalar, 3, 1> J_accd_mass;
- forces2acc(force, torque, angvel, com, inertia, mass, acc_d, J_accd_force, J_accd_torque, J_accd_angvel,
+ forces2acc(force, torque, angvel, com, inertia, mass,
+ acc_d,
+ J_accd_force, J_accd_torque, J_accd_angvel,
J_accd_com, J_accd_inertia, J_accd_mass);
T dt2 = _dt * _dt;
@@ -1401,13 +1403,17 @@ void tangent2delta(const MatrixBase<D1>& _tangent,
_delta_q = exp_q(angvel * _dt);
_delta_L = torque * _dt;
+ // temporaries
+ const Matrix<typename D1::Scalar, 3, 3>& I_dt = Matrix<typename D1::Scalar, 3, 3>::Identity() * _dt;
+ const Matrix<typename D1::Scalar, 3, 3>& I_dt2_2 = 0.5 * Matrix<typename D1::Scalar, 3, 3>::Identity() * dt2;
+
// jacobians
- Matrix<typename D1::Scalar, 3, 3> J_dpi_acci = 0.5 * Matrix<typename D1::Scalar, 3, 3>::Identity() * dt2;
- Matrix<typename D1::Scalar, 3, 3> J_dvi_acci = Matrix<typename D1::Scalar, 3, 3>::Identity() * _dt;
- const auto& J_dpd_accd = J_dpi_acci;
- const auto& J_dvd_accd = J_dvi_acci;
+ Matrix<typename D1::Scalar, 3, 3> J_dpi_acci = I_dt2_2;
+ Matrix<typename D1::Scalar, 3, 3> J_dvi_acci = I_dt;
+ const auto& J_dpd_accd = I_dt2_2;
+ const auto& J_dvd_accd = I_dt;
Matrix<typename D1::Scalar, 3, 3> J_dq_angvel = jac_SO3_right(angvel * _dt) * _dt;
- Matrix<typename D1::Scalar, 3, 3> J_dl_torque = Matrix<typename D1::Scalar, 3, 3>::Identity() * _dt;
+ Matrix<typename D1::Scalar, 3, 3> J_dl_torque = I_dt;
_J_delta_tangent.setZero();
--
GitLab