diff --git a/include/bodydynamics/math/force_torque_inertial_delta_tools.h b/include/bodydynamics/math/force_torque_inertial_delta_tools.h index e5a87d1bf0c6898ecbb5b5a10b3a5a4b4ad709f1..4f771ceb0d39ca5f1b299b4d887bba9603775091 100644 --- a/include/bodydynamics/math/force_torque_inertial_delta_tools.h +++ b/include/bodydynamics/math/force_torque_inertial_delta_tools.h @@ -644,22 +644,24 @@ template <typename D1, typename D16, typename D17, typename D18, + typename D19, class T> -inline void betweenStates(const MatrixBase<D1>& p1, - const MatrixBase<D2>& v1, - const MatrixBase<D5>& L1, - const QuaternionBase<D6>& q1, - const MatrixBase<D7>& p2, - const MatrixBase<D8>& v2, - const MatrixBase<D11>& L2, - const QuaternionBase<D12>& q2, - const T dt, - MatrixBase<D13>& dpi, - MatrixBase<D14>& dvi, - MatrixBase<D15>& dpd, - MatrixBase<D16>& dvd, - MatrixBase<D17>& dL, - QuaternionBase<D18>& dq) +inline void betweenStatesGlobalAcceleration(const MatrixBase<D1>& p1, + const MatrixBase<D2>& v1, + const MatrixBase<D5>& L1, + const QuaternionBase<D6>& q1, + const MatrixBase<D7>& p2, + const MatrixBase<D8>& v2, + const MatrixBase<D11>& L2, + const QuaternionBase<D12>& q2, + const MatrixBase<D13>& g, + const T dt, + MatrixBase<D14>& dpi, + MatrixBase<D15>& dvi, + MatrixBase<D16>& dpd, + MatrixBase<D17>& dvd, + MatrixBase<D18>& dL, + QuaternionBase<D19>& dq) { MatrixSizeCheck<3, 1>::check(p1); MatrixSizeCheck<3, 1>::check(v1); @@ -667,25 +669,31 @@ inline void betweenStates(const MatrixBase<D1>& p1, MatrixSizeCheck<3, 1>::check(p2); MatrixSizeCheck<3, 1>::check(v2); MatrixSizeCheck<3, 1>::check(L2); + MatrixSizeCheck<3, 1>::check(g); MatrixSizeCheck<3, 1>::check(dpi); MatrixSizeCheck<3, 1>::check(dvi); MatrixSizeCheck<3, 1>::check(dpd); MatrixSizeCheck<3, 1>::check(dvd); MatrixSizeCheck<3, 1>::check(dL); - dpi = q1.conjugate() * (p2 - p1 - v1 * dt - 0.5 * gravity() * dt * dt); - dvi = q1.conjugate() * (v2 - v1 - gravity() * dt); + dpi = q1.conjugate() * (p2 - p1 - v1 * dt - 0.5 * g * dt * dt); + dvi = q1.conjugate() * (v2 - v1 - g * dt); dpd = dpi; dvd = dvi; dL = q1.conjugate() * (L2 - L1); dq = q1.conjugate() * q2; } -template <typename D1, typename D2, typename D3, class T> -inline void betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T dt, MatrixBase<D3>& x2_minus_x1) +template <typename D1, typename D2, typename D3, typename D4, class T> +inline void betweenStatesGlobalAcceleration(const MatrixBase<D1>& x1, + const MatrixBase<D2>& x2, + const MatrixBase<D3>& g, + T dt, + MatrixBase<D4>& x2_minus_x1) { MatrixSizeCheck<13, 1>::check(x1); MatrixSizeCheck<13, 1>::check(x2); + MatrixSizeCheck<3, 1>::check(g); MatrixSizeCheck<19, 1>::check(x2_minus_x1); Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&x1(0)); @@ -703,37 +711,120 @@ inline void betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T Map<Matrix<typename D3::Scalar, 3, 1>> dL(&x2_minus_x1(12)); Map<Quaternion<typename D3::Scalar>> dq(&x2_minus_x1(15)); - betweenStates(p1, v1, L1, q1, p2, v2, L2, q2, dt, dpi, dvi, dpd, dvd, dL, dq); + betweenStatesGlobalAcceleration(p1, v1, L1, q1, p2, v2, L2, q2, g, dt, dpi, dvi, dpd, dvd, dL, dq); } -template <typename D1, typename D2, class T> -inline Matrix<typename D1::Scalar, 19, 1> betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T dt) +template <typename D1, typename D2, typename D3, class T> +inline Matrix<typename D1::Scalar, 19, 1> betweenStatesGlobalAcceleration(const MatrixBase<D1>& x1, + const MatrixBase<D2>& x2, + const MatrixBase<D3>& g, + T dt) { Matrix<typename D1::Scalar, 19, 1> ret; - betweenStates(x1, x2, dt, ret); + betweenStatesGlobalAcceleration(x1, x2, g, dt, ret); return ret; } -inline void betweenStates(const VectorComposite& x1, const VectorComposite& x2, double dt, VectorComposite& ret) +inline void betweenStatesGlobalAcceleration(const VectorComposite& x1, + const VectorComposite& x2, + const Vector3d& g, + double dt, VectorComposite& ret) { Map<const Quaterniond> q1(x1.at('O').data()); Map<const Quaterniond> q2(x2.at('O').data()); - - ret['P'] = q1.conjugate() * (x2.at('P') - x1.at('P') - x1.at('V') * dt - 0.5 * gravity() * dt * dt); - ret['V'] = q1.conjugate() * (x2.at('V') - x1.at('V') - gravity() * dt); - ret['p'] = ret.at('P'); - ret['v'] = ret.at('V'); - ret['L'] = q1.conjugate() * (x2.at('L') - x1.at('L')); - ret['O'] = (q1.conjugate() * q2).coeffs(); + Map<Quaterniond> dq(ret.at('O').data()); + + betweenStatesGlobalAcceleration(x1.at('P'), + x1.at('V'), + x1.at('L'), + q1, + x2.at('P'), + x2.at('V'), + x2.at('L'), + q2, + g, + dt, + ret.at('P'), + ret.at('V'), + ret.at('p'), + ret.at('v'), + ret.at('L'), + dq); + + // ret['P'] = q1.conjugate() * (x2.at('P') - x1.at('P') - x1.at('V') * dt - 0.5 * g * dt * dt); + // ret['V'] = q1.conjugate() * (x2.at('V') - x1.at('V') - g * dt); + // ret['p'] = ret.at('P'); + // ret['v'] = ret.at('V'); + // ret['L'] = q1.conjugate() * (x2.at('L') - x1.at('L')); + // ret['O'] = (q1.conjugate() * q2).coeffs(); } -inline VectorComposite betweenStates(const VectorComposite& x1, const VectorComposite& x2, double dt) +inline VectorComposite betweenStatesGlobalAcceleration(const VectorComposite& x1, + const VectorComposite& x2, + const Vector3d& g, + double dt) { VectorComposite ret; - betweenStates(x1, x2, dt, ret); + betweenStatesGlobalAcceleration(x1, x2, g, dt, ret); return ret; } +template <typename D1, + typename D2, + typename D5, + typename D6, + typename D7, + typename D8, + typename D11, + typename D12, + typename D13, + typename D14, + typename D15, + typename D16, + typename D17, + typename D18, + class T> +inline void betweenStates(const MatrixBase<D1>& p1, + const MatrixBase<D2>& v1, + const MatrixBase<D5>& L1, + const QuaternionBase<D6>& q1, + const MatrixBase<D7>& p2, + const MatrixBase<D8>& v2, + const MatrixBase<D11>& L2, + const QuaternionBase<D12>& q2, + const T dt, + MatrixBase<D13>& dpi, + MatrixBase<D14>& dvi, + MatrixBase<D15>& dpd, + MatrixBase<D16>& dvd, + MatrixBase<D17>& dL, + QuaternionBase<D18>& dq) +{ + betweenStatesGlobalAcceleration(p1, v1, L1, q1, p2, v2, L2, q2, gravity(), dt, dpi, dvi, dpd, dvd, dL, dq); +} + +template <typename D1, typename D2, typename D3, class T> +inline void betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T dt, MatrixBase<D3>& x2_minus_x1) +{ + betweenStatesGlobalAcceleration(x1, x2, gravity(), dt, x2_minus_x1); +} + +template <typename D1, typename D2, class T> +inline Matrix<typename D1::Scalar, 19, 1> betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T dt) +{ + return betweenStatesGlobalAcceleration(x1, x2, gravity(), dt);; +} + +inline void betweenStates(const VectorComposite& x1, const VectorComposite& x2, double dt, VectorComposite& ret) +{ + betweenStatesGlobalAcceleration(x1, x2, gravity(), dt, ret); +} + +inline VectorComposite betweenStates(const VectorComposite& x1, const VectorComposite& x2, double dt) +{ + return betweenStatesGlobalAcceleration(x1, x2, gravity(), dt); +} + template <typename Derived> Matrix<typename Derived::Scalar, 18, 1> log_fti(const MatrixBase<Derived>& delta_in) {