Skip to content

Commit

Permalink
[spatial] Assert mass is positive
Browse files Browse the repository at this point in the history
  • Loading branch information
cmastalli committed Apr 4, 2024
1 parent afe7497 commit b5f30fc
Showing 1 changed file with 3 additions and 0 deletions.
3 changes: 3 additions & 0 deletions include/pinocchio/spatial/inertia.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -363,6 +363,7 @@ namespace pinocchio
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 >= 0) ? math::max(mass,eps) : math::min(mass,-eps);

Expand Down Expand Up @@ -440,6 +441,7 @@ namespace pinocchio
const Scalar eps = ::Eigen::NumTraits<Scalar>::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,
Expand All @@ -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;
Expand Down

0 comments on commit b5f30fc

Please sign in to comment.