diff --git a/include/bodydynamics/math/force_torque_inertial_delta_tools.h b/include/bodydynamics/math/force_torque_inertial_delta_tools.h index 42081eba2c3aa4e5e1bc68cf349fa6a57de6cc20..e5a87d1bf0c6898ecbb5b5a10b3a5a4b4ad709f1 100644 --- a/include/bodydynamics/math/force_torque_inertial_delta_tools.h +++ b/include/bodydynamics/math/force_torque_inertial_delta_tools.h @@ -1219,10 +1219,10 @@ void forces2acc(const MatrixBase<D1>& _force, 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_force = Matrix<S, 3, 3>::Identity() / _mass; // (59) + _J_acc_torque = J_acc_angacc * J_angacc_torque; // (?) _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)