Skip to content

Commit

Permalink
Add no gravity link support (#633)
Browse files Browse the repository at this point in the history
* add no gravity link support and test for DartSim

Signed-off-by: youhy <[email protected]>

* remove debug code

Signed-off-by: youhy <[email protected]>

* fix test

Signed-off-by: Ian Chen <[email protected]>

---------

Signed-off-by: youhy <[email protected]>
Signed-off-by: Ian Chen <[email protected]>
Co-authored-by: Ian Chen <[email protected]>
  • Loading branch information
AzulRadio and iche033 authored Jun 7, 2024
1 parent 9a29866 commit 78e87e4
Show file tree
Hide file tree
Showing 3 changed files with 26 additions and 0 deletions.
2 changes: 2 additions & 0 deletions dartsim/src/SDFFeatures.cc
Original file line number Diff line number Diff line change
Expand Up @@ -600,6 +600,8 @@ Identity SDFFeatures::ConstructSdfLink(

bodyProperties.mInertia.setLocalCOM(localCom);

bodyProperties.mGravityMode = _sdfLink.EnableGravity();

dart::dynamics::FreeJoint::Properties jointProperties;
jointProperties.mName = bodyProperties.mName + "_FreeJoint";
// TODO(MXG): Consider adding a UUID to this joint name in order to avoid any
Expand Down
12 changes: 12 additions & 0 deletions dartsim/src/WorldFeatures_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -140,6 +140,12 @@ TEST_F(WorldFeaturesFixture, Gravity)
auto link = model->GetLink(0);
ASSERT_NE(nullptr, link);

auto modelNoGravity = world->GetModel("sphere_no_gravity");
ASSERT_NE(nullptr, modelNoGravity);

auto linkNoGravity = modelNoGravity->GetLink(0);
ASSERT_NE(nullptr, linkNoGravity);

// initial link pose
const Eigen::Vector3d initialLinkPosition(0, 0, 2);
{
Expand Down Expand Up @@ -190,6 +196,12 @@ TEST_F(WorldFeaturesFixture, Gravity)
EXPECT_PRED_FORMAT2(vectorPredicate3,
Eigen::Vector3d(0.5, 0, 2.5),
pos);
// pose for link without gravity should not change
Eigen::Vector3d posNoGravity = linkNoGravity->FrameDataRelativeToWorld()
.pose.translation();
EXPECT_PRED_FORMAT2(vectorPredicate3,
Eigen::Vector3d(10, 10, 10),
posNoGravity);
}
}

Expand Down
12 changes: 12 additions & 0 deletions dartsim/worlds/falling.world
Original file line number Diff line number Diff line change
Expand Up @@ -71,5 +71,17 @@
</visual>
</link>
</model>
<model name='sphere_no_gravity'>
<link name='link'>
<gravity>false</gravity>
<pose>10 10 10 0 0 0</pose>
<visual name='visual'>
<geometry><sphere><radius>1</radius></sphere></geometry>
</visual>
<collision name='collision'>
<geometry><sphere><radius>1</radius></sphere></geometry>
</collision>
</link>
</model>
</world>
</sdf>

0 comments on commit 78e87e4

Please sign in to comment.