From 2a025d59d9453f923ddd70dd597b2102b34884d6 Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Thu, 4 Apr 2024 20:30:39 +0100 Subject: [PATCH 1/6] [spatial] Added minus operator in symmetric3 --- include/pinocchio/spatial/symmetric3.hpp | 11 +++++++++++ unittest/symmetric.cpp | 12 +++++++++++- 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/spatial/symmetric3.hpp b/include/pinocchio/spatial/symmetric3.hpp index cbc8e6eabe..44bc5155f6 100644 --- a/include/pinocchio/spatial/symmetric3.hpp +++ b/include/pinocchio/spatial/symmetric3.hpp @@ -1,5 +1,6 @@ // // Copyright (c) 2014-2019 CNRS INRIA +// Copyright (c) 2024 Heriot-Watt University // #ifndef __pinocchio_symmetric3_hpp__ @@ -339,6 +340,16 @@ namespace pinocchio m_data += s2.m_data; return *this; } + Symmetric3Tpl operator-(const Symmetric3Tpl & s2) const + { + return Symmetric3Tpl((m_data-s2.m_data).eval()); + } + + Symmetric3Tpl & operator-=(const Symmetric3Tpl & s2) + { + m_data -= s2.m_data; return *this; + } + template static void rhsMult(const Symmetric3Tpl & S3, const Eigen::MatrixBase & vin, diff --git a/unittest/symmetric.cpp b/unittest/symmetric.cpp index 2da18f81a4..8288c67b52 100644 --- a/unittest/symmetric.cpp +++ b/unittest/symmetric.cpp @@ -89,7 +89,17 @@ BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 ) S2 = Symmetric3::Random(); Symmetric3 Scopy = S; S+=S2; - BOOST_CHECK(S.matrix().isApprox(S2.matrix()+Scopy.matrix(), 1e-12)); + BOOST_CHECK(S.matrix().isApprox(Scopy.matrix()+S2.matrix(), 1e-12)); + } + + // S -= S + { + Symmetric3 + S = Symmetric3::Random(), + S2 = Symmetric3::Random(); + Symmetric3 Scopy = S; + S-=S2; + BOOST_CHECK(S.matrix().isApprox(Scopy.matrix()-S2.matrix(), 1e-12)); } // S + M From 69f19331cf703f83471ea1eb040ad58c33e8e301 Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Thu, 4 Apr 2024 20:32:03 +0100 Subject: [PATCH 2/6] [spatial] Added inertia minus operators --- .../bindings/python/spatial/inertia.hpp | 5 +++ include/pinocchio/spatial/inertia.hpp | 35 +++++++++++++++++++ unittest/spatial.cpp | 22 ++++++++---- 3 files changed, 56 insertions(+), 6 deletions(-) diff --git a/include/pinocchio/bindings/python/spatial/inertia.hpp b/include/pinocchio/bindings/python/spatial/inertia.hpp index e9f43d97a3..bc418ce552 100644 --- a/include/pinocchio/bindings/python/spatial/inertia.hpp +++ b/include/pinocchio/bindings/python/spatial/inertia.hpp @@ -1,6 +1,7 @@ // // Copyright (c) 2015-2023 CNRS INRIA // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. +// Copyright (c) 2024 Heriot-Watt University // #ifndef __pinocchio_python_spatial_inertia_hpp__ @@ -117,6 +118,10 @@ namespace pinocchio .def(bp::self == bp::self) .def(bp::self != bp::self) + .def(bp::self += bp::self) + .def(bp::self + bp::self) + .def(bp::self -= bp::self) + .def(bp::self - bp::self) .def("isApprox", call::isApprox, diff --git a/include/pinocchio/spatial/inertia.hpp b/include/pinocchio/spatial/inertia.hpp index cf7e766e15..49e12fb7e5 100644 --- a/include/pinocchio/spatial/inertia.hpp +++ b/include/pinocchio/spatial/inertia.hpp @@ -1,6 +1,7 @@ // // Copyright (c) 2015-2021 CNRS INRIA // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. +// Copyright (c) 2024 Heriot-Watt University // #ifndef __pinocchio_inertia_hpp__ @@ -45,6 +46,9 @@ namespace pinocchio Derived_t& operator+= (const Derived_t & Yb) { return derived().__pequ__(Yb); } Derived_t operator+(const Derived_t & Yb) const { return derived().__plus__(Yb); } + + Derived_t& operator-= (const Derived_t & Yb) { return derived().__mequ__(Yb); } + Derived_t operator-(const Derived_t & Yb) const { return derived().__minus__(Yb); } template ForceTpl::Scalar,traits::Options> @@ -424,6 +428,37 @@ namespace pinocchio return *this; } + InertiaTpl __minus__(const InertiaTpl & Yb) const + { + /* Y_{a-b} = ( m_a-m_b, + * (m_a*c_a - m_b*c_b ) / (m_a - m_b), + * I_a - I_b + (m_a*m_b)/(m_a-m_b) * AB_x * AB_x ) + */ + + const Scalar eps = ::Eigen::NumTraits::epsilon(); + + const Scalar & mab = mass()-Yb.mass(); + const Scalar mab_inv = (mass()-Yb.mass() >= 0) ? Scalar(1)/math::max((Scalar)(mass()-Yb.mass()),eps) : -Scalar(1)/math::max((Scalar)(Yb.mass()-mass()),eps); + const Vector3 & AB = (lever()-Yb.lever()).eval(); + return InertiaTpl(mab, + (mass()*lever()-Yb.mass()*Yb.lever())*mab_inv, + inertia()-Yb.inertia() + (mass()*Yb.mass()*mab_inv)* typename Symmetric3::SkewSquare(AB)); + } + + InertiaTpl& __mequ__(const InertiaTpl & Yb) + { + const Scalar eps = ::Eigen::NumTraits::epsilon(); + + const InertiaTpl& Ya = *this; + const Scalar & mab = mass()-Yb.mass(); + const Scalar mab_inv = (mass()-Yb.mass() >= 0) ? Scalar(1)/math::max((Scalar)(mass()-Yb.mass()),eps) : -Scalar(1)/math::max((Scalar)(Yb.mass()-mass()),eps); + const Vector3 & AB = (Ya.lever()-Yb.lever()).eval(); + lever() *= (mass()*mab_inv); lever() -= (Yb.mass()*mab_inv)*Yb.lever(); //c *= mab_inv; + inertia() -= Yb.inertia(); inertia() += (Ya.mass()*Yb.mass()*mab_inv)* typename Symmetric3::SkewSquare(AB); + mass() = mab; + return *this; + } + template ForceTpl::Scalar,traits::Options> __mult__(const MotionDense & v) const diff --git a/unittest/spatial.cpp b/unittest/spatial.cpp index 0fec8fd14a..9e0386aa26 100644 --- a/unittest/spatial.cpp +++ b/unittest/spatial.cpp @@ -1,5 +1,6 @@ // // Copyright (c) 2015-2020 CNRS INRIA +// Copyright (c) 2024 Heriot-Watt University // #include @@ -540,16 +541,25 @@ BOOST_AUTO_TEST_CASE ( test_Inertia ) Force vxIv = aI.vxiv(v); BOOST_CHECK(vxf.toVector().isApprox(vxIv.toVector())); - // Test operator+ + // Test operator+ and operator- I1 = Inertia::Random(); Inertia I2 = Inertia::Random(); BOOST_CHECK((I1.matrix()+I2.matrix()).isApprox((I1+I2).matrix())); + BOOST_CHECK((I1.matrix()-I2.matrix()).isApprox((I1-I2).matrix())); + BOOST_CHECK((I2.matrix()-I1.matrix()).isApprox((I2-I1).matrix())); + BOOST_CHECK((I1-I1).isZero()); + + // operator += and operator -= + Inertia I12p = I1; + Inertia I12m = I1; + Inertia I21m = I2; + I12p += I2; + I12m -= I2; + I21m -= I1; + BOOST_CHECK((I1.matrix()+I2.matrix()).isApprox(I12p.matrix())); + BOOST_CHECK((I1.matrix()-I2.matrix()).isApprox(I12m.matrix())); + BOOST_CHECK((I2.matrix()-I1.matrix()).isApprox(I21m.matrix())); - // operator += - Inertia I12 = I1; - I12 += I2; - BOOST_CHECK((I1.matrix()+I2.matrix()).isApprox(I12.matrix())); - // Test operator vtiv double kinetic_ref = v.toVector().transpose() * aI.matrix() * v.toVector(); double kinetic = aI.vtiv(v); From c4ac479106d98181e3850be935b762a476dd28a1 Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Thu, 4 Apr 2024 20:56:09 +0100 Subject: [PATCH 3/6] [changelog] Updated changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6e00ef3034..3e8be57dd1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ## [Unreleased] ### Changed +- Introduced inertia/symmetric3 minus operators ([#2204](https://github.com/stack-of-tasks/pinocchio/pull/2204)) - Set NOMINMAX as a public definitions on Windows ([#2139](https://github.com/stack-of-tasks/pinocchio/pull/2139)) - Improve documentation of enum ReferenceFrame. - Improve documentation of `getJointJacobian`([#2193](https://github.com/stack-of-tasks/pinocchio/pull/2193)). From a81432480a709c695cb313d59881e06f625b184c Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Fri, 5 Apr 2024 00:23:15 +0100 Subject: [PATCH 4/6] [spatial] Reused mab and removed eval in minus operators (inertia) --- include/pinocchio/spatial/inertia.hpp | 4 ++-- include/pinocchio/spatial/symmetric3.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/pinocchio/spatial/inertia.hpp b/include/pinocchio/spatial/inertia.hpp index 49e12fb7e5..9a5f247db1 100644 --- a/include/pinocchio/spatial/inertia.hpp +++ b/include/pinocchio/spatial/inertia.hpp @@ -438,7 +438,7 @@ namespace pinocchio const Scalar eps = ::Eigen::NumTraits::epsilon(); const Scalar & mab = mass()-Yb.mass(); - const Scalar mab_inv = (mass()-Yb.mass() >= 0) ? Scalar(1)/math::max((Scalar)(mass()-Yb.mass()),eps) : -Scalar(1)/math::max((Scalar)(Yb.mass()-mass()),eps); + const Scalar mab_inv = (mab >= 0) ? Scalar(1)/math::max(mab,eps) : Scalar(1)/math::min(mab,-eps); const Vector3 & AB = (lever()-Yb.lever()).eval(); return InertiaTpl(mab, (mass()*lever()-Yb.mass()*Yb.lever())*mab_inv, @@ -451,7 +451,7 @@ namespace pinocchio const InertiaTpl& Ya = *this; const Scalar & mab = mass()-Yb.mass(); - const Scalar mab_inv = (mass()-Yb.mass() >= 0) ? Scalar(1)/math::max((Scalar)(mass()-Yb.mass()),eps) : -Scalar(1)/math::max((Scalar)(Yb.mass()-mass()),eps); + const Scalar mab_inv = (mab >= 0) ? Scalar(1)/math::max(mab,eps) : Scalar(1)/math::min(mab,-eps); const Vector3 & AB = (Ya.lever()-Yb.lever()).eval(); lever() *= (mass()*mab_inv); lever() -= (Yb.mass()*mab_inv)*Yb.lever(); //c *= mab_inv; inertia() -= Yb.inertia(); inertia() += (Ya.mass()*Yb.mass()*mab_inv)* typename Symmetric3::SkewSquare(AB); diff --git a/include/pinocchio/spatial/symmetric3.hpp b/include/pinocchio/spatial/symmetric3.hpp index 44bc5155f6..ac3ab029ba 100644 --- a/include/pinocchio/spatial/symmetric3.hpp +++ b/include/pinocchio/spatial/symmetric3.hpp @@ -342,7 +342,7 @@ namespace pinocchio Symmetric3Tpl operator-(const Symmetric3Tpl & s2) const { - return Symmetric3Tpl((m_data-s2.m_data).eval()); + return Symmetric3Tpl(m_data-s2.m_data); } Symmetric3Tpl & operator-=(const Symmetric3Tpl & s2) From c78e77a7376c2d82e1af15c6a38687761d0380e5 Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Fri, 5 Apr 2024 00:34:09 +0100 Subject: [PATCH 5/6] [spatial] Used eps to compute lever in FromDynamicParameters --- include/pinocchio/spatial/inertia.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/spatial/inertia.hpp b/include/pinocchio/spatial/inertia.hpp index 9a5f247db1..75b3644c6f 100644 --- a/include/pinocchio/spatial/inertia.hpp +++ b/include/pinocchio/spatial/inertia.hpp @@ -360,9 +360,11 @@ namespace pinocchio { PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, params, 10, 1); + const Scalar eps = ::Eigen::NumTraits::epsilon(); + const Scalar & mass = params[0]; Vector3 lever = params.template segment<3>(1); - lever /= mass; + lever /= (mass >= 0) ? math::max(mass,eps) : math::min(mass,-eps); return InertiaTpl(mass, lever, Symmetric3(params.template segment<6>(4)) + AlphaSkewSquare(mass,lever)); } From 13d6acba9f3fe636247a5c9d29e301cd023e01ef Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Fri, 5 Apr 2024 00:35:02 +0100 Subject: [PATCH 6/6] [spatial] Assert mass is positive --- include/pinocchio/spatial/inertia.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/include/pinocchio/spatial/inertia.hpp b/include/pinocchio/spatial/inertia.hpp index 75b3644c6f..add0f8578c 100644 --- a/include/pinocchio/spatial/inertia.hpp +++ b/include/pinocchio/spatial/inertia.hpp @@ -363,6 +363,7 @@ namespace pinocchio const Scalar eps = ::Eigen::NumTraits::epsilon(); const Scalar & mass = params[0]; + assert(mass >= Scalar(0.)); Vector3 lever = params.template segment<3>(1); lever /= (mass >= 0) ? math::max(mass,eps) : math::min(mass,-eps); @@ -440,6 +441,7 @@ namespace pinocchio const Scalar eps = ::Eigen::NumTraits::epsilon(); const Scalar & mab = mass()-Yb.mass(); + assert(mab >= Scalar(0.)); const Scalar mab_inv = (mab >= 0) ? Scalar(1)/math::max(mab,eps) : Scalar(1)/math::min(mab,-eps); const Vector3 & AB = (lever()-Yb.lever()).eval(); return InertiaTpl(mab, @@ -453,6 +455,7 @@ namespace pinocchio const InertiaTpl& Ya = *this; const Scalar & mab = mass()-Yb.mass(); + assert(mab >= Scalar(0.)); const Scalar mab_inv = (mab >= 0) ? Scalar(1)/math::max(mab,eps) : Scalar(1)/math::min(mab,-eps); const Vector3 & AB = (Ya.lever()-Yb.lever()).eval(); lever() *= (mass()*mab_inv); lever() -= (Yb.mass()*mab_inv)*Yb.lever(); //c *= mab_inv;