diff --git a/include/bodydynamics/math/force_torque_inertial_delta_tools.h b/include/bodydynamics/math/force_torque_inertial_delta_tools.h index 05041c069476b5d4857b3afcb8bf291befd4ca5a..430be43e5336599065e7f08fdf28a176c9bb5890 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();