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

Jump to bottom

bullet-featherstone: Fix model frame data pose #649

Open
wants to merge 9 commits into
base: gz-physics7
Choose a base branch
from
Open

bullet-featherstone: Fix model frame data pose #649

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
4f35c1e
Fix bounding box for collisions with pose offset
iche033 Jun 3, 2024
5c6ec0f
add link frame semantics pose test
iche033 Jun 4, 2024
84054fa
update test
iche033 Jun 4, 2024
27deae9
Add failing test showing that model framedata calculation is incorrect
azeey Jun 5, 2024
069668f
fix frame data for models and nested models
iche033 Jun 5, 2024
12ac207
merge from gz-physics7
iche033 Jun 5, 2024
081c672
remove debug print
iche033 Jun 5, 2024
eae5356
Merge branch 'gz-physics7' into bullet_kinematic_feature
iche033 Jul 9, 2024
a35c00f
Merge branch 'gz-physics7' into bullet_kinematic_feature
iche033 Aug 8, 2024
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
26 changes: 22 additions & 4 deletions bullet-featherstone/src/Base.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,9 +86,13 @@ struct ModelInfo
std::string name;
Identity world;
int indexInWorld;
Eigen::Isometry3d modelToRootLinkTf;
Eigen::Isometry3d baseInertiaToLinkFrame;
Eigen::Isometry3d nestedModelFromRootModelTf;
std::shared_ptr<btMultiBody> body;

bool isNestedModel = false;

std::vector<std::size_t> linkEntityIds;
std::vector<std::size_t> jointEntityIds;
std::vector<std::size_t> nestedModelEntityIds;
Expand All @@ -103,11 +107,15 @@ struct ModelInfo
ModelInfo(
std::string _name,
Identity _world,
Eigen::Isometry3d _modelToRootLinkTf,
Eigen::Isometry3d _baseInertiaToLinkFrame,
Eigen::Isometry3d _nestedModelFromRootModelTf,
std::shared_ptr<btMultiBody> _body)
: name(std::move(_name)),
world(std::move(_world)),
modelToRootLinkTf(_modelToRootLinkTf),
baseInertiaToLinkFrame(_baseInertiaToLinkFrame),
nestedModelFromRootModelTf(_nestedModelFromRootModelTf),
body(std::move(_body))
{
// Do nothing
Expand Down Expand Up @@ -310,8 +318,8 @@ class Base : public Implements3d<FeatureList<Feature>>
auto worldID = this->GenerateIdentity(id, world);

auto worldModel = std::make_shared<ModelInfo>(
world->name, worldID,
Eigen::Isometry3d::Identity(), nullptr);
world->name, worldID, Eigen::Isometry3d::Identity(),
Eigen::Isometry3d::Identity(), Eigen::Isometry3d::Identity(), nullptr);
this->models[id] = worldModel;
world->modelNameToEntityId[worldModel->name] = id;
worldModel->indexInWorld = -1;
Expand All @@ -323,13 +331,17 @@ class Base : public Implements3d<FeatureList<Feature>>
public: inline Identity AddModel(
std::string _name,
Identity _worldID,
Eigen::Isometry3d _modelToRootLinkTf,
Eigen::Isometry3d _baseInertialToLinkFrame,
std::shared_ptr<btMultiBody> _body)
{
const auto id = this->GetNextEntity();
auto model = std::make_shared<ModelInfo>(
std::move(_name), std::move(_worldID),
std::move(_baseInertialToLinkFrame), std::move(_body));
std::move(_modelToRootLinkTf),
std::move(_baseInertialToLinkFrame),
Eigen::Isometry3d::Identity(),
std::move(_body));

