Skip to content

Commit

Permalink
Fix collision detection issue when using joints across nested models (#…
Browse files Browse the repository at this point in the history 
…268)

When a joint is created between BodyNodes in different skeletons, the child BodyNode is moved to the skeleton of the parent BodyNode. When this happens, the BodyNode version of the child needs to be incremented. This is actually fixed by dartsim/dart#1445, but we don't have that merged into our fork. So in the meantime, we call incrementVersion after moveTo is called similar to #31.

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

Co-authored-by: Louise Poubel <louise@openrobotics.org>
  • Loading branch information
@azeey @chapulina
azeey and chapulina authored Jun 24, 2021
1 parent c6cdea6 commit 0860af0
Show file tree
Hide file tree
Showing 3 changed files with 184 additions and 0 deletions.
5 changes: 5 additions & 0 deletions dartsim/src/SDFFeatures.cc
View file
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
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
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>

0 comments on commit 0860af0

Please sign in to comment.