Mimic constraint feature using bullet-featherstone

[scpeters]

🎉 New feature

Targeted at #431

Summary

This adds support for SetMimicConstraintFeature in the bullet-featherstone plugin using btMultiBodyGearConstraint. The common test should be updated to use bullet-featherstone, and some error messages are missing.

Test it

Building with gazebosim/gz-sim#1838, load the gz-sim examples/worlds/mimic_pendulum_world.sdf and observe 3 pairs of pendulums with different lengths. The center pair of pendulums are uncoupled and oscillate at different frequencies. The left and right pairs have <mimic/> tags with multiplier == 1 with different directionality of follower and leader and in both cases the long and short pendula should oscillate at the same frequency, and the frequency should be in between that of the uncoupled long and short pendula.

Checklist

• Signed all commits for DCO
• Added tests
• Added example and/or tutorial
• Updated documentation (as needed)
• Updated migration guide (as needed)
• Consider updating Python bindings (if the library has them)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

[codecov]

Codecov Report

Merging #517 (dda49d8) into main (3383160) will increase coverage by 0.13%.
The diff coverage is 87.50%.

❗ Current head dda49d8 differs from pull request most recent head 1bb90c8. Consider uploading reports for the commit 1bb90c8 to get more accurate results

@@            Coverage Diff             @@
##             main     #517      +/-   ##
==========================================
+ Coverage   77.18%   77.31%   +0.13%     
==========================================
  Files         139      139              
  Lines        7601     7641      +40     
==========================================
+ Hits         5867     5908      +41     
+ Misses       1734     1733       -1     

Files Changed	Coverage Δ
bullet-featherstone/src/Base.hh	100.00% <ø> (ø)
bullet-featherstone/src/JointFeatures.cc	58.25% <86.11%> (+5.89%)	⬆️
include/gz/physics/detail/Joint.hh	100.00% <100.00%> (ø)

... and 1 file with indirect coverage changes

[scpeters]

I've retargeted this at gz-physics6 and redacted the dartsim implementation since it is not yet working. I suggest we close #431 but leave the branch in place as a reference until dartsim/dart#1756 is addressed

[scpeters]

the new mimic tests failed in a flaky way with SEH exceptions in the two most recent builds, and I've reported the flakiness of tests using bullet on windows in #520

I'll retrigger the build to see if it passes this time

[mjcarroll]

Window trace:

 	gz-physics6-bullet-featherstone-plugin.dll!btCollisionWorld::~btCollisionWorld(void)	C++