this->models[id] = model;
auto *world = this->ReferenceInterface<WorldInfo>(model->world);
Expand All @@ -348,14 +360,20 @@ class Base : public Implements3d<FeatureList<Feature>>
std::string _name,
Identity _parentID,
Identity _worldID,
Eigen::Isometry3d _modelToRootLinkTf,
Eigen::Isometry3d _baseInertialToLinkFrame,
Eigen::Isometry3d _nestedModelFromRootModelTf,
std::shared_ptr<btMultiBody> _body)
{
const auto id = this->GetNextEntity();
auto model = std::make_shared<ModelInfo>(
std::move(_name), std::move(_worldID),
std::move(_baseInertialToLinkFrame), std::move(_body));
std::move(_modelToRootLinkTf),
std::move(_baseInertialToLinkFrame),
std::move(_nestedModelFromRootModelTf),
std::move(_body));

model->isNestedModel = true;
this->models[id] = model;
const auto parentModel = this->models.at(_parentID);
parentModel->nestedModelEntityIds.push_back(id);
Expand Down
15 changes: 11 additions & 4 deletions bullet-featherstone/src/KinematicsFeatures.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,10 +113,17 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld(
FrameData data;
if (model && model->body)
{
data.pose = convert(model->body->getBaseWorldTransform());
if (!isModel)
data.pose = data.pose * model->baseInertiaToLinkFrame;
if (isCollision)
data.pose = convert(model->body->getBaseWorldTransform())
* model->baseInertiaToLinkFrame;
if (isModel)
{
data.pose = model->modelToRootLinkTf.inverse() * data.pose;
if (model->isNestedModel)
{
data.pose = data.pose * model->nestedModelFromRootModelTf;
}
}
else if (isCollision)
data.pose = data.pose * collisionPoseOffset;
data.linearVelocity = convert(model->body->getBaseVel());
data.angularVelocity = convert(model->body->getBaseOmega());
Expand Down
81 changes: 50 additions & 31 deletions bullet-featherstone/src/SDFFeatures.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -492,6 +492,12 @@ Identity SDFFeatures::ConstructSdfModelImpl(

const bool isStatic = _sdfModel.Static();
WorldInfo *world = nullptr;

const auto modelToRootLink =
ResolveSdfPose(structure.rootLink->SemanticPose());
if (!modelToRootLink)
return this->GenerateInvalidId();

const auto rootInertialToLink =
gz::math::eigen3::convert(structure.linkToPrincipalAxesPose).inverse();

Expand All @@ -501,7 +507,31 @@ Identity SDFFeatures::ConstructSdfModelImpl(
std::unordered_map<const ::sdf::Model*, Identity> modelIDs;
std::size_t rootModelID = 0u;
std::shared_ptr<btMultiBody> rootMultiBody;
// Add all models, including nested models

auto getModelScopedName = [&](const ::sdf::Model *_targetModel,
const ::sdf::Model *_parentModel, const std::string &_prefix,
std::string &_modelScopedName, auto &&_getModelScopedName)
{
for (std::size_t i = 0u; i < _parentModel->ModelCount(); ++i)
{
auto childModel = _parentModel->ModelByIndex(i);
if (childModel == _targetModel)
{
_modelScopedName = _prefix + childModel->Name();
return true;
}
else
{
if (_getModelScopedName(_targetModel, childModel,
_prefix + childModel->Name() + "::", _modelScopedName,
_getModelScopedName))
return true;
}
}
return false;
};

// Add all models, including nested models
auto addModels = [&](std::size_t _modelOrWorldID, const ::sdf::Model *_model,
auto &&_addModels)
{
Expand All @@ -517,8 +547,22 @@ Identity SDFFeatures::ConstructSdfModelImpl(
auto worldIdentity = modelIt->second->world;
auto modelIdentity =
this->GenerateIdentity(_modelOrWorldID, modelIt->second);

std::string modelScopedName;
math::Pose3d modelPose;
if (!getModelScopedName(_model, &_sdfModel,
_sdfModel.Name() + "::", modelScopedName, getModelScopedName))
this->GenerateInvalidId();
auto errors = _sdfModel.SemanticPose().Resolve(modelPose,
modelScopedName);
if (!errors.empty())
this->GenerateInvalidId();
auto modelToNestedModel = math::eigen3::convert(modelPose).inverse();

return this->AddNestedModel(
_model->Name(), modelIdentity, worldIdentity, rootInertialToLink,
_model->Name(), modelIdentity, worldIdentity,
*modelToRootLink, rootInertialToLink,
modelToNestedModel,
rootMultiBody);
}
else
Expand All @@ -533,7 +577,9 @@ Identity SDFFeatures::ConstructSdfModelImpl(
true);
world = this->ReferenceInterface<WorldInfo>(worldIdentity);
auto id = this->AddModel(
_model->Name(), worldIdentity, rootInertialToLink,
_model->Name(), worldIdentity,
*modelToRootLink,
rootInertialToLink,
rootMultiBody);
rootModelID = id;
return id;
Expand Down Expand Up @@ -829,31 +875,9 @@ Identity SDFFeatures::ConstructSdfModelImpl(
// get the scoped name of the model
// This is used to resolve the model pose
std::string modelScopedName;
auto getModelScopedName = [&](const ::sdf::Model *_targetModel,
const ::sdf::Model *_parentModel, const std::string &_prefix,
auto &&_getModelScopedName)
{
for (std::size_t i = 0u; i < _parentModel->ModelCount(); ++i)
{
auto childModel = _parentModel->ModelByIndex(i);
if (childModel == _targetModel)
{
modelScopedName = _prefix + childModel->Name();
return true;
}
else
{
if (_getModelScopedName(_targetModel, childModel,
_prefix + childModel->Name() + "::", _getModelScopedName))
return true;
}
}
return false;
};

math::Pose3d modelPose;
if (!getModelScopedName(structure.model, &_sdfModel,
_sdfModel.Name() + "::", getModelScopedName))
_sdfModel.Name() + "::", modelScopedName, getModelScopedName))
return this->GenerateInvalidId();

auto errors = _sdfModel.SemanticPose().Resolve(modelPose,
Expand All @@ -863,11 +887,6 @@ Identity SDFFeatures::ConstructSdfModelImpl(
modelToNestedModel = math::eigen3::convert(modelPose).inverse();
}

const auto modelToRootLink =
ResolveSdfPose(structure.rootLink->SemanticPose());
if (!modelToRootLink)
return this->GenerateInvalidId();

const auto worldToRootCom =
*worldToModel * modelToNestedModel * *modelToRootLink *
rootInertialToLink.inverse();
Expand Down
44 changes: 43 additions & 1 deletion test/common_test/kinematic_features.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,11 +76,13 @@ struct KinematicFeaturesList : gz::physics::FeatureList<
gz::physics::sdf::ConstructSdfWorld,
gz::physics::GetShapeFromLink,
gz::physics::GetModelFromWorld,
gz::physics::GetNestedModelFromModel,
gz::physics::GetLinkFromModel,
gz::physics::GetJointFromModel,
gz::physics::JointFrameSemantics,
gz::physics::LinkFrameSemantics,
gz::physics::ShapeFrameSemantics
gz::physics::ShapeFrameSemantics,
gz::physics::ModelFrameSemantics
> { };

using KinematicFeaturesTestTypes =
Expand Down Expand Up @@ -206,7 +208,20 @@ TYPED_TEST(KinematicFeaturesTest, LinkFrameSemanticsPose)
auto linkCol = nonBaseLink->GetShape("link_collision");
ASSERT_NE(nullptr, linkCol);

auto nestedModel = model->GetNestedModel("nested_model");
ASSERT_NE(nullptr, nestedModel);
auto nestedLink = nestedModel->GetLink("nested_link");
ASSERT_NE(nullptr, nestedLink);
auto nestedLinkCol = nestedLink->GetShape("nested_link_collision");
ASSERT_NE(nullptr, nestedLinkCol);

gz::math::Pose3d actualModelPose(1, 0, 0, 0, 0, 0);
auto modelFrameData = model->FrameDataRelativeToWorld();
if (this->PhysicsEngineName(name) == "bullet-featherstone")
{
EXPECT_EQ(actualModelPose,
gz::math::eigen3::convert(modelFrameData.pose));
}
auto baseLinkFrameData = baseLink->FrameDataRelativeToWorld();
auto baseLinkPose = gz::math::eigen3::convert(baseLinkFrameData.pose);
gz::math::Pose3d actualLinkLocalPose(0, 1, 0, 0, 0, 0);
Expand Down Expand Up @@ -237,6 +252,33 @@ TYPED_TEST(KinematicFeaturesTest, LinkFrameSemanticsPose)
EXPECT_EQ(expectedColWorldPose.Pos(), linkColPose.Pos());
EXPECT_EQ(expectedColWorldPose.Rot().Euler(),
linkColPose.Rot().Euler());

gz::math::Pose3d actualNestedModelLocalPose(0, 0, 1, 0, 0, 0);
if (this->PhysicsEngineName(name) == "bullet-featherstone")
{
auto nestedModelFrameData = nestedModel->FrameDataRelativeToWorld();
gz::math::Pose3d expectedNestedModelWorldPose =
actualModelPose * actualNestedModelLocalPose;
EXPECT_EQ(expectedNestedModelWorldPose,
gz::math::eigen3::convert(nestedModelFrameData.pose));
}

auto nestedLinkFrameData = nestedLink->FrameDataRelativeToWorld();
auto nestedLinkPose = gz::math::eigen3::convert(nestedLinkFrameData.pose);
gz::math::Pose3d actualNestedLinkLocalPose(0, 2, 0, 0, 0, 0);
gz::math::Pose3d expectedNestedLinkWorldPose =
actualModelPose * actualNestedModelLocalPose *
actualNestedLinkLocalPose;
EXPECT_EQ(expectedNestedLinkWorldPose, nestedLinkPose);

auto nestedLinkColFrameData = nestedLinkCol->FrameDataRelativeToWorld();
auto nestedLinkColPose =
gz::math::eigen3::convert(nestedLinkColFrameData.pose);
auto actualNestedColLocalPose = gz::math::Pose3d(-0.5, 0, 0, 0, 1.5708, 0);
auto expectedNestedColWorldPose =
actualModelPose * actualNestedModelLocalPose *
actualNestedLinkLocalPose * actualNestedColLocalPose;
EXPECT_EQ(expectedNestedColWorldPose, nestedLinkColPose);
}
}

Expand Down
31 changes: 31 additions & 0 deletions test/common_test/worlds/pose_offset.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
<link name="base">
<pose>0 1 0 0 0 0</pose>
<inertial>
<pose>0 0 3 0 0 0</pose>
<mass>100</mass>
</inertial>
<collision name="base_collision">
Expand Down Expand Up @@ -34,13 +35,43 @@
</geometry>
</collision>
</link>

<model name="nested_model">
<pose>0 0 1 0 0 0</pose>

<link name="nested_link">
<pose>0 2 0 0 0 0</pose>
<inertial>
<pose>0 0 0.5 0 0 0</pose>
</inertial>
<collision name="nested_link_collision">
<pose>-0.5 0 0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.3</length>
</cylinder>
</geometry>
</collision>
</link>
</model>

<joint name="joint" type="fixed">
<parent>base</parent>
<child>link</child>
<axis>
<xyz>1.0 0 0</xyz>
</axis>
</joint>

<joint name="nested_joint" type="fixed">
<parent>link</parent>
<child>nested_model::nested_link</child>
<axis>
<xyz>0.0 0 1</xyz>
</axis>
</joint>

</model>
</world>
</sdf>