Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Introduced inertia/symmetric3 minus operators #2204

Open
wants to merge 6 commits into
base: devel
Choose a base branch
from
Open

Introduced inertia/symmetric3 minus operators #2204

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
2a025d5
[spatial] Added minus operator in symmetric3
cmastalli Apr 4, 2024
69f1933
[spatial] Added inertia minus operators
cmastalli Apr 4, 2024
c4ac479
[changelog] Updated changelog
cmastalli Apr 4, 2024
a814324
[spatial] Reused mab and removed eval in minus operators (inertia)
cmastalli Apr 4, 2024
c78e77a
[spatial] Used eps to compute lever in FromDynamicParameters
cmastalli Apr 4, 2024
13d6acb
[spatial] Assert mass is positive
cmastalli Apr 4, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions CHANGELOG.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)).
Expand Down
5 changes: 5 additions & 0 deletions include/pinocchio/bindings/python/spatial/inertia.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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__
Expand Down Expand Up @@ -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<Inertia>::isApprox,
Expand Down
42 changes: 41 additions & 1 deletion include/pinocchio/spatial/inertia.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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__
Expand Down Expand Up @@ -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<typename MotionDerived>
ForceTpl<typename traits<MotionDerived>::Scalar,traits<MotionDerived>::Options>
Expand Down Expand Up @@ -356,9 +360,12 @@ namespace pinocchio
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, params, 10, 1);

const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();

const Scalar & mass = params[0];
assert(mass >= Scalar(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));
}
Expand Down Expand Up @@ -424,6 +431,39 @@ 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<Scalar>::epsilon();

const Scalar & mab = mass()-Yb.mass();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It would be nice to check whether the resulting inertia remains physically consistent, certainly with an assert.

Copy link
Member Author

@cmastalli cmastalli Apr 4, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I have included asserts for checking mass is positive (here and in FromDynamicParameters). However, I wonder what is the expected design for this class. Should InertialTpl handle fully physically consistent only? Or could it accept "partial" physical consistency or anything?

For instance, the Random function generates positive masses and rotational inertias with positive principal components of inertia. However, this is not what we refer to as fully physical consistency. This condition also implies having positive second moments of inertia, i.e., satisfying the named "triangular inequalities". In short, Random function doesn't support full physical consistency, just a partial physical consistency.

Moreover, FromDynamicParameters doesn't assert positive masses or rotation inertia triangular inequalities. In short, FromDynamicParameters function supports any type of parameter including the ones that don't satisfy physics. It doesn't follow the same convention used in Random.

In conclusion, I am inclined

  1. to allow physically inconsistent inertias (mass and rotational inertia),
  2. to remove asserts (as requested and introduced in this PR), and
  3. to define a "checkConsistency" function where users can trigger their checks on demand.

Please let me know what are your thoughts and how to proceed with this PR.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@jcarpent -- please let me know your thoughts. I still think we should not include these asserts as resulting operators might not necessarily satisfy fully physical consistency. The reason is this class should change much more to do that. We can arrange a meeting to discuss this.

Be aware that the unit tests are not passing because they are designed to test conditions that don't meet full physical consistency. I'll need to modify them if we want to do that.

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,
(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<Scalar>::epsilon();

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;
inertia() -= Yb.inertia(); inertia() += (Ya.mass()*Yb.mass()*mab_inv)* typename Symmetric3::SkewSquare(AB);
mass() = mab;
return *this;
}

template<typename MotionDerived>
ForceTpl<typename traits<MotionDerived>::Scalar,traits<MotionDerived>::Options>
__mult__(const MotionDense<MotionDerived> & v) const
Expand Down
11 changes: 11 additions & 0 deletions include/pinocchio/spatial/symmetric3.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
//
// Copyright (c) 2014-2019 CNRS INRIA
// Copyright (c) 2024 Heriot-Watt University
//

#ifndef __pinocchio_symmetric3_hpp__
Expand Down Expand Up @@ -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);
}

Symmetric3Tpl & operator-=(const Symmetric3Tpl & s2)
{
m_data -= s2.m_data; return *this;
}

template<typename V3in, typename V3out>
static void rhsMult(const Symmetric3Tpl & S3,
const Eigen::MatrixBase<V3in> & vin,
Expand Down
22 changes: 16 additions & 6 deletions unittest/spatial.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2024 Heriot-Watt University
//

#include <iostream>
Expand Down Expand Up @@ -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);
Expand Down
12 changes: 11 additions & 1 deletion unittest/symmetric.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Loading