Verified Commit 9f5db041 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

spatial: add isValid method to check wether an inertia is valid or not

parent 2deaac0e
Pipeline #13772 failed with stage
in 156 minutes and 57 seconds
......@@ -551,6 +551,14 @@ namespace pinocchio
lever().template cast<NewScalar>(),
inertia().template cast<NewScalar>());
}
/// \brief Check whether *this is a valid inertia, resulting from a positive mass distribution
bool isValid() const
{
return
(m_mass > Scalar(0) && m_inertia.isValid())
|| (m_mass == Scalar(0) && (m_inertia.data() == Scalar(0)).all());
}
protected:
Scalar m_mass;
......
......@@ -454,6 +454,14 @@ namespace pinocchio
{
return Symmetric3Tpl<NewScalar,Options>(m_data.template cast<NewScalar>());
}
bool isValid() const
{
return
m_data(0) >= Scalar(0)
&& m_data(2) >= Scalar(0)
&& m_data(5) >= Scalar(0);
}
protected:
Vector6 m_data;
......
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