diff --git a/include/bodydynamics/math/force_torque_inertial_delta_tools.h b/include/bodydynamics/math/force_torque_inertial_delta_tools.h
index ef226f75c8dd863de548916a5255912de03d48ea..05041c069476b5d4857b3afcb8bf291befd4ca5a 100644
--- a/include/bodydynamics/math/force_torque_inertial_delta_tools.h
+++ b/include/bodydynamics/math/force_torque_inertial_delta_tools.h
@@ -1487,198 +1487,6 @@ void data2delta(const MatrixBase<D1>& _data,
_J_delta_model = _J_delta_model + J_delta_tangent * J_tangent_model;
}
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-// template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8,
-// typename D9, typename D10, typename D11, typename D12, typename D19, typename D14> inline void diff(const
-// MatrixBase<D1>& dpi1, const MatrixBase<D2>& dvi1, const MatrixBase<D3>& dL1, const QuaternionBase<D4>& dq1,
-// const MatrixBase<D5>& dpi2, const MatrixBase<D6>& dvi2, const MatrixBase<D7>& dL2, const
-// QuaternionBase<D8>& dq2, MatrixBase<D9>& diff_pi, MatrixBase<D10>& diff_vi, MatrixBase<D11>&
-// diff_L, MatrixBase<D12>& diff_o, MatrixBase<D19>& J_do_dq1, MatrixBase<D14>& J_do_dq2)
-//{
-// diff(dpi1, dvi1, dL1, dq1, dpi2, dvi2, dL2, dq2, diff_pi, diff_vi, diff_L, diff_o);
-//
-// J_do_dq1 = - jac_SO3_left_inv(diff_o);
-// J_do_dq2 = jac_SO3_right_inv(diff_o);
-// }
-
-// template<typename D1, typename D2, typename D3, typename D4, typename D5>
-// inline void diff(const MatrixBase<D1>& d1,
-// const MatrixBase<D2>& d2,
-// MatrixBase<D3>& diff_d1_d2,
-// MatrixBase<D4>& J_diff_d1,
-// MatrixBase<D5>& J_diff_d2)
-//{
-// Map<const Matrix<typename D1::Scalar, 3, 1> > dpi1 ( & d1(0) );
-// Map<const Matrix<typename D1::Scalar, 3, 1> > dvi1 ( & d1(3) );
-// Map<const Matrix<typename D1::Scalar, 3, 1> > dL1 ( & d1(6) );
-// Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(9) );
-// Map<const Matrix<typename D2::Scalar, 3, 1> > dpi2 ( & d2(0) );
-// Map<const Matrix<typename D2::Scalar, 3, 1> > dvi2 ( & d2(3) );
-// Map<const Matrix<typename D2::Scalar, 3, 1> > dL2 ( & d2(6) );
-// Map<const Quaternion<typename D1::Scalar> > dq2 ( & d2(9) );
-// Map<Matrix<typename D3::Scalar, 3, 1> > diff_pi ( & diff_d1_d2(0) );
-// Map<Matrix<typename D3::Scalar, 3, 1> > diff_vi ( & diff_d1_d2(3) );
-// Map<Matrix<typename D3::Scalar, 3, 1> > diff_L ( & diff_d1_d2(6) );
-// Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & diff_d1_d2(9) );
-//
-// Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
-//
-// diff(dpi1, dvi1, dL1, dq1, dpi2, dvi2, dL2, dq2, diff_pi, diff_vi, diff_L, diff_o, J_do_dq1, J_do_dq2);
-//
-// /* d = diff(d1, d2) is
-// * dpi = dpi2 - dpi1
-// * dvi = dvi2 - dvi1
-// * dL = dL2 - dL1
-// * do = Log(dq1.conj * dq2)
-// *
-// * With trivial Jacobians for dpi and dvi, and:
-// * J_do_dq1 = - J_l_inv(theta)
-// * J_do_dq2 = J_r_inv(theta)
-// */
-//
-// J_diff_d1 = - Matrix<typename D4::Scalar, 12, 12>::Identity(); // d(pi2 - pi1) / d(pi1) = - Identity, for
-// instance J_diff_d1.template block<3,3>(9,9) = J_do_dq1; // d(R1.tr*R2) / d(R1) = -
-// J_l_inv(theta) J_diff_d2.setIdentity(); // d(pi2 - pi1) / d(pi2) =
-// Identity, for instance J_diff_d2.template block<3,3>(9,9) = J_do_dq2; // d(R1.tr*R2) / d(R2) =
-// J_r_inv(theta)
-// }
-
-// template<typename D1, typename D2>
-// inline void body2delta(const MatrixBase<D1>& body,
-// const typename D1::Scalar& dt,
-// const typename D1::Scalar& mass,
-// int nbc,
-// int dimc,
-// MatrixBase<D2>& delta)
-//{
-// typedef typename D1::Scalar T;
-//
-// delta.setZero();
-// Map< Matrix<T, 3, 1>> dpi ( & delta(0) );
-// Map< Matrix<T, 3, 1>> dvi( & delta(3) );
-// Map< Matrix<T, 3, 1>> dL( & delta(6) );
-// Map< Quaternion<T>> dq ( & delta(9) );
-//
-// Vector3d pbc = body.segment(nbc*dimc, 3);
-// Vector3d wb = body.segment(nbc*dimc + 3, 3);
-//
-// // general computation for any 3D or 6D contacts
-// for (int i=0; i<nbc; i++){
-// Vector3d pbli = body.segment(nbc*dimc + 6 + i*3, 3);
-// Vector4d qbli_v = body.segment(nbc*dimc + 6 + nbc*3 + i*4, 4);
-// Quaterniond qbli(qbli_v);
-// Vector3d fi = body.segment(i*3, 3);
-//
-// dpi += qbli*fi;
-// dvi += qbli*fi;
-// dL += (pbli - pbc).cross(qbli * fi);
-// if (dimc == 6){
-// Vector3d taui = body.segment(3*nbc + i*3, 3);
-// dL += qbli*taui;
-// }
-// }
-//
-// // formulas for 2 6D contacts:
-// // dpi = 0.5 * (qbl1*f1 + qbl2*f2) * dt * dt / mass;
-// // dvi = (qbl1*f1 + qbl2*f2) * dt / mass;
-// // dL = (qbl1*tau1 + (pbl1-pbc).cross(qbl1*f1)
-// // + qbl2*tau2 + (pbl2-pbc).cross(qbl2*f2)) * dt;
-//
-// dpi *= (0.5*dt*dt/mass);
-// dvi *= (dt/mass);
-// dL *= dt;
-// dq = exp_q(wb * dt);
-// }
-//
-// template<typename D1>
-// inline Matrix<typename D1::Scalar, 19, 1> body2delta(const MatrixBase<D1>& body,
-// const typename D1::Scalar& dt,
-// const typename D1::Scalar& mass,
-// int nbc,
-// int dimc)
-//{
-// typedef typename D1::Scalar T;
-// Matrix<T, 19, 1> delta;
-// body2delta(body, dt, mass, nbc, dimc, delta);
-//
-// return delta;
-// }
-//
-// template<typename D1, typename D2, typename D3>
-// inline void body2delta(const MatrixBase<D1>& body,
-// const typename D1::Scalar& dt,
-// const typename D1::Scalar& mass,
-// int nbc,
-// int dimc,
-// MatrixBase<D2>& delta,
-// MatrixBase<D3>& jac_delta_body)
-//{
-// Vector3d pbc = body.segment(nbc*dimc, 3);
-// Vector3d wb = body.segment(nbc*dimc + 3, 3);
-//
-// jac_delta_body.setZero();
-// for (int i=0; i<nbc; i++){
-// Vector3d pbli = body.segment(nbc*dimc + 6 + i*3, 3);
-// Vector4d qbli_v = body.segment(nbc*dimc + 6 + nbc*3 + i*4, 4);
-// Quaterniond qbli(qbli_v);
-// Matrix3d bRli = q2R(qbli);
-// Vector3d fi = body.segment(i*3, 3);
-//
-// jac_delta_body.template block<3,3>(0,i*3) = 0.5 * bRli * dt * dt / mass;
-// jac_delta_body.template block<3,3>(3,i*3) = bRli * dt / mass;
-// jac_delta_body.template block<3,3>(6,i*3) = skew(pbli - pbc) * bRli * dt;
-// if (dimc == 6){
-// jac_delta_body.template block<3,3>(6,nbc*3 + i*3) = bRli * dt;
-// }
-// jac_delta_body.template block<3,3>(6,nbc*dimc) += skew(bRli*fi) * dt;
-// }
-// jac_delta_body.template block<3,3>(9,nbc*dimc+3) = dt * jac_SO3_right(wb * dt);
-//
-// // formulas for 2 6D contacts:
-// // jac_delta_body.template block<3,3>(0,0) = 0.5 * bRl1 * dt * dt / mass;
-// // jac_delta_body.template block<3,3>(0,3) = 0.5 * bRl2 * dt * dt / mass;
-// // jac_delta_body.template block<3,3>(3,0) = bRl1 * dt / mass;
-// // jac_delta_body.template block<3,3>(3,3) = bRl2 * dt / mass;
-// // jac_delta_body.template block<3,3>(6,0) = skew(pbl1 - pbc) * bRl1 * dt;
-// // jac_delta_body.template block<3,3>(6,3) = skew(pbl2 - pbc) * bRl2 * dt;
-// // jac_delta_body.template block<3,3>(6,6) = bRl1 * dt;
-// // jac_delta_body.template block<3,3>(6,9) = bRl2 * dt;
-// // jac_delta_body.template block<3,3>(6,12) = (skew(bRl1*f1) + skew(bRl2*f2)) * dt;
-// // jac_delta_body.template block<3,3>(9,15) = dt * jac_SO3_right(wb * dt);
-//
-// // TODO: kinematics uncertainties (bRl1, bRl2, bpl1, bpl2) are not implemented currently
-//
-// body2delta(body, dt, mass, nbc, dimc, delta);
-// }
-//
-// template<typename D1, typename D2, typename D3>
-// inline void debiasData(const MatrixBase<D1>& _data, const MatrixBase<D2>& _bias, int nbc, int dimc, MatrixBase<D3>&
-// _body)
-//{
-// MatrixSizeCheck<6 , 1>::check(_bias);
-// _body = _data;
-// _body.template block<6,1>(nbc*dimc,0) = _data.template block<6,1>(nbc*dimc,0) - _bias;
-// }
-//
-// template<typename D1, typename D2, typename D3, typename D4>
-// inline void debiasData(const MatrixBase<D1>& _data, const MatrixBase<D2>& _bias, int nbc, int dimc, MatrixBase<D3>&
-// _body, MatrixBase<D4>& _J_body_bias)
-//{
-// debiasData(_data, _bias, nbc, dimc, _body);
-// // MatrixSizeCheck<30, 6>::check(_J_body_bias); // 30 because the 2 last body quantities are quaternions
-// _J_body_bias.setZero();
-// _J_body_bias.template block<3,3>(nbc*dimc, 0) = -Matrix<typename D1::Scalar, 3, 3>::Identity();
-// _J_body_bias.template block<3,3>(nbc*dimc+3, 3) = -Matrix<typename D1::Scalar, 3, 3>::Identity();
-// }
} // namespace fti
} // namespace bodydynamics