diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 62137286e..da0f01d11 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -600,6 +600,11 @@ Identity SDFFeatures::ConstructSdfLink( bodyProperties.mInertia.setLocalCOM(localCom); + // ignmsg << "SDF gravity status = " << _sdfLink.EnableGravity() << std::endl; + + bodyProperties.mGravityMode = _sdfLink.EnableGravity(); + // ignmsg << "gravity in dart set to: " << bodyProperties.mGravityMode << std::endl; + 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 diff --git a/dartsim/src/WorldFeatures_TEST.cc b/dartsim/src/WorldFeatures_TEST.cc index 0126e2621..25ef17243 100644 --- a/dartsim/src/WorldFeatures_TEST.cc +++ b/dartsim/src/WorldFeatures_TEST.cc @@ -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); { @@ -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), + pos); } } diff --git a/dartsim/worlds/falling.world b/dartsim/worlds/falling.world index 3bae3f366..881daa5b9 100644 --- a/dartsim/worlds/falling.world +++ b/dartsim/worlds/falling.world @@ -71,5 +71,17 @@ + + + false + 10 10 10 0 0 0 + + 1 + + + 1 + + +