From ee80426a63737ae6c7a363789590468fb73add6e Mon Sep 17 00:00:00 2001 From: youhy Date: Tue, 7 May 2024 23:44:45 -0500 Subject: [PATCH 1/3] add no gravity link support and test for DartSim Signed-off-by: youhy --- dartsim/src/SDFFeatures.cc | 5 +++++ dartsim/src/WorldFeatures_TEST.cc | 12 ++++++++++++ dartsim/worlds/falling.world | 12 ++++++++++++ 3 files changed, 29 insertions(+) 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 + + + From 5ac9d9dfcc97ed0231ac2b71780eca03c3fd9628 Mon Sep 17 00:00:00 2001 From: youhy Date: Thu, 9 May 2024 04:36:31 -0500 Subject: [PATCH 2/3] remove debug code Signed-off-by: youhy --- dartsim/src/SDFFeatures.cc | 3 --- 1 file changed, 3 deletions(-) diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index da0f01d11..27b8be036 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -600,10 +600,7 @@ 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"; From 5f4c03345b07f5b29a3be134026fde0045dc4c63 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 7 Jun 2024 09:09:45 +0000 Subject: [PATCH 3/3] fix test Signed-off-by: Ian Chen --- dartsim/src/WorldFeatures_TEST.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dartsim/src/WorldFeatures_TEST.cc b/dartsim/src/WorldFeatures_TEST.cc index 25ef17243..24a094865 100644 --- a/dartsim/src/WorldFeatures_TEST.cc +++ b/dartsim/src/WorldFeatures_TEST.cc @@ -201,7 +201,7 @@ TEST_F(WorldFeaturesFixture, Gravity) .pose.translation(); EXPECT_PRED_FORMAT2(vectorPredicate3, Eigen::Vector3d(10, 10, 10), - pos); + posNoGravity); } }