Verified Commit b0c18326 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

spatial: fix ill-posed Inertias

parent b996d939
Pipeline #13775 passed with stage
in 169 minutes and 17 seconds
......@@ -289,7 +289,7 @@ namespace pinocchio
return getFrameId(frame.name,frame.type);
frames.push_back(frame);
if(frame.inertia.mass() > Scalar(0))
// if(frame.inertia.mass() > Scalar(0))
inertias[frame.parent] += frame.placement.act(frame.inertia);
nframes++;
return FrameIndex(nframes - 1);
......
......@@ -364,7 +364,9 @@ namespace pinocchio
* I_a + I_b - (m_a*m_b)/(m_a+m_b) * AB_x * AB_x )
*/
const Scalar & mab = mass()+Yb.mass();
const Scalar eps = std::numeric_limits<Scalar>::epsilon();
const Scalar & mab = math::max(mass()+Yb.mass(),eps);
const Scalar mab_inv = Scalar(1)/mab;
const Vector3 & AB = (lever()-Yb.lever()).eval();
return InertiaTpl(mab,
......@@ -374,8 +376,10 @@ namespace pinocchio
InertiaTpl& __pequ__(const InertiaTpl & Yb)
{
const Scalar eps = std::numeric_limits<Scalar>::epsilon();
const InertiaTpl& Ya = *this;
const Scalar & mab = Ya.mass()+Yb.mass();
const Scalar & mab = math::max(mass()+Yb.mass(),eps);
const Scalar mab_inv = Scalar(1)/mab;
const Vector3 & AB = (Ya.lever()-Yb.lever()).eval();
lever() *= (mass()*mab_inv); lever() += (Yb.mass()*mab_inv)*Yb.lever(); //c *= mab_inv;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment