Skip to content

Commit

Permalink
Merge branch 'gz-physics7' into scpeters/merge_7_8
Browse files Browse the repository at this point in the history
  • Loading branch information
@scpeters
scpeters committed Nov 10, 2023
2 parents 0f8eb1f + 780e2d0 commit d247f1f
Show file tree
Hide file tree
Showing 11 changed files with 631 additions and 184 deletions.
24 changes: 1 addition & 23 deletions bullet-featherstone/src/FreeGroupFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,29 +68,7 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity(

if (model != nullptr)
{
// Set angular velocity the each one of the joints of the model
for (const auto& jointID : model->jointEntityIds)
{
auto jointInfo = this->joints[jointID];
if (!jointInfo->motor)
{
auto *modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
jointInfo->motor = std::make_shared<btMultiBodyJointMotor>(
modelInfo->body.get(),
std::get<InternalJoint>(jointInfo->identifier).indexInBtModel,
0,
static_cast<btScalar>(0),
static_cast<btScalar>(jointInfo->effort));
auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
world->world->addMultiBodyConstraint(jointInfo->motor.get());
}

if (jointInfo->motor)
{
jointInfo->motor->setVelocityTarget(
static_cast<btScalar>(_angularVelocity[2]));
}
}
model->body->setBaseOmega(convertVec(_angularVelocity));
}
}

Expand Down
18 changes: 10 additions & 8 deletions bullet-featherstone/src/JointFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -304,11 +304,11 @@ Identity JointFeatures::AttachFixedJoint(
const BaseLink3dPtr &_parent,
const std::string &_name)
{
auto linkInfo = this->ReferenceInterface<LinkInfo>(_childID);
auto modelInfo = this->ReferenceInterface<ModelInfo>(linkInfo->model);
auto parentLinkInfo = this->ReferenceInterface<LinkInfo>(
auto *linkInfo = this->ReferenceInterface<LinkInfo>(_childID);
auto *modelInfo = this->ReferenceInterface<ModelInfo>(linkInfo->model);
auto *parentLinkInfo = this->ReferenceInterface<LinkInfo>(
_parent->FullIdentity());
auto parentModelInfo = this->ReferenceInterface<ModelInfo>(
auto *parentModelInfo = this->ReferenceInterface<ModelInfo>(
parentLinkInfo->model);
auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);

Expand All @@ -323,16 +323,18 @@ Identity JointFeatures::AttachFixedJoint(
linkInfo->model
});

auto jointInfo = this->ReferenceInterface<JointInfo>(jointID);
auto parentLinkIdx = parentLinkInfo->indexInModel.value_or(-1);
auto childLinkIdx = linkInfo->indexInModel.value_or(-1);
auto *jointInfo = this->ReferenceInterface<JointInfo>(jointID);

jointInfo->fixedConstraint = std::make_shared<btMultiBodyFixedConstraint>(
parentModelInfo->body.get(), -1,
modelInfo->body.get(), -1,
parentModelInfo->body.get(), parentLinkIdx,
modelInfo->body.get(), childLinkIdx,
btVector3(0, 0, 0), btVector3(0, 0, 0),
btMatrix3x3::getIdentity(),
btMatrix3x3::getIdentity());

if (world && world->world)
if (world != nullptr && world->world)
{
world->world->addMultiBodyConstraint(jointInfo->fixedConstraint.get());
return this->GenerateIdentity(jointID, this->joints.at(jointID));
Expand Down
2 changes: 1 addition & 1 deletion bullet-featherstone/src/KinematicsFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,7 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld(
}
}

if (model->body == nullptr)
if (!model || model->body == nullptr)
model = this->FrameInterface<ModelInfo>(_id);
}

Expand Down
172 changes: 75 additions & 97 deletions bullet-featherstone/src/SDFFeatures.cc
View file
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 All @@ -135,9 +150,12 @@ std::optional<Structure> ValidateModel(const ::sdf::Model &_sdfModel)
const ::sdf::Joint *rootJoint = nullptr;
bool fixed = false;
const std::string &rootModelName = _sdfModel.Name();
// a map of parent link to a vector of its child links
std::unordered_map<const ::sdf::Link*, std::vector<const ::sdf::Link*>>
linkTree;

const auto checkModel =
[&rootLink, &rootJoint, &fixed, &parentOf, &rootModelName](
[&rootLink, &rootJoint, &fixed, &parentOf, &rootModelName, &linkTree](
const ::sdf::Model &model) -> bool
{
for (std::size_t i = 0; i < model.JointCount(); ++i)
Expand Down Expand Up @@ -217,6 +235,10 @@ std::optional<Structure> ValidateModel(const ::sdf::Model &_sdfModel)
<< rootModelName << "] has multiple parent joints. That is not "
<< "supported by the gz-physics-bullet-featherstone plugin.\n";
}
if (parent != nullptr)
{
linkTree[parent].push_back(child);
}
}

return true;
Expand All @@ -231,11 +253,10 @@ std::optional<Structure> ValidateModel(const ::sdf::Model &_sdfModel)
return std::nullopt;
}

std::vector<const ::sdf::Link*> flatLinks;
std::unordered_map<const ::sdf::Link*, std::size_t> linkIndex;
const auto flattenLinks =
[&rootLink, &parentOf, &rootModelName, &flatLinks, &linkIndex](
const ::sdf::Model &model) -> bool
// Find root link in model and verify that there is only one root link in
// the model. Returns false if more than one root link is found
const auto findRootLink =
[&rootLink, &parentOf, &rootModelName](const ::sdf::Model &model) -> bool
{
for (std::size_t i = 0; i < model.LinkCount(); ++i)
{
Expand All @@ -254,62 +275,28 @@ std::optional<Structure> ValidateModel(const ::sdf::Model &_sdfModel)
}

rootLink = link;
continue;
}

linkIndex[link] = linkIndex.size();
flatLinks.push_back(link);
}

return true;
};

if (!flattenLinks(_sdfModel))
return std::nullopt;

for (std::size_t i = 0; i < _sdfModel.ModelCount(); ++i)
if (rootLink == nullptr && !findRootLink(_sdfModel))
{
if (!flattenLinks(*_sdfModel.ModelByIndex(i)))
return std::nullopt;
// No root link was found in this model
return std::nullopt;
}

// The documentation for bullet does not mention whether parent links must
// have a lower index than their child links, but the Featherstone Algorithm
// needs to crawl up and down the tree systematically, and so the flattened
// tree structures used by the algorithm usually do expect the parents to
// come before their children in the array, and do not work correctly if that
// ordering is not held. Out of an abundance of caution we will assume that
// ordering is necessary.
for (std::size_t i = 0; i < flatLinks.size(); ++i)
// find root link in nested models if one was not already found
for (std::size_t i = 0; i < _sdfModel.ModelCount(); ++i)
{
// Every element in flatLinks should have a parent if the earlier validation
// was done correctly.
if (parentOf.size() == 0)
if (rootLink != nullptr)

Check warning on line 293 in bullet-featherstone/src/SDFFeatures.cc

View check run for this annotation

Codecov / codecov/patch

bullet-featherstone/src/SDFFeatures.cc#L293 

Added line #L293 was not covered by tests
{
break;
}
const auto *parentJoint = parentOf.at(flatLinks[i]).joint;

const auto *parentLink =
_sdfModel.LinkByName(parentJoint->ParentName());
const auto p_index_it = linkIndex.find(parentLink);
if (p_index_it == linkIndex.end())
if (!findRootLink(*_sdfModel.ModelByIndex(i)))

Check warning on line 297 in bullet-featherstone/src/SDFFeatures.cc

View check run for this annotation

Codecov / codecov/patch

bullet-featherstone/src/SDFFeatures.cc#L297 

Added line #L297 was not covered by tests
{
// If the parent index cannot be found, that must mean the parent is the
// root link, so this link can go anywhere in the list as long as it is
// before its own children.
assert(parentLink == rootLink);
continue;
}

auto &p_index = p_index_it->second;
if (i < p_index)
{
// The current link is in front of its parent link in the array. We must
// swap their places.
std::swap(flatLinks[i], flatLinks[p_index]);
p_index = i;
linkIndex[flatLinks[p_index]] = p_index;
return std::nullopt;

Check warning on line 299 in bullet-featherstone/src/SDFFeatures.cc

View check run for this annotation

Codecov / codecov/patch

bullet-featherstone/src/SDFFeatures.cc#L299 

Added line #L299 was not covered by tests
}
}

Expand All @@ -319,23 +306,39 @@ std::optional<Structure> ValidateModel(const ::sdf::Model &_sdfModel)
return std::nullopt;
}

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()})
// The documentation for bullet does not mention whether parent links must
// have a lower index than their child links, but the Featherstone Algorithm
// needs to crawl up and down the tree systematically, and so the flattened
// tree structures used by the algorithm usually do expect the parents to
// come before their children in the array, and do not work correctly if that
// ordering is not held. Out of an abundance of caution we will assume that
// ordering is necessary.
std::vector<const ::sdf::Link*> flatLinks;
std::function<void(const ::sdf::Link *)> flattenLinkTree =
[&](const ::sdf::Link *link)
{
if (std::abs(I) > 1e-3)
if (link != rootLink)
{
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;
flatLinks.push_back(link);
}
}
if (auto it = linkTree.find(link); it != linkTree.end())
{
for (const auto &child_link : it->second)
{
flattenLinkTree(child_link);
}
}
};
flattenLinkTree(rootLink);

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 @@ -353,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 @@ -392,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 @@ -402,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 @@ -617,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
1 change: 1 addition & 0 deletions test/common_test/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@ set(tests
basic_test
collisions
construct_empty_world
detachable_joint
free_joint_features
joint_features
joint_mimic_features
Expand Down
Loading

0 comments on commit d247f1f

Please sign in to comment.