diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 401fe9a5d..1a1fc2ed4 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -573,6 +573,16 @@ Identity SDFFeatures::ConstructSdfCollision( math::Vector3d fdir1 = odeFriction->Get("fdir1"); aspect->setFirstFrictionDirection(math::eigen3::convert(fdir1)); } + + const auto &surfaceBounce = _collision.Element() + ->GetElement("surface") + ->GetElement("bounce"); + + if (surfaceBounce->HasElement("restitution_coefficient")) + { + aspect->setRestitutionCoeff( + surfaceBounce->Get("restitution_coefficient")); + } #else // We are setting the friction coefficient of a collision element // to be the coefficient for the whole link. If there are multiple collision @@ -580,6 +590,15 @@ Identity SDFFeatures::ConstructSdfCollision( // TODO(addisu) Assign the coefficient to the shape node when support is // added in DART. bn->setFrictionCoeff(odeFriction->Get("mu")); + const auto &surfaceBounce = _collision.Element() + ->GetElement("surface") + ->GetElement("bounce"); + + if (surfaceBounce->HasElement("restitution_coefficient")) + { + bn->setRestitutionCoeff( + surfaceBounce->Get("restitution_coefficient")); + } #endif // TODO(anyone) add category_bitmask as well const auto bitmaskElement = _collision.Element() diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index a177d752a..58dbac8a6 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -158,20 +158,30 @@ TEST(SDFFeatures_TEST, CheckDartsimData) EXPECT_DOUBLE_EQ(0.0, axis[2]); } - const dart::dynamics::SkeletonPtr freeBody = + { + const dart::dynamics::SkeletonPtr freeBody = dartWorld->getSkeleton("free_body"); - ASSERT_NE(nullptr, freeBody); - ASSERT_EQ(1u, freeBody->getNumBodyNodes()); - const dart::dynamics::BodyNode *bn = freeBody->getBodyNode(0); - ASSERT_NE(nullptr, bn); - - EXPECT_TRUE(dynamic_cast( - bn->getParentJoint())); - - const Eigen::Vector3d translation = bn->getTransform().translation(); - EXPECT_DOUBLE_EQ(0.0, translation[0]); - EXPECT_DOUBLE_EQ(10.0, translation[1]); - EXPECT_DOUBLE_EQ(10.0, translation[2]); + ASSERT_NE(nullptr, freeBody); + ASSERT_EQ(1u, freeBody->getNumBodyNodes()); + const dart::dynamics::BodyNode *bn = freeBody->getBodyNode(0); + ASSERT_NE(nullptr, bn); + + EXPECT_TRUE(dynamic_cast( + bn->getParentJoint())); + + const Eigen::Vector3d translation = bn->getTransform().translation(); + EXPECT_DOUBLE_EQ(0.0, translation[0]); + EXPECT_DOUBLE_EQ(10.0, translation[1]); + EXPECT_DOUBLE_EQ(10.0, translation[2]); + +#if DART_VERSION_AT_LEAST(6, 10, 0) + const dart::dynamics::ShapeNode *collision1 = bn->getShapeNode(0); + auto aspect = collision1->getDynamicsAspect(); + EXPECT_DOUBLE_EQ(0.8, aspect->getRestitutionCoeff()); +#else + EXPECT_DOUBLE_EQ(0.8, bn->getRestitutionCoeff()); + #endif + } const dart::dynamics::SkeletonPtr screwJointTest = dartWorld->getSkeleton("screw_joint_test"); diff --git a/dartsim/worlds/test.world b/dartsim/worlds/test.world index 7f5282ed6..17bc75ab4 100644 --- a/dartsim/worlds/test.world +++ b/dartsim/worlds/test.world @@ -246,6 +246,18 @@ 0.0 10.0 10.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 + + + + 1.0 + + + + + 0.8 + + +