>	gz-physics6-bullet-featherstone-plugin.dll!btMultiBodyDynamicsWorld::`vector deleting destructor'(unsigned int)	C++
 	[Inline Frame] gz-physics6-bullet-featherstone-plugin.dll!std::default_delete<btMultiBodyDynamicsWorld>::operator()(btMultiBodyDynamicsWorld *) Line 3167	C++
 	[Inline Frame] gz-physics6-bullet-featherstone-plugin.dll!std::unique_ptr<btMultiBodyDynamicsWorld,std::default_delete<btMultiBodyDynamicsWorld>>::{dtor}() Line 3278	C++
 	gz-physics6-bullet-featherstone-plugin.dll!gz::physics::bullet_featherstone::WorldInfo::~WorldInfo()	C++
 	[Inline Frame] gz-physics6-bullet-featherstone-plugin.dll!std::_Ref_count_base::_Decref() Line 1171	C++
 	[Inline Frame] gz-physics6-bullet-featherstone-plugin.dll!std::_Ptr_base<gz::physics::bullet_featherstone::WorldInfo>::_Decref() Line 1396	C++
 	[Inline Frame] gz-physics6-bullet-featherstone-plugin.dll!std::shared_ptr<gz::physics::bullet_featherstone::WorldInfo>::{dtor}() Line 1680	C++
 	[Inline Frame] gz-physics6-bullet-featherstone-plugin.dll!std::_Default_allocator_traits<std::allocator<std::_List_node<std::pair<unsigned __int64 const ,std::shared_ptr<gz::physics::bullet_featherstone::WorldInfo>>,void *>>>::destroy(std::allocator<std::_List_node<std::pair<unsigned __int64 const ,std::shared_ptr<gz::physics::bullet_featherstone::WorldInfo>>,void *>> &) Line 686	C++
 	[Inline Frame] gz-physics6-bullet-featherstone-plugin.dll!std::_List_node<std::pair<unsigned __int64 const ,std::shared_ptr<gz::physics::bullet_featherstone::WorldInfo>>,void *>::_Freenode(std::allocator<std::_List_node<std::pair<unsigned __int64 const ,std::shared_ptr<gz::physics::bullet_featherstone::WorldInfo>>,void *>> &) Line 318	C++
 	gz-physics6-bullet-featherstone-plugin.dll!std::_List_node<std::pair<unsigned __int64 const ,std::shared_ptr<gz::physics::bullet_featherstone::WorldInfo>>,void *>::_Free_non_head<std::allocator<std::_List_node<std::pair<unsigned __int64 const ,std::shared_ptr<gz::physics::bullet_featherstone::WorldInfo>>,void *>>>(std::allocator<std::_List_node<std::pair<unsigned __int64 const ,std::shared_ptr<gz::physics::bullet_featherstone::WorldInfo>>,void *>> & _Head, std::_List_node<std::pair<unsigned __int64 const ,std::shared_ptr<gz::physics::bullet_featherstone::WorldInfo>>,void *> *) Line 330	C++
 	[Inline Frame] gz-physics6-bullet-featherstone-plugin.dll!std::list<std::pair<unsigned __int64 const ,std::shared_ptr<gz::physics::bullet_featherstone::WorldInfo>>,std::allocator<std::pair<unsigned __int64 const ,std::shared_ptr<gz::physics::bullet_featherstone::WorldInfo>>>>::_Tidy() Line 1498	C++
 	[Inline Frame] gz-physics6-bullet-featherstone-plugin.dll!std::list<std::pair<unsigned __int64 const ,std::shared_ptr<gz::physics::bullet_featherstone::WorldInfo>>,std::allocator<std::pair<unsigned __int64 const ,std::shared_ptr<gz::physics::bullet_featherstone::WorldInfo>>>>::{dtor}() Line 1043	C++
 	gz-physics6-bullet-featherstone-plugin.dll!std::_Hash<std::_Umap_traits<unsigned __int64,std::shared_ptr<gz::physics::bullet_featherstone::WorldInfo>,std::_Uhash_compare<unsigned __int64,std::hash<unsigned __int64>,std::equal_to<unsigned __int64>>,std::allocator<std::pair<unsigned __int64 const ,std::shared_ptr<gz::physics::bullet_featherstone::WorldInfo>>>,0>>::~_Hash<std::_Umap_traits<unsigned __int64,std::shared_ptr<gz::physics::bullet_featherstone::WorldInfo>,std::_Uhash_compare<unsigned __int64,std::hash<unsigned __int64>,std::equal_to<unsigned __int64>>,std::allocator<std::pair<unsigned __int64 const ,std::shared_ptr<gz::physics::bullet_featherstone::WorldInfo>>>,0>>()	C++
 	gz-physics6-bullet-featherstone-plugin.dll!gz::physics::bullet_featherstone::Plugin::`scalar deleting destructor'(unsigned int)	C++
 	[Inline Frame] gz-plugin2.dll!std::_Func_class<void,void *>::operator()(void * <_Args_0>) Line 861	C++
 	[Inline Frame] gz-plugin2.dll!gz::plugin::PluginWithDlHandle::{dtor}() Line 64	C++
 	[Inline Frame] gz-plugin2.dll!std::_Destroy_in_place(gz::plugin::PluginWithDlHandle &) Line 298	C++
 	gz-plugin2.dll!std::_Ref_count_obj2<gz::plugin::PluginWithDlHandle>::_Destroy() Line 2110	C++
 	[Inline Frame] gz-plugin2.dll!std::_Ref_count_base::_Decref() Line 1171	C++
 	[Inline Frame] gz-plugin2.dll!std::_Ptr_base<void>::_Decref() Line 1396	C++
 	[Inline Frame] gz-plugin2.dll!std::shared_ptr<void>::{dtor}() Line 1680	C++
 	[Inline Frame] gz-plugin2.dll!std::default_delete<gz::plugin::Plugin::Implementation>::operator()(gz::plugin::Plugin::Implementation *) Line 3167	C++
 	gz-plugin2.dll!std::unique_ptr<gz::plugin::Plugin::Implementation,std::default_delete<gz::plugin::Plugin::Implementation>>::~unique_ptr<gz::plugin::Plugin::Implementation,std::default_delete<gz::plugin::Plugin::Implementation>>() Line 3278	C++
 	gz-plugin2-loader.dll!gz::plugin::Plugin::`scalar deleting destructor'(unsigned int)	C++
 	[Inline Frame] COMMON_TEST_joint_mimic_features.exe!std::default_delete<gz::plugin::Plugin>::operator()(gz::plugin::Plugin *) Line 3167	C++
 	[Inline Frame] COMMON_TEST_joint_mimic_features.exe!std::unique_ptr<gz::plugin::Plugin,std::default_delete<gz::plugin::Plugin>>::{dtor}() Line 3278	C++
 	[Inline Frame] COMMON_TEST_joint_mimic_features.exe!gz::plugin::TemplatePluginPtr<gz::plugin::Plugin>::{dtor}() Line 57	C++
 	COMMON_TEST_joint_mimic_features.exe!JointMimicFeatureTest_PendulumsFastSlowMimicTest_Test::TestBody() Line 358	C++
 	COMMON_TEST_joint_mimic_features.exe!testing::internal::HandleSehExceptionsInMethodIfSupported<testing::Test,void>(testing::Test * object, void(testing::Test::*)() method, const char * location) Line 2584	C++
 	COMMON_TEST_joint_mimic_features.exe!testing::internal::HandleExceptionsInMethodIfSupported<testing::Test,void>(testing::Test * object, void(testing::Test::*)() method, const char * location) Line 2635	C++
 	COMMON_TEST_joint_mimic_features.exe!testing::Test::Run() Line 2681	C++
 	COMMON_TEST_joint_mimic_features.exe!testing::TestInfo::Run() Line 2856	C++
 	COMMON_TEST_joint_mimic_features.exe!testing::TestSuite::Run() Line 3014	C++
 	COMMON_TEST_joint_mimic_features.exe!testing::internal::UnitTestImpl::RunAllTests() Line 5871	C++
 	COMMON_TEST_joint_mimic_features.exe!testing::internal::HandleSehExceptionsInMethodIfSupported<testing::internal::UnitTestImpl,bool>(testing::internal::UnitTestImpl * object, bool(testing::internal::UnitTestImpl::*)()) Line 2584	C++
 	COMMON_TEST_joint_mimic_features.exe!testing::internal::HandleExceptionsInMethodIfSupported<testing::internal::UnitTestImpl,bool>(testing::internal::UnitTestImpl * object, bool(testing::internal::UnitTestImpl::*)()) Line 2635	C++
 	COMMON_TEST_joint_mimic_features.exe!testing::UnitTest::Run() Line 5444	C++
 	[External Code]	

[scpeters]

retargeted to harmonic

[scpeters]

this doesn't actually depend on sdformat for the mimic functionality, since it's not part of the sdf component, so I didn't have to rerun CI after merging gazebosim/sdformat#1166

[azeey]

Looks great! Mostly minor comments. My main concern is with the API taking strings instead of Joint objects and DOF values.

[scpeters]

Looks great! Mostly minor comments. My main concern is with the API taking strings instead of Joint objects and DOF values.

thanks for the feedback; I've updated the API locally and am going to update gz-sim before pushing

[scpeters]

I believe I've responded to your review comments

[scpeters]

there are some windows test failures, but I'm going to merge, since they are hard to track down