diff --git a/include/bodydynamics/math/force_torque_inertial_delta_tools.h b/include/bodydynamics/math/force_torque_inertial_delta_tools.h index 430be43e5336599065e7f08fdf28a176c9bb5890..42081eba2c3aa4e5e1bc68cf349fa6a57de6cc20 100644 --- a/include/bodydynamics/math/force_torque_inertial_delta_tools.h +++ b/include/bodydynamics/math/force_torque_inertial_delta_tools.h @@ -402,18 +402,15 @@ inline void compose(const MatrixBase<D1>& d1, Matrix<typename D1::Scalar, 3, 3> dR1 = dq1.matrix(); Matrix<typename D2::Scalar, 3, 3> dR2 = dq2.matrix(); - // JACOBIANS OF COMPOSE() JAC WRT D1 JAC WRT - // D2 - // dpi1 dvi1 dpd1 dvd1 dL1 dR1 dpi2 dvi2 dpd2 dvd2 dL2 - // dR2 - // ------------------------------------------ -------------------------- - // sum_pi = dpi1 + dvi1*dt + dq1*dpi2; // I, I*dt, 0, 0 , 0, -dR1 * dpi2_x // dR1, 0, 0, 0, 0, - // 0 sum_vi = dvi1 + dq1*dvi2; // 0, I , 0, 0 , 0, -dR1 * dvi2_x // 0, dR1, 0, 0, 0, - // 0 sum_pd = dpd1 + dvd1*dt + dq1*dpd2; // 0, 0, I, I*dt, 0, -dR1 * dpd2_x // 0, 0, dR1, 0, 0, - // 0 sum_vd = dvd1 + dq1*dvd2; // 0, 0 , 0, I , 0, -dR1 * dvd2_x // 0, 0, 0, dR1, 0, - // 0 sum_L = dL1 + dq1*dL2; // 0, 0 , 0, 0 , I, -dR1 * dL2_x // 0, 0, 0, 0, - // dR1, 0 sum_q = dq1*dq2; // 0, 0 , 0, 0 , 0, dR2.tr // 0, 0, 0, 0, - // 0, I + // JACOBIANS OF COMPOSE() JAC WRT D1 JAC WRT D2 + // dpi1 dvi1 dpd1 dvd1 dL1 dR1 dpi2 dvi2 dpd2 dvd2 dL2 dR2 + // ------------------------------------------ -------------------------- + // sum_pi = dpi1 + dvi1*dt + dq1*dpi2; // I, I*dt, 0, 0 , 0, -dR1 * dpi2_x // dR1, 0, 0, 0, 0, 0 + // sum_vi = dvi1 + dq1*dvi2; // 0, I , 0, 0 , 0, -dR1 * dvi2_x // 0, dR1, 0, 0, 0, 0 + // sum_pd = dpd1 + dvd1*dt + dq1*dpd2; // 0, 0 , I, I*dt, 0, -dR1 * dpd2_x // 0, 0, dR1, 0, 0, 0 + // sum_vd = dvd1 + dq1*dvd2; // 0, 0 , 0, I , 0, -dR1 * dvd2_x // 0, 0, 0, dR1, 0, 0 + // sum_L = dL1 + dq1*dL2; // 0, 0 , 0, 0 , I, -dR1 * dL2_x // 0, 0, 0, 0, dR1, 0 + // sum_q = dq1*dq2; // 0, 0 , 0, 0 , 0, dR2.tr // 0, 0, 0, 0, 0, I // // Jac wrt first delta J_sum_d1.setIdentity();