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

bullet-featherstone: support off-diagonal inertia #544

Merged
merged 3 commits into from
Nov 9, 2023
+33 −52
Merged

bullet-featherstone: support off-diagonal inertia #544

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
b541796
bullet-featherstone: support off-diagonal inertia
scpeters Sep 20, 2023
3d3714c
Merge branch 'gz-physics6' into scpeters/bullet_offdiagonal_inertia
scpeters Sep 20, 2023
2fe5481
Merge branch 'gz-physics6' into scpeters/bullet_offdiagonal_inertia
scpeters Nov 9, 2023
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
85 changes: 33 additions & 52 deletions bullet-featherstone/src/SDFFeatures.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -116,6 +116,7 @@ struct Structure
const ::sdf::Joint *rootJoint;
btScalar mass;
btVector3 inertia;
math::Pose3d linkToPrincipalAxesPose;

/// Is the root link fixed
bool fixedBase;
Expand All @@ -127,6 +128,20 @@ struct Structure
std::vector<const ::sdf::Link*> flatLinks;
};

/////////////////////////////////////////////////
void extractInertial(
const math::Inertiald &_inertial,
btScalar &_mass,
btVector3 &_principalInertiaMoments,
math::Pose3d &_linkToPrincipalAxesPose)
{
const auto &M = _inertial.MassMatrix();
_mass = static_cast<btScalar>(M.Mass());
_principalInertiaMoments = convertVec(M.PrincipalMoments());
_linkToPrincipalAxesPose = _inertial.Pose();
_linkToPrincipalAxesPose.Rot() *= M.PrincipalAxesOffset();
}

/////////////////////////////////////////////////
std::optional<Structure> ValidateModel(const ::sdf::Model &_sdfModel)
{
Expand Down Expand Up @@ -316,23 +331,14 @@ std::optional<Structure> ValidateModel(const ::sdf::Model &_sdfModel)
};
flattenLinkTree(rootLink);

const auto &M = rootLink->Inertial().MassMatrix();
const auto mass = static_cast<btScalar>(M.Mass());
btVector3 inertia(convertVec(M.DiagonalMoments()));
for (const double &I : {M.Ixy(), M.Ixz(), M.Iyz()})
{
if (std::abs(I) > 1e-3)
{
gzerr << "The base link of the model is required to have a diagonal "
<< "inertia matrix by gz-physics-bullet-featherstone, but the "
<< "Model [" << _sdfModel.Name() << "] has a non-zero diagonal "
<< "value: " << I << "\n";
return std::nullopt;
}
}
btScalar mass;
btVector3 inertia;
math::Pose3d linkToPrincipalAxesPose;
extractInertial(rootLink->Inertial(), mass, inertia, linkToPrincipalAxesPose);

return Structure{
rootLink, rootJoint, mass, inertia, fixed, parentOf, flatLinks};
rootLink, rootJoint, mass, inertia, linkToPrincipalAxesPose, fixed,
parentOf, flatLinks};
}

/////////////////////////////////////////////////
Expand All @@ -350,7 +356,7 @@ Identity SDFFeatures::ConstructSdfModel(
const auto *world = this->ReferenceInterface<WorldInfo>(_worldID);

const auto rootInertialToLink =
gz::math::eigen3::convert(structure.rootLink->Inertial().Pose()).inverse();
gz::math::eigen3::convert(structure.linkToPrincipalAxesPose).inverse();
const auto modelID = this->AddModel(
_sdfModel.Name(), _worldID, rootInertialToLink,
std::make_unique<btMultiBody>(
Expand Down Expand Up @@ -389,8 +395,12 @@ Identity SDFFeatures::ConstructSdfModel(
for (int i = 0; i < static_cast<int>(structure.flatLinks.size()); ++i)
{
const auto *link = structure.flatLinks[static_cast<std::size_t>(i)];
btScalar mass;
btVector3 inertia;
math::Pose3d linkToPrincipalAxesPose;
extractInertial(link->Inertial(), mass, inertia, linkToPrincipalAxesPose);
const Eigen::Isometry3d linkToComTf = gz::math::eigen3::convert(
link->Inertial().Pose());
linkToPrincipalAxesPose);

if (linkIDs.find(link) == linkIDs.end())
{
Expand All @@ -399,22 +409,6 @@ Identity SDFFeatures::ConstructSdfModel(
linkIDs.insert(std::make_pair(link, linkID));
}

const auto &M = link->Inertial().MassMatrix();
const btScalar mass = static_cast<btScalar>(M.Mass());
const auto inertia = btVector3(convertVec(M.DiagonalMoments()));

for (const double I : {M.Ixy(), M.Ixz(), M.Iyz()})
{
if (std::abs(I) > 1e-3)
{
gzerr << "Links are required to have a diagonal inertia matrix in "
<< "gz-physics-bullet-featherstone, but Link [" << link->Name()
<< "] in Model [" << model->name << "] has a non-zero off "
<< "diagonal value in its inertia matrix\n";
return this->GenerateInvalidId();
}
}

if (structure.parentOf.size())
{
const auto &parentInfo = structure.parentOf.at(link);
Expand Down Expand Up @@ -614,25 +608,12 @@ Identity SDFFeatures::ConstructSdfModel(

{
const auto *link = structure.rootLink;
const auto &M = link->Inertial().MassMatrix();
const btScalar mass = static_cast<btScalar>(M.Mass());
const auto inertia = convertVec(M.DiagonalMoments());

for (const double I : {M.Ixy(), M.Ixz(), M.Iyz()})
{
if (std::abs(I) > 1e-3)
{
gzerr << "Links are required to have a diagonal inertia matrix in "
<< "gz-physics-bullet-featherstone, but Link [" << link->Name()
<< "] in Model [" << model->name << "] has a non-zero off "
<< "diagonal value in its inertia matrix\n";
}
else
{
model->body->setBaseMass(mass);
model->body->setBaseInertia(inertia);
}
}
btScalar mass;
btVector3 inertia;
math::Pose3d linkToPrincipalAxesPose;
extractInertial(link->Inertial(), mass, inertia, linkToPrincipalAxesPose);
model->body->setBaseMass(mass);
model->body->setBaseInertia(inertia);
}

world->world->addMultiBody(model->body.get());
Expand Down