From dc394e0b479acaa77e55b449983d8f6d5b73dc20 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 23 Feb 2024 22:07:41 +0000 Subject: [PATCH 1/3] Tune erp2 and mesh margin Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 7 +++++++ bullet-featherstone/src/SDFFeatures.cc | 2 +- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index 13dd6c8fa..e4a28cea7 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -42,6 +42,13 @@ WorldInfo::WorldInfo(std::string name_) // Needed for force-torque sensor this->world->getSolverInfo().m_jointFeedbackInJointFrame = true; this->world->getSolverInfo().m_jointFeedbackInWorldSpace = false; + + // By default a large impulse is is applied when collisions penetrate + // which causes unstable behavior. Bullet featherstone does not support + // configuring split impulse and penetration threhold parameters. Instead the + // penentration impulse depends on the erp2 parameter so set to a small value + // (default is 0.2). + this->world->getSolverInfo().m_erp2 = 0.002; } } // namespace bullet_featherstone diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index da528ae35..c3d80f2d7 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -775,7 +775,7 @@ bool SDFFeatures::AddSdfCollision( std::make_unique( this->triangleMeshes.back().get())); this->meshesGImpact.back()->updateBound(); - this->meshesGImpact.back()->setMargin(btScalar(0.001)); + this->meshesGImpact.back()->setMargin(btScalar(0.01)); compoundShape->addChildShape(btTransform::getIdentity(), this->meshesGImpact.back().get()); } From bae42771f496529ee68c6b712606f72308eea99e Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 24 Feb 2024 02:44:18 +0000 Subject: [PATCH 2/3] increase tol in joint features test Signed-off-by: Ian Chen --- test/common_test/joint_features.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 5f0a695b6..9bdfe0ef6 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1138,7 +1138,7 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetach) } frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); - EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 1e-3); + EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 2e-3); } } @@ -1266,9 +1266,9 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); gz::math::Vector3d body3LinearVelocity = gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); - EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 3e-3); EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); - EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 3e-3); } // Detach the joints. M1 and M3 should fall as there is now nothing stopping @@ -1296,9 +1296,9 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) gz::math::Vector3d body3LinearVelocity = gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); - EXPECT_NEAR(dt * (numSteps) * -10, body1LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(dt * (numSteps) * -10, body1LinearVelocity.Z(), 3e-3); EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); - EXPECT_NEAR(dt * (numSteps) * -10, body3LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(dt * (numSteps) * -10, body3LinearVelocity.Z(), 3e-3); } } } From 8ea3e67e90177a28a5e6ed31da02dbec7bb93415 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 2 Mar 2024 02:09:46 +0000 Subject: [PATCH 3/3] update test Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 4 +- test/common_test/joint_features.cc | 62 ++++++++++++++++++------------ 2 files changed, 40 insertions(+), 26 deletions(-) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index e4a28cea7..d157e33d8 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -43,9 +43,9 @@ WorldInfo::WorldInfo(std::string name_) this->world->getSolverInfo().m_jointFeedbackInJointFrame = true; this->world->getSolverInfo().m_jointFeedbackInWorldSpace = false; - // By default a large impulse is is applied when collisions penetrate + // By default a large impulse is applied when collisions penetrate // which causes unstable behavior. Bullet featherstone does not support - // configuring split impulse and penetration threhold parameters. Instead the + // configuring split impulse and penetration threshold parameters. Instead the // penentration impulse depends on the erp2 parameter so set to a small value // (default is 0.2). this->world->getSolverInfo().m_erp2 = 0.002; diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 9bdfe0ef6..646cf5e09 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1132,13 +1132,22 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetach) } // After a while, body2 should reach the ground and come to a stop - for (std::size_t i = 0; i < 1000; ++i) + std::size_t stepCount = 0u; + const std::size_t maxNumSteps = 1000u; + while (stepCount++ < maxNumSteps) { world->Step(output, state, input); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + // Expected Z height of model2 is 0.75 when both boxes are stacked on top + // of each other since each is 0.5 high. + if (fabs(frameDataModel2Body.pose.translation().z() - 0.75) < 2e-2 && + fabs(frameDataModel2Body.linearVelocity.z()) < 1e-3) + { + break; + } } - frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); - - EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 2e-3); + EXPECT_GT(stepCount, 1u); + EXPECT_LT(stepCount, maxNumSteps); } } @@ -1246,8 +1255,8 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) EXPECT_EQ(initialModel3Pose, gz::math::eigen3::convert(frameDataModel3Body.pose)); + // Step through initial transients const std::size_t numSteps = 100; - /// Step through initial transients for (std::size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); @@ -1260,15 +1269,18 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) // Expect all the bodies to be at rest. // (since they're held in place by the joints) - gz::math::Vector3d body1LinearVelocity = - gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); - gz::math::Vector3d body2LinearVelocity = - gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); - gz::math::Vector3d body3LinearVelocity = - gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); - EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 3e-3); - EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); - EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 3e-3); + { + world->Step(output, state, input); + EXPECT_NEAR(initialModel1Pose.Z(), + frameDataModel1Body.pose.translation().z(), 1e-3); + EXPECT_NEAR(initialModel2Pose.Z(), + frameDataModel2Body.pose.translation().z(), 1e-3); + EXPECT_NEAR(initialModel3Pose.Z(), + frameDataModel3Body.pose.translation().z(), 1e-3); + EXPECT_NEAR(0.0, frameDataModel1Body.linearVelocity.z(), 3e-3); + EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 1e-3); + EXPECT_NEAR(0.0, frameDataModel3Body.linearVelocity.z(), 3e-3); + } } // Detach the joints. M1 and M3 should fall as there is now nothing stopping @@ -1276,6 +1288,13 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) fixedJoint_m2m1->Detach(); fixedJoint_m2m3->Detach(); + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + const double preStepBody1LinearVelocityZ = + frameDataModel1Body.linearVelocity.z(); + const double preStepBody3LinearVelocityZ = + frameDataModel3Body.linearVelocity.z(); + /// Step through initial transients for (std::size_t i = 0; i < numSteps; ++i) { @@ -1289,16 +1308,11 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) // Expect the middle box to be still as it is already at rest. // Expect the two side boxes to fall away. - gz::math::Vector3d body1LinearVelocity = - gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); - gz::math::Vector3d body2LinearVelocity = - gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); - gz::math::Vector3d body3LinearVelocity = - gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); - - EXPECT_NEAR(dt * (numSteps) * -10, body1LinearVelocity.Z(), 3e-3); - EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3); - EXPECT_NEAR(dt * (numSteps) * -10, body3LinearVelocity.Z(), 3e-3); + EXPECT_NEAR(preStepBody1LinearVelocityZ + dt * (numSteps) * -10, + frameDataModel1Body.linearVelocity.z(), 1e-3); + EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 1e-3); + EXPECT_NEAR(preStepBody3LinearVelocityZ + dt * (numSteps) * -10, + frameDataModel3Body.linearVelocity.z(), 1e-3); } } }