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

[dartsim] Fix collision detection issue when using joints across nested models #268

Merged
merged 2 commits into from
Jun 24, 2021
Merged

[dartsim] Fix collision detection issue when using joints across nested models #268

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a92487d
Fix collision detection issue when using joints across nested models
azeey Jun 21, 2021
472ba25
Merge branch 'ign-physics4' into fix_joint_across_models
chapulina Jun 24, 2021
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
5 changes: 5 additions & 0 deletions dartsim/src/SDFFeatures.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1140,6 +1140,11 @@ Identity SDFFeatures::ConstructSdfJoint(
joint->setTransformFromParentBodyNode(parent_T_prejoint_final);

const std::size_t jointID = this->AddJoint(joint);
// Increment BodyNode version since the child could be moved to a new skeleton
// when a joint is created.
// TODO(azeey) Remove incrementVersion once DART has been updated to
// internally increment the BodyNode's version after Joint::moveTo.
_child->incrementVersion();

return this->GenerateIdentity(jointID, this->joints.at(jointID));
}
Expand Down
52 changes: 52 additions & 0 deletions dartsim/src/SDFFeatures_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@

#include <ignition/plugin/Loader.hh>

#include <ignition/physics/FrameSemantics.hh>
#include <ignition/physics/GetEntities.hh>
#include <ignition/physics/Joint.hh>
#include <ignition/physics/RequestEngine.hh>
Expand Down Expand Up @@ -57,6 +58,7 @@ struct TestFeatureList : ignition::physics::FeatureList<
ignition::physics::GetEntities,
ignition::physics::GetBasicJointState,
ignition::physics::SetBasicJointState,
ignition::physics::LinkFrameSemantics,
ignition::physics::dartsim::RetrieveWorld,
ignition::physics::sdf::ConstructSdfCollision,
ignition::physics::sdf::ConstructSdfJoint,
Expand Down Expand Up @@ -661,6 +663,56 @@ TEST_P(SDFFeatures_TEST, FallbackToFixedJoint)
}
}

/////////////////////////////////////////////////
// Check that joints between links in different models work as expected
TEST_P(SDFFeatures_TEST, JointsAcrossNestedModels)
{
WorldPtr world = this->LoadWorld(
TEST_WORLD_DIR "/joint_across_nested_models.sdf");
ASSERT_NE(nullptr, world);

dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld();
ASSERT_NE(nullptr, dartWorld);

auto checkModel = [&world](const std::string &_modelName){
SCOPED_TRACE("checkModel " + _modelName);
// check top level model
auto parentModel = world->GetModel(_modelName);
ASSERT_NE(nullptr, parentModel);

auto link1 = parentModel->GetLink("link1");
ASSERT_NE(nullptr, link1);

auto nestedModel = parentModel->GetNestedModel("nested_model");
ASSERT_NE(nullptr, nestedModel);

auto link2 = nestedModel->GetLink("link2");
ASSERT_NE(nullptr, link2);

Eigen::Vector3d link1Pos =
link1->FrameDataRelativeToWorld().pose.translation();
Eigen::Vector3d link2Pos =
link2->FrameDataRelativeToWorld().pose.translation();
EXPECT_NEAR(0.25, link1Pos.z(), 1e-6);
EXPECT_NEAR(0.25, link2Pos.z(), 1e-6);
};

{
SCOPED_TRACE("Before step");
checkModel("M1");
checkModel("M2");
}
for (int i = 0; i < 1000; ++i)
{
dartWorld->step();
}
{
SCOPED_TRACE("After step");
checkModel("M1");
checkModel("M2");
}
}

/////////////////////////////////////////////////
class SDFFeatures_FrameSemantics: public SDFFeatures_TEST
{
Expand Down
127 changes: 127 additions & 0 deletions dartsim/worlds/joint_across_nested_models.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,127 @@
<?xml version="1.0"?>
<sdf version="1.8">
<world name="default">

<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</visual>
</link>
</model>

<model name="M1">
<pose>0 0 0.25 0 0.0 0</pose>
<!-- link1 doesn't have a collision so it would fall indefinitely if the
contraint created by joint j1 is not enforced -->
<link name="link1">
<visual name="box_visual">
<geometry>
<box>
<size>1 1 0.5</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>1 0 0 1</specular>
</material>
</visual>
</link>
<joint name="j1" type="fixed">
<parent>link1</parent>
<child>nested_model::link2</child>
</joint>

<model name="nested_model">
<pose>1 0 0.0 0 0.0 0</pose>
<link name="link2">
<collision name="box_collision">
<geometry>
<box>
<size>1 1 0.5</size>
</box>
</geometry>
</collision>

<visual name="box_visual">
<geometry>
<box>
<size>1 1 0.5</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>1 0 0 1</specular>
</material>
</visual>
</link>
</model>
</model>

<!-- This model is the same as M1 except the model pose and the parent and child elements of joint j1-->
<model name="M2">
<pose>0 5 0.25 0 0.0 0</pose>
<!-- link1 doesn't have a collision so it would fall indefinitely if the
contraint created by joint j1 is not enforced -->
<link name="link1">
<visual name="box_visual">
<geometry>
<box>
<size>1 1 0.5</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>1 0 0 1</specular>
</material>
</visual>
</link>
<joint name="j1" type="fixed">
<parent>nested_model::link2</parent>
<child>link1</child>
</joint>

<model name="nested_model">
<pose>1.0 0 0.0 0 0.0 0</pose>
<link name="link2">
<collision name="box_collision">
<geometry>
<box>
<size>1 1 0.5</size>
</box>
</geometry>
</collision>

<visual name="box_visual">
<geometry>
<box>
<size>1 1 0.5</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>1 0 0 1</specular>
</material>
</visual>
</link>
</model>
</model>
</world>
</sdf>