From 5080bedfec48e1ce29d930f717263c1b1cd8b639 Mon Sep 17 00:00:00 2001 From: Aditya Date: Thu, 29 Sep 2022 14:57:44 -0700 Subject: [PATCH 01/21] Skeletal code for SetMimicJointFeature Signed-off-by: Aditya --- dartsim/src/JointFeatures.cc | 11 ++++++++++ dartsim/src/JointFeatures.hh | 11 +++++++++- include/gz/physics/Joint.hh | 32 ++++++++++++++++++++++++++++++ include/gz/physics/detail/Joint.hh | 12 +++++++++++ 4 files changed, 65 insertions(+), 1 deletion(-) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index c25655763..12a5d231b 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -696,6 +696,17 @@ Wrench3d JointFeatures::GetJointTransmittedWrenchInJointFrame( wrenchOut.force = transmittedWrenchInJoint.tail<3>(); return wrenchOut; } + +///////////////////////////////////////////////// +void JointFeatures::SetJointMimicConstraint( + const Identity &_id, std::size_t &_dof, + std::string &_joint, std::string &_axis, + const double _multiplier, + const double _offset) +{ +} + + } } } diff --git a/dartsim/src/JointFeatures.hh b/dartsim/src/JointFeatures.hh index 62201d765..42e899694 100644 --- a/dartsim/src/JointFeatures.hh +++ b/dartsim/src/JointFeatures.hh @@ -56,7 +56,9 @@ struct JointFeatureList : FeatureList< SetJointPositionLimitsFeature, SetJointVelocityLimitsFeature, SetJointEffortLimitsFeature, - GetJointTransmittedWrench + GetJointTransmittedWrench, + + SetMimicConstraintFeature > { }; class JointFeatures : @@ -203,6 +205,13 @@ class JointFeatures : // ----- Transmitted wrench ----- public: Wrench3d GetJointTransmittedWrenchInJointFrame( const Identity &_id) const override; + + // ----- Mimic joint constraint ----- + public: void SetJointMimicConstraint( + const Identity &_id, std::size_t &_dof, + std::string &_joint, std::string &_axis, + const double _multiplier, + const double _offset) override; }; } diff --git a/include/gz/physics/Joint.hh b/include/gz/physics/Joint.hh index c8d819e26..7c295223f 100644 --- a/include/gz/physics/Joint.hh +++ b/include/gz/physics/Joint.hh @@ -584,6 +584,38 @@ namespace gz }; }; + ///////////////////////////////////////////////// + /// \brief This feature sets the mimic constraint for this Joint. + class GZ_PHYSICS_VISIBLE SetMimicConstraintFeature + : public virtual Feature + { + /// \brief The Joint API for setting the mimic joint constraint. + public: template + class Joint : public virtual Feature::Joint + { + public: using Scalar = typename PolicyT::Scalar; + + public: void SetMimicConstraint( + const std::size_t &_dof, std::string &_joint, + std::string &_axis, Scalar _multiplier, + Scalar _offset); + }; + + /// \private The implementation API for setting the mimic constraint. + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Scalar = typename PolicyT::Scalar; + + // See Joint::MimicConstraint above + public: virtual void SetJointMimicConstraint( + const Identity &_id, std::size_t &_dof, + std::string &_joint, std::string &_axis, + Scalar _multiplier, Scalar _offset) = 0; + }; + }; + + } } diff --git a/include/gz/physics/detail/Joint.hh b/include/gz/physics/detail/Joint.hh index 8fc762bc1..2b4d8ef8d 100644 --- a/include/gz/physics/detail/Joint.hh +++ b/include/gz/physics/detail/Joint.hh @@ -214,6 +214,18 @@ namespace gz ->SetJointMaxEffort(this->identity, _dof, _value); } + ///////////////////////////////////////////////// + template + void SetMimicConstraintFeature::Joint:: + SetMimicConstraint( + const std::size_t &_dof, std::string &_joint, + std::string &_axis, Scalar _multiplier, Scalar _offset) + { + this->template Interface() + ->SetJointMimicConstraint(this->identity, _dof, _joint, + _axis, _multiplier, _offset); + } + ///////////////////////////////////////////////// template void DetachJointFeature::Joint::Detach() From 6584c4cacff577f5f9d033c81e08410c76d4114d Mon Sep 17 00:00:00 2001 From: Aditya Date: Thu, 29 Sep 2022 16:37:19 -0700 Subject: [PATCH 02/21] Added implementation, test to be written Signed-off-by: Aditya --- dartsim/src/JointFeatures.cc | 26 ++++++++++++++++++++++++-- dartsim/src/JointFeatures.hh | 2 +- include/gz/physics/Joint.hh | 4 ++-- include/gz/physics/detail/Joint.hh | 4 ++-- 4 files changed, 29 insertions(+), 7 deletions(-) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index 12a5d231b..df12404ca 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -700,12 +700,34 @@ Wrench3d JointFeatures::GetJointTransmittedWrenchInJointFrame( ///////////////////////////////////////////////// void JointFeatures::SetJointMimicConstraint( const Identity &_id, std::size_t &_dof, - std::string &_joint, std::string &_axis, + const Identity &_idMimicJoint, std::string &_axis, const double _multiplier, const double _offset) { -} + auto joint = this->ReferenceInterface(_id)->joint; + auto jointMimic = this->ReferenceInterface(_idMimicJoint)->joint; + // Take extra care that the value is valid. A nan can cause the DART + // constraint solver to fail, which will in turn either cause a crash or + // collisions to fail + if (std::isnan(_multiplier)) + { + gzerr << "Invalid value found in multiplier [" << _multiplier + << "] for joint [" << joint->getName() << " DOF " << _dof + << "]. The command will be ignored\n"; + return; + } + if (std::isnan(_offset)) + { + gzerr << "Invalid value found in offset [" << _offset + << "] for joint [" << joint->getName() << " DOF " << _dof + << "]. The command will be ignored\n"; + return; + } + + joint->setActuatorType(dart::dynamics::Joint::MIMIC); + joint->setMimicJoint(jointMimic, _multiplier, _offset); +} } } diff --git a/dartsim/src/JointFeatures.hh b/dartsim/src/JointFeatures.hh index 42e899694..0e16427bd 100644 --- a/dartsim/src/JointFeatures.hh +++ b/dartsim/src/JointFeatures.hh @@ -209,7 +209,7 @@ class JointFeatures : // ----- Mimic joint constraint ----- public: void SetJointMimicConstraint( const Identity &_id, std::size_t &_dof, - std::string &_joint, std::string &_axis, + const Identity &_idMimicJoint, std::string &_axis, const double _multiplier, const double _offset) override; }; diff --git a/include/gz/physics/Joint.hh b/include/gz/physics/Joint.hh index 7c295223f..854ae647c 100644 --- a/include/gz/physics/Joint.hh +++ b/include/gz/physics/Joint.hh @@ -596,7 +596,7 @@ namespace gz public: using Scalar = typename PolicyT::Scalar; public: void SetMimicConstraint( - const std::size_t &_dof, std::string &_joint, + const std::size_t &_dof, const Identity &_idMimicJoint, std::string &_axis, Scalar _multiplier, Scalar _offset); }; @@ -610,7 +610,7 @@ namespace gz // See Joint::MimicConstraint above public: virtual void SetJointMimicConstraint( const Identity &_id, std::size_t &_dof, - std::string &_joint, std::string &_axis, + const Identity &_idMimicJoint, std::string &_axis, Scalar _multiplier, Scalar _offset) = 0; }; }; diff --git a/include/gz/physics/detail/Joint.hh b/include/gz/physics/detail/Joint.hh index 2b4d8ef8d..c12651cb2 100644 --- a/include/gz/physics/detail/Joint.hh +++ b/include/gz/physics/detail/Joint.hh @@ -218,11 +218,11 @@ namespace gz template void SetMimicConstraintFeature::Joint:: SetMimicConstraint( - const std::size_t &_dof, std::string &_joint, + const std::size_t &_dof, const Identity &_idMimicJoint, std::string &_axis, Scalar _multiplier, Scalar _offset) { this->template Interface() - ->SetJointMimicConstraint(this->identity, _dof, _joint, + ->SetJointMimicConstraint(this->identity, _dof, _idMimicJoint, _axis, _multiplier, _offset); } From 0cf04bd81c920570741560a1a56ed32eeb44716c Mon Sep 17 00:00:00 2001 From: Aditya Date: Fri, 30 Sep 2022 01:57:35 -0700 Subject: [PATCH 03/21] Skeletal test case Signed-off-by: Aditya --- test/common_test/joint_features.cc | 106 +++++++++++++++++++++++++++++ 1 file changed, 106 insertions(+) diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 468bd548f..dfbdcf278 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1820,6 +1820,112 @@ TYPED_TEST(JointTransmittedWrenchFixture, ContactForces) wrenchAtMotorJointInJoint, 1e-4)); } +struct JointMimicFeatureList : gz::physics::FeatureList< + gz::physics::ForwardStep, + gz::physics::GetBasicJointProperties, + gz::physics::GetBasicJointState, + gz::physics::GetEngineInfo, + gz::physics::GetJointFromModel, + gz::physics::GetLinkFromModel, + gz::physics::GetModelFromWorld, + gz::physics::LinkFrameSemantics, + gz::physics::SetBasicJointState, + gz::physics::SetJointVelocityCommandFeature, + gz::physics::SetMimicConstraintFeature, + gz::physics::sdf::ConstructSdfWorld>{}; + +template +class JointMimicFeatureFixture : + public JointFeaturesTest{}; +using JointMimicFeatureTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(JointMimicFeatureFixture, + JointMimicFeatureTestTypes); + +TYPED_TEST(JointMimicFeatureFixture, JointMimicTest) +{ + for (const std::string &name : this->pluginNames) + { + if(this->PhysicsEngineName(name) != "dartsim") + { + GTEST_SKIP(); + } + + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "mimic_constraint.world")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + auto model = world->GetModel("double_pendulum_with_base"); + auto upperJoint = model->GetJoint("upper_joint"); + auto lowerJoint = model->GetJoint("lower_joint"); + + std::cout << upperJoint->GetPosition(0) << std::endl; + std::cout << lowerJoint->GetPosition(0) << std::endl; + + /* // Test joint velocity command */ + /* gz::physics::ForwardStep::Output output; */ + /* gz::physics::ForwardStep::State state; */ + /* gz::physics::ForwardStep::Input input; */ + + /* // Expect negative joint velocity after 1 step without joint command */ + /* world->Step(output, state, input); */ + /* EXPECT_LT(joint->GetVelocity(0), 0.0); */ + + /* auto base_link = model->GetLink("base"); */ + /* ASSERT_NE(nullptr, base_link); */ + + /* // Check that invalid velocity commands don't cause collisions to fail */ + /* for (std::size_t i = 0; i < 1000; ++i) */ + /* { */ + /* joint->SetForce(0, std::numeric_limits::quiet_NaN()); */ + /* // expect the position of the pendulum to stay above ground */ + /* world->Step(output, state, input); */ + /* auto frameData = base_link->FrameDataRelativeToWorld(); */ + /* EXPECT_NEAR(0.0, frameData.pose.translation().z(), 1e-3); */ + /* } */ + + /* joint->SetVelocityCommand(0, 1); */ + /* world->Step(output, state, input); */ + /* // Setting a velocity command changes the actuator type to SERVO */ + /* // EXPECT_EQ(dart::dynamics::Joint::SERVO, dartJoint->getActuatorType()); */ + + /* const std::size_t numSteps = 10; */ + /* for (std::size_t i = 0; i < numSteps; ++i) */ + /* { */ + /* // Call SetVelocityCommand before each step */ + /* joint->SetVelocityCommand(0, 1); */ + /* world->Step(output, state, input); */ + /* EXPECT_NEAR(1.0, joint->GetVelocity(0), 1e-6); */ + /* } */ + + /* for (std::size_t i = 0; i < numSteps; ++i) */ + /* { */ + /* // expect joint to freeze in subsequent steps without SetVelocityCommand */ + /* world->Step(output, state, input); */ + /* EXPECT_NEAR(0.0, joint->GetVelocity(0), 1e-6); */ + /* } */ + + /* // Check that invalid velocity commands don't cause collisions to fail */ + /* for (std::size_t i = 0; i < 1000; ++i) */ + /* { */ + /* joint->SetVelocityCommand(0, std::numeric_limits::quiet_NaN()); */ + /* // expect the position of the pendulum to stay aabove ground */ + /* world->Step(output, state, input); */ + /* auto frameData = base_link->FrameDataRelativeToWorld(); */ + /* EXPECT_NEAR(0.0, frameData.pose.translation().z(), 1e-3); */ + /* } */ + } +} + + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); From e862d376ef18dd150d3c5366916c1d4621e3d2af Mon Sep 17 00:00:00 2001 From: Aditya Date: Fri, 30 Sep 2022 02:00:12 -0700 Subject: [PATCH 04/21] Added world file Signed-off-by: Aditya --- .../common_test/worlds/mimic_constraint.world | 245 ++++++++++++++++++ 1 file changed, 245 insertions(+) create mode 100644 test/common_test/worlds/mimic_constraint.world diff --git a/test/common_test/worlds/mimic_constraint.world b/test/common_test/worlds/mimic_constraint.world new file mode 100644 index 000000000..a5f7006a1 --- /dev/null +++ b/test/common_test/worlds/mimic_constraint.world @@ -0,0 +1,245 @@ + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + false + + + 0 0 1 + 100 100 + + + + 0.9 0.9 0.9 1 + + + + + + 1 0 0 0 0 0 + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 1 1 0 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 1 1 0 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + 1.1 + + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 1 1 0 1 + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + 1 1 0 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 1 1 0 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + + 0.1 + + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + 0.25 1.0 2.1 -2 0 0 + 0 + + 0 0 0.5 0 0 0 + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + 1 1 0 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 1 1 0 1 + + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + 3.0 + + + + + + upper_link + lower_link + + 1.0 0 0 + + 1.0 + 0 + + + + + + + From 89baee9827a5829fbd1f52cb5304f7d36702735e Mon Sep 17 00:00:00 2001 From: Aditya Date: Fri, 30 Sep 2022 14:30:49 -0700 Subject: [PATCH 05/21] Added basic test Signed-off-by: Aditya --- dartsim/src/JointFeatures.cc | 10 +- dartsim/src/JointFeatures.hh | 4 +- include/gz/physics/Joint.hh | 15 +- include/gz/physics/detail/Joint.hh | 8 +- test/common_test/joint_features.cc | 107 ++++---- .../common_test/worlds/mimic_constraint.world | 245 ------------------ 6 files changed, 72 insertions(+), 317 deletions(-) delete mode 100644 test/common_test/worlds/mimic_constraint.world diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index df12404ca..ff8904876 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -699,13 +699,13 @@ Wrench3d JointFeatures::GetJointTransmittedWrenchInJointFrame( ///////////////////////////////////////////////// void JointFeatures::SetJointMimicConstraint( - const Identity &_id, std::size_t &_dof, - const Identity &_idMimicJoint, std::string &_axis, + const Identity &_id, + const std::string _mimicJoint, const double _multiplier, const double _offset) { auto joint = this->ReferenceInterface(_id)->joint; - auto jointMimic = this->ReferenceInterface(_idMimicJoint)->joint; + auto jointMimic = joint->getSkeleton()->getJoint(_mimicJoint); // Take extra care that the value is valid. A nan can cause the DART // constraint solver to fail, which will in turn either cause a crash or @@ -713,14 +713,14 @@ void JointFeatures::SetJointMimicConstraint( if (std::isnan(_multiplier)) { gzerr << "Invalid value found in multiplier [" << _multiplier - << "] for joint [" << joint->getName() << " DOF " << _dof + << "] for joint [" << joint->getName() << "]. The command will be ignored\n"; return; } if (std::isnan(_offset)) { gzerr << "Invalid value found in offset [" << _offset - << "] for joint [" << joint->getName() << " DOF " << _dof + << "] for joint [" << joint->getName() << "]. The command will be ignored\n"; return; } diff --git a/dartsim/src/JointFeatures.hh b/dartsim/src/JointFeatures.hh index 0e16427bd..19b573f3e 100644 --- a/dartsim/src/JointFeatures.hh +++ b/dartsim/src/JointFeatures.hh @@ -208,8 +208,8 @@ class JointFeatures : // ----- Mimic joint constraint ----- public: void SetJointMimicConstraint( - const Identity &_id, std::size_t &_dof, - const Identity &_idMimicJoint, std::string &_axis, + const Identity &_id, + const std::string _mimicJoint, const double _multiplier, const double _offset) override; }; diff --git a/include/gz/physics/Joint.hh b/include/gz/physics/Joint.hh index 854ae647c..065dc66a1 100644 --- a/include/gz/physics/Joint.hh +++ b/include/gz/physics/Joint.hh @@ -590,14 +590,21 @@ namespace gz : public virtual Feature { /// \brief The Joint API for setting the mimic joint constraint. + /// \param[in] _mimicJoint + /// name of the joint to be mimicked, i.e. the parent joint. + /// \param[in] _multiplier + /// The multiplier to be applied to poistion of parent joint. + /// \param[in] _offset + /// The offset to be applied to position of parent joint after + /// the multiplier is applied. public: template class Joint : public virtual Feature::Joint { public: using Scalar = typename PolicyT::Scalar; public: void SetMimicConstraint( - const std::size_t &_dof, const Identity &_idMimicJoint, - std::string &_axis, Scalar _multiplier, + const std::string _mimicJoint, + Scalar _multiplier, Scalar _offset); }; @@ -609,8 +616,8 @@ namespace gz // See Joint::MimicConstraint above public: virtual void SetJointMimicConstraint( - const Identity &_id, std::size_t &_dof, - const Identity &_idMimicJoint, std::string &_axis, + const Identity &_id, + const std::string _mimicJoint, Scalar _multiplier, Scalar _offset) = 0; }; }; diff --git a/include/gz/physics/detail/Joint.hh b/include/gz/physics/detail/Joint.hh index c12651cb2..10fd04be7 100644 --- a/include/gz/physics/detail/Joint.hh +++ b/include/gz/physics/detail/Joint.hh @@ -218,12 +218,12 @@ namespace gz template void SetMimicConstraintFeature::Joint:: SetMimicConstraint( - const std::size_t &_dof, const Identity &_idMimicJoint, - std::string &_axis, Scalar _multiplier, Scalar _offset) + const std::string _mimicJoint, + Scalar _multiplier, Scalar _offset) { this->template Interface() - ->SetJointMimicConstraint(this->identity, _dof, _idMimicJoint, - _axis, _multiplier, _offset); + ->SetJointMimicConstraint(this->identity, _mimicJoint, + _multiplier, _offset); } ///////////////////////////////////////////////// diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index dfbdcf278..4b7e195d7 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1854,11 +1854,11 @@ TYPED_TEST(JointMimicFeatureFixture, JointMimicTest) std::cout << "Testing plugin: " << name << std::endl; gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); - auto engine = gz::physics::RequestEngine3d::From(plugin); + auto engine = gz::physics::RequestEngine3d::From(plugin); ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "mimic_constraint.world")); + const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -1867,61 +1867,54 @@ TYPED_TEST(JointMimicFeatureFixture, JointMimicTest) auto upperJoint = model->GetJoint("upper_joint"); auto lowerJoint = model->GetJoint("lower_joint"); - std::cout << upperJoint->GetPosition(0) << std::endl; - std::cout << lowerJoint->GetPosition(0) << std::endl; - - /* // Test joint velocity command */ - /* gz::physics::ForwardStep::Output output; */ - /* gz::physics::ForwardStep::State state; */ - /* gz::physics::ForwardStep::Input input; */ - - /* // Expect negative joint velocity after 1 step without joint command */ - /* world->Step(output, state, input); */ - /* EXPECT_LT(joint->GetVelocity(0), 0.0); */ - - /* auto base_link = model->GetLink("base"); */ - /* ASSERT_NE(nullptr, base_link); */ - - /* // Check that invalid velocity commands don't cause collisions to fail */ - /* for (std::size_t i = 0; i < 1000; ++i) */ - /* { */ - /* joint->SetForce(0, std::numeric_limits::quiet_NaN()); */ - /* // expect the position of the pendulum to stay above ground */ - /* world->Step(output, state, input); */ - /* auto frameData = base_link->FrameDataRelativeToWorld(); */ - /* EXPECT_NEAR(0.0, frameData.pose.translation().z(), 1e-3); */ - /* } */ - - /* joint->SetVelocityCommand(0, 1); */ - /* world->Step(output, state, input); */ - /* // Setting a velocity command changes the actuator type to SERVO */ - /* // EXPECT_EQ(dart::dynamics::Joint::SERVO, dartJoint->getActuatorType()); */ - - /* const std::size_t numSteps = 10; */ - /* for (std::size_t i = 0; i < numSteps; ++i) */ - /* { */ - /* // Call SetVelocityCommand before each step */ - /* joint->SetVelocityCommand(0, 1); */ - /* world->Step(output, state, input); */ - /* EXPECT_NEAR(1.0, joint->GetVelocity(0), 1e-6); */ - /* } */ - - /* for (std::size_t i = 0; i < numSteps; ++i) */ - /* { */ - /* // expect joint to freeze in subsequent steps without SetVelocityCommand */ - /* world->Step(output, state, input); */ - /* EXPECT_NEAR(0.0, joint->GetVelocity(0), 1e-6); */ - /* } */ - - /* // Check that invalid velocity commands don't cause collisions to fail */ - /* for (std::size_t i = 0; i < 1000; ++i) */ - /* { */ - /* joint->SetVelocityCommand(0, std::numeric_limits::quiet_NaN()); */ - /* // expect the position of the pendulum to stay aabove ground */ - /* world->Step(output, state, input); */ - /* auto frameData = base_link->FrameDataRelativeToWorld(); */ - /* EXPECT_NEAR(0.0, frameData.pose.translation().z(), 1e-3); */ - /* } */ + // Ensure both joints start from zero angle. + EXPECT_EQ(upperJoint->GetPosition(0), 0); + EXPECT_EQ(lowerJoint->GetPosition(0), 0); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + // Case : Without mimic constraint + + // Let the simulation run without mimic constraint. + // The postions of joints should not be equal. + double upperJointPrevPos = 0; + for (int _ = 0; _ < 10; _++) + { + world->Step(output, state, input); + EXPECT_NE(upperJointPrevPos, lowerJoint->GetPosition(0)); + upperJointPrevPos = upperJoint->GetPosition(0); + } + + auto testMimicFcn = [&](double multiplier, double offset) + { + // Set mimic joint constraint. + lowerJoint->SetMimicConstraint("upper_joint", multiplier, offset); + // Reset positions and run a few iterations so the positions reach nontrivial values. + upperJoint->SetPosition(0, 0); + lowerJoint->SetPosition(0, 0); + for (int _ = 0; _ < 10; _++) + world->Step(output, state, input); + + // Lower joint's position should be equal to that of upper joint in previous timestep. + upperJointPrevPos = upperJoint->GetPosition(0); + for (int _ = 0; _ < 10; _++) + { + world->Step(output, state, input); + EXPECT_FLOAT_EQ(upperJointPrevPos, multiplier * lowerJoint->GetPosition(0) - offset); + upperJointPrevPos = upperJoint->GetPosition(0); + } + }; + + // Case : Mimic -> muliplier = 1, offset = 0 + testMimicFcn(1, 0); + + // Case : Mimic -> muliplier = -1, offset = 0 + testMimicFcn(-1, 0); + + // Case : Mimic -> muliplier = 2, offset = 0.5 + testMimicFcn(2, 0.5); } } diff --git a/test/common_test/worlds/mimic_constraint.world b/test/common_test/worlds/mimic_constraint.world deleted file mode 100644 index a5f7006a1..000000000 --- a/test/common_test/worlds/mimic_constraint.world +++ /dev/null @@ -1,245 +0,0 @@ - - - - - true - - - - - 0 0 1 - 100 100 - - - - - - 100 - 50 - - - - - - false - - - 0 0 1 - 100 100 - - - - 0.9 0.9 0.9 1 - - - - - - 1 0 0 0 0 0 - - - 100 - - - 0 0 0.01 0 0 0 - - - 0.8 - 0.02 - - - - 1 1 0 1 - - - - -0.275 0 1.1 0 0 0 - - - 0.2 0.2 2.2 - - - - 1 1 0 1 - - - - 0 0 0.01 0 0 0 - - - 0.8 - 0.02 - - - - - -0.275 0 1.1 0 0 0 - - - 0.2 0.2 2.2 - - - - - - 1.1 - - - - - - - - 0 0 2.1 -1.5708 0 0 - 0 - - 0 0 0.5 0 0 0 - - - -0.05 0 0 0 1.5708 0 - - - 0.1 - 0.3 - - - - 1 1 0 1 - - - - 0 0 1.0 0 1.5708 0 - - - 0.1 - 0.2 - - - - 1 1 0 1 - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - 1 1 0 1 - - - - -0.05 0 0 0 1.5708 0 - - - 0.1 - 0.3 - - - - - - 0.1 - - - - - - 0 0 1.0 0 1.5708 0 - - - 0.1 - 0.2 - - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - - - - 0.25 1.0 2.1 -2 0 0 - 0 - - 0 0 0.5 0 0 0 - - - 0 0 0 0 1.5708 0 - - - 0.08 - 0.3 - - - - 1 1 0 1 - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - 1 1 0 1 - - - - 0 0 0 0 1.5708 0 - - - 0.08 - 0.3 - - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - - - - base - upper_link - - 1.0 0 0 - - 3.0 - - - - - - upper_link - lower_link - - 1.0 0 0 - - 1.0 - 0 - - - - - - - From 845c07627065c576bc4f55e643f3f09f76b9e847 Mon Sep 17 00:00:00 2001 From: Aditya Date: Fri, 30 Sep 2022 14:52:11 -0700 Subject: [PATCH 06/21] Linter fix Signed-off-by: Aditya --- include/gz/physics/Joint.hh | 2 ++ include/gz/physics/detail/Joint.hh | 2 ++ 2 files changed, 4 insertions(+) diff --git a/include/gz/physics/Joint.hh b/include/gz/physics/Joint.hh index 065dc66a1..231d785a5 100644 --- a/include/gz/physics/Joint.hh +++ b/include/gz/physics/Joint.hh @@ -22,6 +22,8 @@ #include #include +#include + namespace gz { namespace physics diff --git a/include/gz/physics/detail/Joint.hh b/include/gz/physics/detail/Joint.hh index 10fd04be7..7cfcd55fc 100644 --- a/include/gz/physics/detail/Joint.hh +++ b/include/gz/physics/detail/Joint.hh @@ -21,6 +21,8 @@ #include #include "gz/physics/detail/FrameSemantics.hh" +#include + namespace gz { namespace physics From ab64ba492abd5886650ffbf8793989b773e8ca56 Mon Sep 17 00:00:00 2001 From: Aditya Date: Mon, 3 Oct 2022 18:49:51 -0700 Subject: [PATCH 07/21] Fix tests Signed-off-by: Aditya --- test/common_test/joint_features.cc | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 4b7e195d7..93e5902f9 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1902,18 +1902,17 @@ TYPED_TEST(JointMimicFeatureFixture, JointMimicTest) for (int _ = 0; _ < 10; _++) { world->Step(output, state, input); - EXPECT_FLOAT_EQ(upperJointPrevPos, multiplier * lowerJoint->GetPosition(0) - offset); + EXPECT_FLOAT_EQ(multiplier * upperJointPrevPos + offset, lowerJoint->GetPosition(0)); upperJointPrevPos = upperJoint->GetPosition(0); } }; - // Case : Mimic -> muliplier = 1, offset = 0 + // Testing with different (multiplier, offset) combinations. testMimicFcn(1, 0); - - // Case : Mimic -> muliplier = -1, offset = 0 testMimicFcn(-1, 0); - - // Case : Mimic -> muliplier = 2, offset = 0.5 + testMimicFcn(1, 0.1); + testMimicFcn(1, 0.3); + testMimicFcn(2, 0); testMimicFcn(2, 0.5); } } From efe14ff783c5d07edbca63f926066076892fa22a Mon Sep 17 00:00:00 2001 From: Aditya Date: Mon, 3 Oct 2022 23:25:42 -0700 Subject: [PATCH 08/21] Delete unnecessary spaces Signed-off-by: Aditya --- include/gz/physics/Joint.hh | 2 -- test/common_test/joint_features.cc | 7 +++---- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/include/gz/physics/Joint.hh b/include/gz/physics/Joint.hh index 231d785a5..429fe4b8a 100644 --- a/include/gz/physics/Joint.hh +++ b/include/gz/physics/Joint.hh @@ -623,8 +623,6 @@ namespace gz Scalar _multiplier, Scalar _offset) = 0; }; }; - - } } diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 93e5902f9..5280d50ed 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1878,7 +1878,7 @@ TYPED_TEST(JointMimicFeatureFixture, JointMimicTest) // Case : Without mimic constraint // Let the simulation run without mimic constraint. - // The postions of joints should not be equal. + // The positions of joints should not be equal. double upperJointPrevPos = 0; for (int _ = 0; _ < 10; _++) { @@ -1911,13 +1911,12 @@ TYPED_TEST(JointMimicFeatureFixture, JointMimicTest) testMimicFcn(1, 0); testMimicFcn(-1, 0); testMimicFcn(1, 0.1); - testMimicFcn(1, 0.3); - testMimicFcn(2, 0); + testMimicFcn(-1, 0.3); + testMimicFcn(-2, 0); testMimicFcn(2, 0.5); } } - int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); From 44848b78d676ff7cf9de889b7ea8833b10b86d6d Mon Sep 17 00:00:00 2001 From: Aditya Date: Tue, 4 Oct 2022 16:31:41 -0700 Subject: [PATCH 09/21] Added test for prismatic joint Signed-off-by: Aditya --- test/common_test/joint_features.cc | 83 ++++++++++++++++++++++++++++-- 1 file changed, 80 insertions(+), 3 deletions(-) diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 5280d50ed..54c44b38a 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1842,7 +1842,7 @@ using JointMimicFeatureTestTypes = TYPED_TEST_SUITE(JointMimicFeatureFixture, JointMimicFeatureTestTypes); -TYPED_TEST(JointMimicFeatureFixture, JointMimicTest) +TYPED_TEST(JointMimicFeatureFixture, RevoluteMimicTest) { for (const std::string &name : this->pluginNames) { @@ -1863,6 +1863,7 @@ TYPED_TEST(JointMimicFeatureFixture, JointMimicTest) auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + // Test mimic constraint between two revolute joints. auto model = world->GetModel("double_pendulum_with_base"); auto upperJoint = model->GetJoint("upper_joint"); auto lowerJoint = model->GetJoint("lower_joint"); @@ -1911,9 +1912,85 @@ TYPED_TEST(JointMimicFeatureFixture, JointMimicTest) testMimicFcn(1, 0); testMimicFcn(-1, 0); testMimicFcn(1, 0.1); - testMimicFcn(-1, 0.3); + testMimicFcn(-1, 0.2); testMimicFcn(-2, 0); - testMimicFcn(2, 0.5); + testMimicFcn(2, 0.1); + } +} + +TYPED_TEST(JointMimicFeatureFixture, PrismaticMimicTest) +{ + for (const std::string &name : this->pluginNames) + { + if(this->PhysicsEngineName(name) != "dartsim") + { + GTEST_SKIP(); + } + + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "mimic_world.sdf")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + // Test mimic constraint between two revolute joints. + auto model = world->GetModel("prismatic_model"); + auto parentJoint = model->GetJoint("prismatic_joint_1"); + auto childJoint = model->GetJoint("prismatic_joint_2"); + + // Ensure both joints start from zero angle. + EXPECT_EQ(parentJoint->GetPosition(0), 0); + EXPECT_EQ(childJoint->GetPosition(0), 0); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + // Case : Without mimic constraint + + // Let the simulation run without mimic constraint. + // The positions of joints should not be equal. + double parentJointPrevPos = 0; + for (int _ = 0; _ < 10; _++) + { + world->Step(output, state, input); + EXPECT_NE(parentJointPrevPos, childJoint->GetPosition(0)); + parentJointPrevPos = parentJoint->GetPosition(0); + } + + auto testMimicFcn = [&](double multiplier, double offset) + { + // Set mimic joint constraint. + childJoint->SetMimicConstraint("prismatic_joint_1", multiplier, offset); + // Reset positions and run a few iterations so the positions reach nontrivial values. + parentJoint->SetPosition(0, 0); + childJoint->SetPosition(0, 0); + for (int _ = 0; _ < 10; _++) + world->Step(output, state, input); + + // Child joint's position should be equal to that of parent joint in previous timestep. + parentJointPrevPos = parentJoint->GetPosition(0); + for (int _ = 0; _ < 10; _++) + { + world->Step(output, state, input); + EXPECT_FLOAT_EQ(multiplier * parentJointPrevPos + offset, childJoint->GetPosition(0)); + parentJointPrevPos = parentJoint->GetPosition(0); + } + }; + + // Testing with different (multiplier, offset) combinations. + testMimicFcn(1, 0); + testMimicFcn(-1, 0); + testMimicFcn(1, 0.1); + testMimicFcn(-1, 0.2); + testMimicFcn(-2, 0); + testMimicFcn(2, 0.1); } } From 1bb3239566fce4f95984d8fc2520c5ab24289b98 Mon Sep 17 00:00:00 2001 From: Aditya Date: Tue, 4 Oct 2022 16:32:38 -0700 Subject: [PATCH 10/21] Prismatic world Signed-off-by: Aditya --- test/common_test/worlds/mimic_world.sdf | 249 ++++++++++++++++++++++++ 1 file changed, 249 insertions(+) create mode 100644 test/common_test/worlds/mimic_world.sdf diff --git a/test/common_test/worlds/mimic_world.sdf b/test/common_test/worlds/mimic_world.sdf new file mode 100644 index 000000000..8de386201 --- /dev/null +++ b/test/common_test/worlds/mimic_world.sdf @@ -0,0 +1,249 @@ + + + + + 0 + + + + + + + + + true + + 0 0 -0.5 0 0 0 + + + + 100 100 1 + + + + + + 100 + 50 + + + + + + false + + + 100 100 1 + + + + 1 1 1 1 + 1 1 1 1 + 1 1 1 1 + + + + + + + + + 0 0 0.05 0 0 0 + + 100 + + + 8.4167 + 0 + 0 + + 40.417 + 0 + + 48.667 + + + + + + + 2.2 1 0.1 + + + + + + + + 2.2 1 0.1 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + + 0.09 0 0.35 0 0 0 + + + + 0.021667 + 0 + 0 + + 0.021667 + 0 + + 0.0016667 + + + + + + + 0.1 0.1 0.5 + + + + + + + + 0.1 0.1 0.5 + + + + 0.7 0.7 0.7 1 + 0.7 0.7 0.7 1 + 0.7 0.7 0.7 1 + + + + + + heavy_base + prismatic_base + + + + + 0.2 0 0.70 0 0 0 + + + + 0.0060417 + 0 + 0 + + 0.0060417 + 0 + + 0.0016667 + + + + + + + 0.1 0.1 0.25 + + + + + + + + 0.1 0.1 0.25 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + +` + + + 0.5 0 0.70 0 0 0 + + + + 0.0060417 + 0 + 0 + + 0.0060417 + 0 + + 0.0016667 + + + + + + + 0.1 0.1 0.25 + + + + + + + + 0.1 0.1 0.25 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + prismatic_base + prismatic_slide + + 0 0 1 + + + -0.25 + 0.25 + + + 50 + + + + + + prismatic_base + prismatic_slide_2 + + 0 0 1 + + + -0.25 + 0.25 + + + 100 + + + + + + + From 5f53cb79f9e22a187b6b025dec5b5deec8a86fe8 Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 5 Oct 2022 17:21:02 -0700 Subject: [PATCH 11/21] Added test for universal joint Signed-off-by: Aditya --- test/common_test/joint_features.cc | 90 +++++- ...ic_world.sdf => mimic_prismatic_world.sdf} | 0 .../worlds/mimic_universal_world.sdf | 256 ++++++++++++++++++ 3 files changed, 345 insertions(+), 1 deletion(-) rename test/common_test/worlds/{mimic_world.sdf => mimic_prismatic_world.sdf} (100%) create mode 100644 test/common_test/worlds/mimic_universal_world.sdf diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 54c44b38a..37c0f8651 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1934,7 +1934,8 @@ TYPED_TEST(JointMimicFeatureFixture, PrismaticMimicTest) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "mimic_world.sdf")); + const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, + "mimic_prismatic_world.sdf")); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -1994,6 +1995,93 @@ TYPED_TEST(JointMimicFeatureFixture, PrismaticMimicTest) } } +TYPED_TEST(JointMimicFeatureFixture, UniversalMimicTest) +{ + for (const std::string &name : this->pluginNames) + { + if(this->PhysicsEngineName(name) != "dartsim") + { + GTEST_SKIP(); + } + + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, + "mimic_universal_world.sdf")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + // Test mimic constraint between two revolute joints. + auto model = world->GetModel("universal_model"); + auto parentJoint = model->GetJoint("universal_joint_1"); + auto childJoint = model->GetJoint("universal_joint_2"); + + // Ensure both joints start from zero angle. + EXPECT_EQ(parentJoint->GetPosition(0), 0); + EXPECT_EQ(childJoint->GetPosition(0), 0); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + // Case : Without mimic constraint + + // Let the simulation run without mimic constraint. + // The positions of joints should not be equal. + double parentJointPrevPosAxis1 = 0; + double parentJointPrevPosAxis2 = 0; + for (int _ = 0; _ < 10; _++) + { + world->Step(output, state, input); + EXPECT_NE(parentJointPrevPosAxis1, childJoint->GetPosition(0)); + EXPECT_NE(parentJointPrevPosAxis2, childJoint->GetPosition(1)); + parentJointPrevPosAxis1 = parentJoint->GetPosition(0); + parentJointPrevPosAxis2 = parentJoint->GetPosition(1); + } + + auto testMimicFcn = [&](double multiplier, double offset) + { + // Set mimic joint constraint. + childJoint->SetMimicConstraint("universal_joint_1", multiplier, offset); + // Reset positions and run a few iterations so the positions reach nontrivial values. + parentJoint->SetPosition(0, 0); + parentJoint->SetPosition(1, 0); + childJoint->SetPosition(0, 0); + childJoint->SetPosition(1, 0); + for (int _ = 0; _ < 10; _++) + world->Step(output, state, input); + + // Child joint's position should be equal to that of parent joint in previous timestep. + parentJointPrevPosAxis1 = parentJoint->GetPosition(0); + parentJointPrevPosAxis2 = parentJoint->GetPosition(1); + for (int _ = 0; _ < 10; _++) + { + world->Step(output, state, input); + EXPECT_FLOAT_EQ(multiplier * parentJointPrevPosAxis1 + offset, + childJoint->GetPosition(0)); + EXPECT_FLOAT_EQ(multiplier * parentJointPrevPosAxis2 + offset, + childJoint->GetPosition(1)); + parentJointPrevPosAxis1 = parentJoint->GetPosition(0); + parentJointPrevPosAxis2 = parentJoint->GetPosition(1); + } + }; + + // Testing with different (multiplier, offset) combinations. + testMimicFcn(1, 0); + testMimicFcn(-1, 0); + testMimicFcn(1, 0.1); + testMimicFcn(-1, 0.2); + testMimicFcn(-2, 0); + testMimicFcn(2, 0.1); + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/common_test/worlds/mimic_world.sdf b/test/common_test/worlds/mimic_prismatic_world.sdf similarity index 100% rename from test/common_test/worlds/mimic_world.sdf rename to test/common_test/worlds/mimic_prismatic_world.sdf diff --git a/test/common_test/worlds/mimic_universal_world.sdf b/test/common_test/worlds/mimic_universal_world.sdf new file mode 100644 index 000000000..ed86066b3 --- /dev/null +++ b/test/common_test/worlds/mimic_universal_world.sdf @@ -0,0 +1,256 @@ + + + + + 0 + + + + + + + + + true + + 0 0 -0.5 0 0 0 + + + + 100 100 1 + + + + + + 100 + 50 + + + + + + false + + + 100 100 1 + + + + 1 1 1 1 + 1 1 1 1 + 1 1 1 1 + + + + + + + + + 0 0 0.05 0 0 0 + + 100 + + + 8.4167 + 0 + 0 + + 40.417 + 0 + + 48.667 + + + + + + + 2.2 1 0.1 + + + + + + + + 2.2 1 0.1 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + + 0.9 0 0.55 0 0 0 + + + + 0.0016667 + 0 + 0 + + 0.0016667 + 0 + + 0.0016667 + + + + + + + 0.1 0.1 0.1 + + + + + + + + 0.1 0.1 0.1 + + + + 0.7 0.7 0.7 1 + 0.7 0.7 0.7 1 + 0.7 0.7 0.7 1 + + + + + + heavy_base + universal_base + + + + + 0.9 -0.075 0.44 1.5708 0 0 + + + + 0.0058333 + 0 + 0 + + 0.0058333 + 0 + + 0.00125 + + + -0.02 0.02 0 0 0 0 + + + + + + 0.05 + 0.25 + + + + + + + + 0.05 + 0.25 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 0.9 -0.075 1.44 1.5708 0 0 + + + + 0.0058333 + 0 + 0 + + 0.0058333 + 0 + + 0.00125 + + + -0.02 0.02 0 0 0 0 + + + + + + 0.05 + 0.1 + + + + + + + + 0.05 + 0.1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + universal_base + universal_arm + 0 0 -0.075 0 0 0 + + 1 0 0 + + + 0 1 0 + + -1e16 + 1e16 + + + + + universal_base + universal_arm_2 + 0 0 -0.075 0 0 0 + + 1 0 0 + + + 0 1 0 + + -1e16 + 1e16 + + + + + + + From 1317a6d5840eb8ece1ad0df23ab5c23de5bd0aa0 Mon Sep 17 00:00:00 2001 From: Aditya Date: Tue, 11 Oct 2022 15:32:38 -0700 Subject: [PATCH 12/21] Added tests between different joint types Signed-off-by: Aditya --- test/common_test/joint_features.cc | 157 ++++++++---------- .../worlds/mimic_prismatic_world.sdf | 152 +++++++++++++++++ 2 files changed, 218 insertions(+), 91 deletions(-) diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 37c0f8651..b8fc052c6 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1842,84 +1842,15 @@ using JointMimicFeatureTestTypes = TYPED_TEST_SUITE(JointMimicFeatureFixture, JointMimicFeatureTestTypes); -TYPED_TEST(JointMimicFeatureFixture, RevoluteMimicTest) -{ - for (const std::string &name : this->pluginNames) - { - if(this->PhysicsEngineName(name) != "dartsim") - { - GTEST_SKIP(); - } - - std::cout << "Testing plugin: " << name << std::endl; - gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); - - auto engine = gz::physics::RequestEngine3d::From(plugin); - ASSERT_NE(nullptr, engine); - - sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); - ASSERT_TRUE(errors.empty()) << errors.front(); - - auto world = engine->ConstructWorld(*root.WorldByIndex(0)); - - // Test mimic constraint between two revolute joints. - auto model = world->GetModel("double_pendulum_with_base"); - auto upperJoint = model->GetJoint("upper_joint"); - auto lowerJoint = model->GetJoint("lower_joint"); - - // Ensure both joints start from zero angle. - EXPECT_EQ(upperJoint->GetPosition(0), 0); - EXPECT_EQ(lowerJoint->GetPosition(0), 0); - - gz::physics::ForwardStep::Output output; - gz::physics::ForwardStep::State state; - gz::physics::ForwardStep::Input input; - - // Case : Without mimic constraint - - // Let the simulation run without mimic constraint. - // The positions of joints should not be equal. - double upperJointPrevPos = 0; - for (int _ = 0; _ < 10; _++) - { - world->Step(output, state, input); - EXPECT_NE(upperJointPrevPos, lowerJoint->GetPosition(0)); - upperJointPrevPos = upperJoint->GetPosition(0); - } - - auto testMimicFcn = [&](double multiplier, double offset) - { - // Set mimic joint constraint. - lowerJoint->SetMimicConstraint("upper_joint", multiplier, offset); - // Reset positions and run a few iterations so the positions reach nontrivial values. - upperJoint->SetPosition(0, 0); - lowerJoint->SetPosition(0, 0); - for (int _ = 0; _ < 10; _++) - world->Step(output, state, input); - - // Lower joint's position should be equal to that of upper joint in previous timestep. - upperJointPrevPos = upperJoint->GetPosition(0); - for (int _ = 0; _ < 10; _++) - { - world->Step(output, state, input); - EXPECT_FLOAT_EQ(multiplier * upperJointPrevPos + offset, lowerJoint->GetPosition(0)); - upperJointPrevPos = upperJoint->GetPosition(0); - } - }; - - // Testing with different (multiplier, offset) combinations. - testMimicFcn(1, 0); - testMimicFcn(-1, 0); - testMimicFcn(1, 0.1); - testMimicFcn(-1, 0.2); - testMimicFcn(-2, 0); - testMimicFcn(2, 0.1); - } -} - -TYPED_TEST(JointMimicFeatureFixture, PrismaticMimicTest) +TYPED_TEST(JointMimicFeatureFixture, PrismaticRevoluteMimicTest) { + // This test contains 5 joints : 3 prismatic and 2 revolute. + // They are connected as follows : + // prismatic_joint_1 : Free joint + // prismatic_joint_2 : Mimics prismatic_joint_1 + // revolute_joint_1 : Mimics prismatic_joint_1 + // revolute_joint_2 : Mimics revolute_joint_1 + // prismatic_joint_3 : Mimics revolute_joint_1 for (const std::string &name : this->pluginNames) { if(this->PhysicsEngineName(name) != "dartsim") @@ -1940,14 +1871,20 @@ TYPED_TEST(JointMimicFeatureFixture, PrismaticMimicTest) auto world = engine->ConstructWorld(*root.WorldByIndex(0)); - // Test mimic constraint between two revolute joints. auto model = world->GetModel("prismatic_model"); + auto parentJoint = model->GetJoint("prismatic_joint_1"); - auto childJoint = model->GetJoint("prismatic_joint_2"); + auto prismaticChildJoint1 = model->GetJoint("prismatic_joint_2"); + auto revoluteChildJoint1 = model->GetJoint("revolute_joint_1"); + auto revoluteChildJoint2 = model->GetJoint("revolute_joint_2"); + auto prismaticChildJoint2 = model->GetJoint("prismatic_joint_3"); // Ensure both joints start from zero angle. EXPECT_EQ(parentJoint->GetPosition(0), 0); - EXPECT_EQ(childJoint->GetPosition(0), 0); + EXPECT_EQ(prismaticChildJoint1->GetPosition(0), 0); + EXPECT_EQ(revoluteChildJoint1->GetPosition(0), 0); + EXPECT_EQ(revoluteChildJoint2->GetPosition(0), 0); + EXPECT_EQ(prismaticChildJoint2->GetPosition(0), 0); gz::physics::ForwardStep::Output output; gz::physics::ForwardStep::State state; @@ -1957,31 +1894,69 @@ TYPED_TEST(JointMimicFeatureFixture, PrismaticMimicTest) // Let the simulation run without mimic constraint. // The positions of joints should not be equal. - double parentJointPrevPos = 0; - for (int _ = 0; _ < 10; _++) + double prismaticJointPrevPos = 0; + double revoluteJointPrevPos = 0; + for (int i = 0; i < 10; i++) { world->Step(output, state, input); - EXPECT_NE(parentJointPrevPos, childJoint->GetPosition(0)); - parentJointPrevPos = parentJoint->GetPosition(0); + if (i > 5) + { + EXPECT_NE(prismaticJointPrevPos, prismaticChildJoint1->GetPosition(0)); + EXPECT_NE(prismaticJointPrevPos, revoluteChildJoint1->GetPosition(0)); + EXPECT_NE(revoluteJointPrevPos, revoluteChildJoint2->GetPosition(0)); + EXPECT_NE(revoluteJointPrevPos, prismaticChildJoint2->GetPosition(0)); + } + + // Update previous positions. + prismaticJointPrevPos = parentJoint->GetPosition(0); + revoluteJointPrevPos = revoluteChildJoint1->GetPosition(0); } auto testMimicFcn = [&](double multiplier, double offset) { - // Set mimic joint constraint. - childJoint->SetMimicConstraint("prismatic_joint_1", multiplier, offset); + // Set mimic joint constraints. + // Parent --> Child + // prismatic_joint_1 -> prismatic_joint_2 + // prismatic_joint_1 -> revolute_joint_1 + // revolute_joint_1 --> revolute_joint_2 + // revolute_joint_1 --> prismatic_joint_3 + prismaticChildJoint1->SetMimicConstraint("prismatic_joint_1", multiplier, offset); + revoluteChildJoint1->SetMimicConstraint("prismatic_joint_1", multiplier, offset); + revoluteChildJoint2->SetMimicConstraint("revolute_joint_1", multiplier, offset); + prismaticChildJoint2->SetMimicConstraint("revolute_joint_1", multiplier, offset); + // Reset positions and run a few iterations so the positions reach nontrivial values. parentJoint->SetPosition(0, 0); - childJoint->SetPosition(0, 0); + prismaticChildJoint1->SetPosition(0, 0); + prismaticChildJoint2->SetPosition(0, 0); + revoluteChildJoint1->SetPosition(0, 0); + revoluteChildJoint2->SetPosition(0, 0); for (int _ = 0; _ < 10; _++) world->Step(output, state, input); - // Child joint's position should be equal to that of parent joint in previous timestep. - parentJointPrevPos = parentJoint->GetPosition(0); + // Child joint's position should be equal to that of parent joint in previous timestep, + // considering the offsets and multipliers. + prismaticJointPrevPos = parentJoint->GetPosition(0); + revoluteJointPrevPos = revoluteChildJoint1->GetPosition(0); for (int _ = 0; _ < 10; _++) { world->Step(output, state, input); - EXPECT_FLOAT_EQ(multiplier * parentJointPrevPos + offset, childJoint->GetPosition(0)); - parentJointPrevPos = parentJoint->GetPosition(0); + // Check for prismatic -> prismatic mimicking. + EXPECT_FLOAT_EQ(multiplier * prismaticJointPrevPos + offset, + prismaticChildJoint1->GetPosition(0)); + // Check for prismatic -> revolute mimicking. + EXPECT_FLOAT_EQ(multiplier * prismaticJointPrevPos + offset, + revoluteChildJoint1->GetPosition(0)); + // Check for revolute -> revolute mimicking. + EXPECT_FLOAT_EQ(multiplier * revoluteJointPrevPos + offset, + revoluteChildJoint2->GetPosition(0)); + // Check for revolute --> prismatic mimicking. + EXPECT_FLOAT_EQ(multiplier * revoluteJointPrevPos + offset, + prismaticChildJoint2->GetPosition(0)); + + // Update previous positions. + prismaticJointPrevPos = parentJoint->GetPosition(0); + revoluteJointPrevPos = revoluteChildJoint1->GetPosition(0); } }; diff --git a/test/common_test/worlds/mimic_prismatic_world.sdf b/test/common_test/worlds/mimic_prismatic_world.sdf index 8de386201..b65d62fba 100644 --- a/test/common_test/worlds/mimic_prismatic_world.sdf +++ b/test/common_test/worlds/mimic_prismatic_world.sdf @@ -212,6 +212,123 @@ + + + -0.5 0 0.70 0 0 0 + + + + 0.0060417 + 0 + 0 + + 0.0060417 + 0 + + 0.0016667 + + + + + + + 0.1 0.1 0.25 + + + + + + + + 0.1 0.1 0.25 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + -1 0 0.70 0 0 0 + + + + 0.0060417 + 0 + 0 + + 0.0060417 + 0 + + 0.0016667 + + + + + + + 0.1 0.1 0.25 + + + + + + + + 0.1 0.1 0.25 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + -1.5 0 0.70 0 0 0 + + + + 0.0060417 + 0 + 0 + + 0.0060417 + 0 + + 0.0016667 + + + + + + + 0.1 0.1 0.25 + + + + + + + + 0.1 0.1 0.25 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + prismatic_base prismatic_slide @@ -244,6 +361,41 @@ + + prismatic_base + revolute_link_1 + + 1.0 0 0 + + 3.0 + + + + + + prismatic_base + revolute_link_2 + + 1.0 0 0 + + 3.0 + + + + + + prismatic_base + prismatic_slide_3 + + 0 0 1 + + + 100 + + + + + From 943740c85adc8d6266fc5a6818ade5b9985467d2 Mon Sep 17 00:00:00 2001 From: Aditya Date: Tue, 11 Oct 2022 16:38:59 -0700 Subject: [PATCH 13/21] Added reference to offset Signed-off-by: Aditya --- dartsim/src/JointFeatures.cc | 5 ++- dartsim/src/JointFeatures.hh | 3 +- include/gz/physics/Joint.hh | 5 ++- include/gz/physics/detail/Joint.hh | 4 +- test/common_test/joint_features.cc | 62 +++++++++++++++++------------- 5 files changed, 45 insertions(+), 34 deletions(-) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index ff8904876..b2a6d22af 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -702,7 +702,8 @@ void JointFeatures::SetJointMimicConstraint( const Identity &_id, const std::string _mimicJoint, const double _multiplier, - const double _offset) + const double _offset, + const double _reference) { auto joint = this->ReferenceInterface(_id)->joint; auto jointMimic = joint->getSkeleton()->getJoint(_mimicJoint); @@ -726,7 +727,7 @@ void JointFeatures::SetJointMimicConstraint( } joint->setActuatorType(dart::dynamics::Joint::MIMIC); - joint->setMimicJoint(jointMimic, _multiplier, _offset); + joint->setMimicJoint(jointMimic, _multiplier, _offset - _multiplier * _reference); } } diff --git a/dartsim/src/JointFeatures.hh b/dartsim/src/JointFeatures.hh index 19b573f3e..087d8324a 100644 --- a/dartsim/src/JointFeatures.hh +++ b/dartsim/src/JointFeatures.hh @@ -211,7 +211,8 @@ class JointFeatures : const Identity &_id, const std::string _mimicJoint, const double _multiplier, - const double _offset) override; + const double _offset, + const double _reference) override; }; } diff --git a/include/gz/physics/Joint.hh b/include/gz/physics/Joint.hh index 429fe4b8a..eaeed7763 100644 --- a/include/gz/physics/Joint.hh +++ b/include/gz/physics/Joint.hh @@ -607,7 +607,8 @@ namespace gz public: void SetMimicConstraint( const std::string _mimicJoint, Scalar _multiplier, - Scalar _offset); + Scalar _offset, + Scalar _reference); }; /// \private The implementation API for setting the mimic constraint. @@ -620,7 +621,7 @@ namespace gz public: virtual void SetJointMimicConstraint( const Identity &_id, const std::string _mimicJoint, - Scalar _multiplier, Scalar _offset) = 0; + Scalar _multiplier, Scalar _offset, Scalar _reference) = 0; }; }; } diff --git a/include/gz/physics/detail/Joint.hh b/include/gz/physics/detail/Joint.hh index 7cfcd55fc..13b5b67b0 100644 --- a/include/gz/physics/detail/Joint.hh +++ b/include/gz/physics/detail/Joint.hh @@ -221,11 +221,11 @@ namespace gz void SetMimicConstraintFeature::Joint:: SetMimicConstraint( const std::string _mimicJoint, - Scalar _multiplier, Scalar _offset) + Scalar _multiplier, Scalar _offset, Scalar _reference) { this->template Interface() ->SetJointMimicConstraint(this->identity, _mimicJoint, - _multiplier, _offset); + _multiplier, _offset, _reference); } ///////////////////////////////////////////////// diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index b8fc052c6..f38e32144 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1912,7 +1912,7 @@ TYPED_TEST(JointMimicFeatureFixture, PrismaticRevoluteMimicTest) revoluteJointPrevPos = revoluteChildJoint1->GetPosition(0); } - auto testMimicFcn = [&](double multiplier, double offset) + auto testMimicFcn = [&](double multiplier, double offset, double reference) { // Set mimic joint constraints. // Parent --> Child @@ -1920,10 +1920,14 @@ TYPED_TEST(JointMimicFeatureFixture, PrismaticRevoluteMimicTest) // prismatic_joint_1 -> revolute_joint_1 // revolute_joint_1 --> revolute_joint_2 // revolute_joint_1 --> prismatic_joint_3 - prismaticChildJoint1->SetMimicConstraint("prismatic_joint_1", multiplier, offset); - revoluteChildJoint1->SetMimicConstraint("prismatic_joint_1", multiplier, offset); - revoluteChildJoint2->SetMimicConstraint("revolute_joint_1", multiplier, offset); - prismaticChildJoint2->SetMimicConstraint("revolute_joint_1", multiplier, offset); + prismaticChildJoint1->SetMimicConstraint("prismatic_joint_1", multiplier, + offset, reference); + revoluteChildJoint1->SetMimicConstraint("prismatic_joint_1", multiplier, + offset, reference); + revoluteChildJoint2->SetMimicConstraint("revolute_joint_1", multiplier, + offset, reference); + prismaticChildJoint2->SetMimicConstraint("revolute_joint_1", multiplier, + offset, reference); // Reset positions and run a few iterations so the positions reach nontrivial values. parentJoint->SetPosition(0, 0); @@ -1942,16 +1946,16 @@ TYPED_TEST(JointMimicFeatureFixture, PrismaticRevoluteMimicTest) { world->Step(output, state, input); // Check for prismatic -> prismatic mimicking. - EXPECT_FLOAT_EQ(multiplier * prismaticJointPrevPos + offset, + EXPECT_FLOAT_EQ(multiplier * (prismaticJointPrevPos - reference) + offset, prismaticChildJoint1->GetPosition(0)); // Check for prismatic -> revolute mimicking. - EXPECT_FLOAT_EQ(multiplier * prismaticJointPrevPos + offset, + EXPECT_FLOAT_EQ(multiplier * (prismaticJointPrevPos - reference) + offset, revoluteChildJoint1->GetPosition(0)); // Check for revolute -> revolute mimicking. - EXPECT_FLOAT_EQ(multiplier * revoluteJointPrevPos + offset, + EXPECT_FLOAT_EQ(multiplier * (revoluteJointPrevPos - reference) + offset, revoluteChildJoint2->GetPosition(0)); // Check for revolute --> prismatic mimicking. - EXPECT_FLOAT_EQ(multiplier * revoluteJointPrevPos + offset, + EXPECT_FLOAT_EQ(multiplier * (revoluteJointPrevPos - reference) + offset, prismaticChildJoint2->GetPosition(0)); // Update previous positions. @@ -1960,13 +1964,15 @@ TYPED_TEST(JointMimicFeatureFixture, PrismaticRevoluteMimicTest) } }; - // Testing with different (multiplier, offset) combinations. - testMimicFcn(1, 0); - testMimicFcn(-1, 0); - testMimicFcn(1, 0.1); - testMimicFcn(-1, 0.2); - testMimicFcn(-2, 0); - testMimicFcn(2, 0.1); + // Testing with different (multiplier, offset, reference) combinations. + testMimicFcn(1, 0, 0); + testMimicFcn(-1, 0, 0); + testMimicFcn(1, 0.1, 0); + testMimicFcn(1, 0.05, 0.05); + testMimicFcn(-1, 0.2, 0); + testMimicFcn(-1, 0.05, -0.05); + testMimicFcn(-2, 0, 0); + testMimicFcn(2, 0.1, 0); } } @@ -2020,10 +2026,10 @@ TYPED_TEST(JointMimicFeatureFixture, UniversalMimicTest) parentJointPrevPosAxis2 = parentJoint->GetPosition(1); } - auto testMimicFcn = [&](double multiplier, double offset) + auto testMimicFcn = [&](double multiplier, double offset, double reference) { // Set mimic joint constraint. - childJoint->SetMimicConstraint("universal_joint_1", multiplier, offset); + childJoint->SetMimicConstraint("universal_joint_1", multiplier, offset, reference); // Reset positions and run a few iterations so the positions reach nontrivial values. parentJoint->SetPosition(0, 0); parentJoint->SetPosition(1, 0); @@ -2038,22 +2044,24 @@ TYPED_TEST(JointMimicFeatureFixture, UniversalMimicTest) for (int _ = 0; _ < 10; _++) { world->Step(output, state, input); - EXPECT_FLOAT_EQ(multiplier * parentJointPrevPosAxis1 + offset, + EXPECT_FLOAT_EQ(multiplier * (parentJointPrevPosAxis1 - reference) + offset, childJoint->GetPosition(0)); - EXPECT_FLOAT_EQ(multiplier * parentJointPrevPosAxis2 + offset, + EXPECT_FLOAT_EQ(multiplier * (parentJointPrevPosAxis2 - reference) + offset, childJoint->GetPosition(1)); parentJointPrevPosAxis1 = parentJoint->GetPosition(0); parentJointPrevPosAxis2 = parentJoint->GetPosition(1); } }; - // Testing with different (multiplier, offset) combinations. - testMimicFcn(1, 0); - testMimicFcn(-1, 0); - testMimicFcn(1, 0.1); - testMimicFcn(-1, 0.2); - testMimicFcn(-2, 0); - testMimicFcn(2, 0.1); + // Testing with different (multiplier, offset, reference) combinations. + testMimicFcn(1, 0, 0); + testMimicFcn(-1, 0, 0); + testMimicFcn(1, 0.1, 0); + testMimicFcn(1, 0.2, 0.1); + testMimicFcn(-1, 0.2, 0); + testMimicFcn(-2, 0, 0); + testMimicFcn(2, 0.1, 0); + testMimicFcn(2, 0.3, -0.1); } } From 87e8de3dfa02f0ffd05773d84b0cae93a75574ae Mon Sep 17 00:00:00 2001 From: Aditya Date: Tue, 11 Oct 2022 17:15:48 -0700 Subject: [PATCH 14/21] Linter Signed-off-by: Aditya --- dartsim/src/JointFeatures.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index b2a6d22af..3d93cb2fb 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -727,7 +727,8 @@ void JointFeatures::SetJointMimicConstraint( } joint->setActuatorType(dart::dynamics::Joint::MIMIC); - joint->setMimicJoint(jointMimic, _multiplier, _offset - _multiplier * _reference); + joint->setMimicJoint(jointMimic, _multiplier, + _offset - _multiplier * _reference); } } From 4dd1b216c88a2c54558f5f4e6487990e88919986 Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 12 Oct 2022 15:12:54 -0700 Subject: [PATCH 15/21] Added checks for DOFs and joint name Signed-off-by: Aditya --- dartsim/src/JointFeatures.cc | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index 3d93cb2fb..a125f03de 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -725,6 +725,20 @@ void JointFeatures::SetJointMimicConstraint( << "]. The command will be ignored\n"; return; } + // Check if the joint tries to mimic itself. + if (joint == jointMimic) + { + gzerr << "Parent and child joints for the mimic contraint" + << " should not be the same. The constraint will be ignored" + << std::endl; + } + // Check for degrees of freedom. + if (joint->getNumDofs() != jointMimic->getNumDofs()) + { + gzerr << "Mimic constraint pair of joints should have the same" + << " degress of freedom. The constraint will be ignored" + << std::endl; + } joint->setActuatorType(dart::dynamics::Joint::MIMIC); joint->setMimicJoint(jointMimic, _multiplier, From 4f19f386c677ec1cfa071bccb1f9ac494439a831 Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 16 Nov 2022 14:02:08 -0800 Subject: [PATCH 16/21] Added a test to check vioaltion of consv of energy Signed-off-by: Aditya --- test/common_test/joint_features.cc | 87 +++++++ .../worlds/mimic_pendulum_world.sdf | 231 ++++++++++++++++++ 2 files changed, 318 insertions(+) create mode 100644 test/common_test/worlds/mimic_pendulum_world.sdf diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 6bad459b2..2a7fecb00 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1856,6 +1856,8 @@ using JointMimicFeatureTestTypes = TYPED_TEST_SUITE(JointMimicFeatureFixture, JointMimicFeatureTestTypes); +// Here, we test mimic constraints on various combinations of +// prismatic and revolute joints using a chain of constraints. TYPED_TEST(JointMimicFeatureFixture, PrismaticRevoluteMimicTest) { // This test contains 5 joints : 3 prismatic and 2 revolute. @@ -1990,6 +1992,7 @@ TYPED_TEST(JointMimicFeatureFixture, PrismaticRevoluteMimicTest) } } +// Here, we test the mimic constraint for a pair of universal joints. TYPED_TEST(JointMimicFeatureFixture, UniversalMimicTest) { for (const std::string &name : this->pluginNames) @@ -2079,6 +2082,90 @@ TYPED_TEST(JointMimicFeatureFixture, UniversalMimicTest) } } +// In this test, we have 2 pendulums of different lengths. +// Originally, their time periods are different, but after applying +// the mimic constraint, they follow the same velocity, effectively +// violating some laws. (Work done = Change in kinetic energy, for instance) +TYPED_TEST(JointMimicFeatureFixture, PendulumMimicTest) +{ + for (const std::string &name : this->pluginNames) + { + if(this->PhysicsEngineName(name) != "dartsim") + { + GTEST_SKIP(); + } + + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, + "mimic_pendulum_world.sdf")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + // Test mimic constraint between two revolute joints. + auto model = world->GetModel("pendulum_with_base"); + auto parentJoint = model->GetJoint("upper_joint_1"); + auto childJoint = model->GetJoint("upper_joint_2"); + + // Ensure both joints start from zero angle. + EXPECT_EQ(parentJoint->GetPosition(0), 0); + EXPECT_EQ(childJoint->GetPosition(0), 0); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + // Case : Without mimic constraint + + // Let the simulation run without mimic constraint. + // The positions of joints should not be equal. + double parentJointPrevPosAxis = 0; + for (int _ = 0; _ < 10; _++) + { + world->Step(output, state, input); + EXPECT_NE(parentJointPrevPosAxis, childJoint->GetPosition(0)); + parentJointPrevPosAxis = parentJoint->GetPosition(0); + } + + auto testMimicFcn = [&](double multiplier, double offset, double reference) + { + // Set mimic joint constraint. + childJoint->SetMimicConstraint("upper_joint_1", multiplier, offset, reference); + // Reset positions and run a few iterations so the positions reach nontrivial values. + parentJoint->SetPosition(0, 0); + childJoint->SetPosition(0, 0); + for (int _ = 0; _ < 10; _++) + world->Step(output, state, input); + + // Child joint's position should be equal to that of parent joint in previous timestep. + parentJointPrevPosAxis = parentJoint->GetPosition(0); + for (int _ = 0; _ < 10; _++) + { + world->Step(output, state, input); + EXPECT_FLOAT_EQ(multiplier * (parentJointPrevPosAxis - reference) + offset, + childJoint->GetPosition(0)); + parentJointPrevPosAxis = parentJoint->GetPosition(0); + } + }; + + // Testing with different (multiplier, offset, reference) combinations. + testMimicFcn(1, 0, 0); + testMimicFcn(-1, 0, 0); + testMimicFcn(1, 0.1, 0); + testMimicFcn(1, 0.2, 0.1); + testMimicFcn(-1, 0.2, 0); + testMimicFcn(-2, 0, 0); + testMimicFcn(2, 0.1, 0); + testMimicFcn(2, 0.3, -0.1); + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/common_test/worlds/mimic_pendulum_world.sdf b/test/common_test/worlds/mimic_pendulum_world.sdf new file mode 100644 index 000000000..e4471c9c0 --- /dev/null +++ b/test/common_test/worlds/mimic_pendulum_world.sdf @@ -0,0 +1,231 @@ + + + + + 0.001 + 0 + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0 0 0 0 + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + + -0.5 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.2 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.2 0 0 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 0.2 0 0 0 + + + 0.1 + 0.3 + + + + + + + base + upper_link_1 + + 1.0 0 0 + + + + + base + upper_link_2 + + 1.0 0 0 + + + + + + + From 6a0294b1c790dd7950843d6aa75acf52bcf7c054 Mon Sep 17 00:00:00 2001 From: Aditya Pande Date: Wed, 10 May 2023 20:34:44 +0000 Subject: [PATCH 17/21] Change parent/child to leader/follower, added axis arg to SetMimicConstraint() Signed-off-by: Aditya Pande --- dartsim/src/JointFeatures.cc | 3 +- dartsim/src/JointFeatures.hh | 1 + include/gz/physics/Joint.hh | 2 + include/gz/physics/detail/Joint.hh | 3 +- test/common_test/joint_mimic_features.cc | 148 +++++++++++------------ 5 files changed, 81 insertions(+), 76 deletions(-) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index cc14bfe58..ed40022ce 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -728,6 +728,7 @@ Wrench3d JointFeatures::GetJointTransmittedWrenchInJointFrame( void JointFeatures::SetJointMimicConstraint( const Identity &_id, const std::string _mimicJoint, + const std::string, const double _multiplier, const double _offset, const double _reference) @@ -755,7 +756,7 @@ void JointFeatures::SetJointMimicConstraint( // Check if the joint tries to mimic itself. if (joint == jointMimic) { - gzerr << "Parent and child joints for the mimic contraint" + gzerr << "Leader and follower joints for the mimic contraint" << " should not be the same. The constraint will be ignored" << std::endl; } diff --git a/dartsim/src/JointFeatures.hh b/dartsim/src/JointFeatures.hh index 087d8324a..7e346093c 100644 --- a/dartsim/src/JointFeatures.hh +++ b/dartsim/src/JointFeatures.hh @@ -210,6 +210,7 @@ class JointFeatures : public: void SetJointMimicConstraint( const Identity &_id, const std::string _mimicJoint, + const std::string _axis, const double _multiplier, const double _offset, const double _reference) override; diff --git a/include/gz/physics/Joint.hh b/include/gz/physics/Joint.hh index eaeed7763..84653e91c 100644 --- a/include/gz/physics/Joint.hh +++ b/include/gz/physics/Joint.hh @@ -606,6 +606,7 @@ namespace gz public: void SetMimicConstraint( const std::string _mimicJoint, + const std::string _axis, Scalar _multiplier, Scalar _offset, Scalar _reference); @@ -621,6 +622,7 @@ namespace gz public: virtual void SetJointMimicConstraint( const Identity &_id, const std::string _mimicJoint, + const std::string _axis, Scalar _multiplier, Scalar _offset, Scalar _reference) = 0; }; }; diff --git a/include/gz/physics/detail/Joint.hh b/include/gz/physics/detail/Joint.hh index 13b5b67b0..c9278fa02 100644 --- a/include/gz/physics/detail/Joint.hh +++ b/include/gz/physics/detail/Joint.hh @@ -221,11 +221,12 @@ namespace gz void SetMimicConstraintFeature::Joint:: SetMimicConstraint( const std::string _mimicJoint, + const std::string _axis, Scalar _multiplier, Scalar _offset, Scalar _reference) { this->template Interface() ->SetJointMimicConstraint(this->identity, _mimicJoint, - _multiplier, _offset, _reference); + _axis, _multiplier, _offset, _reference); } ///////////////////////////////////////////////// diff --git a/test/common_test/joint_mimic_features.cc b/test/common_test/joint_mimic_features.cc index 247ade6eb..fae5ac65b 100644 --- a/test/common_test/joint_mimic_features.cc +++ b/test/common_test/joint_mimic_features.cc @@ -130,18 +130,18 @@ TEST_F(JointMimicFeatureTest, PrismaticRevoluteMimicTest) auto model = world->GetModel("prismatic_model"); - auto parentJoint = model->GetJoint("prismatic_joint_1"); - auto prismaticChildJoint1 = model->GetJoint("prismatic_joint_2"); - auto revoluteChildJoint1 = model->GetJoint("revolute_joint_1"); - auto revoluteChildJoint2 = model->GetJoint("revolute_joint_2"); - auto prismaticChildJoint2 = model->GetJoint("prismatic_joint_3"); + auto leaderJoint = model->GetJoint("prismatic_joint_1"); + auto prismaticFollowerJoint1 = model->GetJoint("prismatic_joint_2"); + auto revoluteFollowerJoint1 = model->GetJoint("revolute_joint_1"); + auto revoluteFollowerJoint2 = model->GetJoint("revolute_joint_2"); + auto prismaticFollowerJoint2 = model->GetJoint("prismatic_joint_3"); // Ensure both joints start from zero angle. - EXPECT_EQ(parentJoint->GetPosition(0), 0); - EXPECT_EQ(prismaticChildJoint1->GetPosition(0), 0); - EXPECT_EQ(revoluteChildJoint1->GetPosition(0), 0); - EXPECT_EQ(revoluteChildJoint2->GetPosition(0), 0); - EXPECT_EQ(prismaticChildJoint2->GetPosition(0), 0); + EXPECT_EQ(leaderJoint->GetPosition(0), 0); + EXPECT_EQ(prismaticFollowerJoint1->GetPosition(0), 0); + EXPECT_EQ(revoluteFollowerJoint1->GetPosition(0), 0); + EXPECT_EQ(revoluteFollowerJoint2->GetPosition(0), 0); + EXPECT_EQ(prismaticFollowerJoint2->GetPosition(0), 0); gz::physics::ForwardStep::Output output; gz::physics::ForwardStep::State state; @@ -158,66 +158,66 @@ TEST_F(JointMimicFeatureTest, PrismaticRevoluteMimicTest) world->Step(output, state, input); if (i > 5) { - EXPECT_NE(prismaticJointPrevPos, prismaticChildJoint1->GetPosition(0)); - EXPECT_NE(prismaticJointPrevPos, revoluteChildJoint1->GetPosition(0)); - EXPECT_NE(revoluteJointPrevPos, revoluteChildJoint2->GetPosition(0)); - EXPECT_NE(revoluteJointPrevPos, prismaticChildJoint2->GetPosition(0)); + EXPECT_NE(prismaticJointPrevPos, prismaticFollowerJoint1->GetPosition(0)); + EXPECT_NE(prismaticJointPrevPos, revoluteFollowerJoint1->GetPosition(0)); + EXPECT_NE(revoluteJointPrevPos, revoluteFollowerJoint2->GetPosition(0)); + EXPECT_NE(revoluteJointPrevPos, prismaticFollowerJoint2->GetPosition(0)); } // Update previous positions. - prismaticJointPrevPos = parentJoint->GetPosition(0); - revoluteJointPrevPos = revoluteChildJoint1->GetPosition(0); + prismaticJointPrevPos = leaderJoint->GetPosition(0); + revoluteJointPrevPos = revoluteFollowerJoint1->GetPosition(0); } auto testMimicFcn = [&](double multiplier, double offset, double reference) { // Set mimic joint constraints. - // Parent --> Child + // Leader --> Follower // prismatic_joint_1 -> prismatic_joint_2 // prismatic_joint_1 -> revolute_joint_1 // revolute_joint_1 --> revolute_joint_2 // revolute_joint_1 --> prismatic_joint_3 - prismaticChildJoint1->SetMimicConstraint("prismatic_joint_1", multiplier, + prismaticFollowerJoint1->SetMimicConstraint("prismatic_joint_1", "axis", multiplier, offset, reference); - revoluteChildJoint1->SetMimicConstraint("prismatic_joint_1", multiplier, + revoluteFollowerJoint1->SetMimicConstraint("prismatic_joint_1", "axis" , multiplier, offset, reference); - revoluteChildJoint2->SetMimicConstraint("revolute_joint_1", multiplier, + revoluteFollowerJoint2->SetMimicConstraint("revolute_joint_1", "axis", multiplier, offset, reference); - prismaticChildJoint2->SetMimicConstraint("revolute_joint_1", multiplier, + prismaticFollowerJoint2->SetMimicConstraint("revolute_joint_1", "axis", multiplier, offset, reference); // Reset positions and run a few iterations so the positions reach nontrivial values. - parentJoint->SetPosition(0, 0); - prismaticChildJoint1->SetPosition(0, 0); - prismaticChildJoint2->SetPosition(0, 0); - revoluteChildJoint1->SetPosition(0, 0); - revoluteChildJoint2->SetPosition(0, 0); + leaderJoint->SetPosition(0, 0); + prismaticFollowerJoint1->SetPosition(0, 0); + prismaticFollowerJoint2->SetPosition(0, 0); + revoluteFollowerJoint1->SetPosition(0, 0); + revoluteFollowerJoint2->SetPosition(0, 0); for (int _ = 0; _ < 10; _++) world->Step(output, state, input); - // Child joint's position should be equal to that of parent joint in previous timestep, + // Follower joint's position should be equal to that of leader joint in previous timestep, // considering the offsets and multipliers. - prismaticJointPrevPos = parentJoint->GetPosition(0); - revoluteJointPrevPos = revoluteChildJoint1->GetPosition(0); + prismaticJointPrevPos = leaderJoint->GetPosition(0); + revoluteJointPrevPos = revoluteFollowerJoint1->GetPosition(0); for (int _ = 0; _ < 10; _++) { world->Step(output, state, input); // Check for prismatic -> prismatic mimicking. EXPECT_FLOAT_EQ(multiplier * (prismaticJointPrevPos - reference) + offset, - prismaticChildJoint1->GetPosition(0)); + prismaticFollowerJoint1->GetPosition(0)); // Check for prismatic -> revolute mimicking. EXPECT_FLOAT_EQ(multiplier * (prismaticJointPrevPos - reference) + offset, - revoluteChildJoint1->GetPosition(0)); + revoluteFollowerJoint1->GetPosition(0)); // Check for revolute -> revolute mimicking. EXPECT_FLOAT_EQ(multiplier * (revoluteJointPrevPos - reference) + offset, - revoluteChildJoint2->GetPosition(0)); + revoluteFollowerJoint2->GetPosition(0)); // Check for revolute --> prismatic mimicking. EXPECT_FLOAT_EQ(multiplier * (revoluteJointPrevPos - reference) + offset, - prismaticChildJoint2->GetPosition(0)); + prismaticFollowerJoint2->GetPosition(0)); // Update previous positions. - prismaticJointPrevPos = parentJoint->GetPosition(0); - revoluteJointPrevPos = revoluteChildJoint1->GetPosition(0); + prismaticJointPrevPos = leaderJoint->GetPosition(0); + revoluteJointPrevPos = revoluteFollowerJoint1->GetPosition(0); } }; @@ -258,12 +258,12 @@ TEST_F(JointMimicFeatureTest, UniversalMimicTest) // Test mimic constraint between two revolute joints. auto model = world->GetModel("universal_model"); - auto parentJoint = model->GetJoint("universal_joint_1"); - auto childJoint = model->GetJoint("universal_joint_2"); + auto leaderJoint = model->GetJoint("universal_joint_1"); + auto followerJoint = model->GetJoint("universal_joint_2"); // Ensure both joints start from zero angle. - EXPECT_EQ(parentJoint->GetPosition(0), 0); - EXPECT_EQ(childJoint->GetPosition(0), 0); + EXPECT_EQ(leaderJoint->GetPosition(0), 0); + EXPECT_EQ(followerJoint->GetPosition(0), 0); gz::physics::ForwardStep::Output output; gz::physics::ForwardStep::State state; @@ -273,41 +273,41 @@ TEST_F(JointMimicFeatureTest, UniversalMimicTest) // Let the simulation run without mimic constraint. // The positions of joints should not be equal. - double parentJointPrevPosAxis1 = 0; - double parentJointPrevPosAxis2 = 0; + double leaderJointPrevPosAxis1 = 0; + double leaderJointPrevPosAxis2 = 0; for (int _ = 0; _ < 10; _++) { world->Step(output, state, input); - EXPECT_NE(parentJointPrevPosAxis1, childJoint->GetPosition(0)); - EXPECT_NE(parentJointPrevPosAxis2, childJoint->GetPosition(1)); - parentJointPrevPosAxis1 = parentJoint->GetPosition(0); - parentJointPrevPosAxis2 = parentJoint->GetPosition(1); + EXPECT_NE(leaderJointPrevPosAxis1, followerJoint->GetPosition(0)); + EXPECT_NE(leaderJointPrevPosAxis2, followerJoint->GetPosition(1)); + leaderJointPrevPosAxis1 = leaderJoint->GetPosition(0); + leaderJointPrevPosAxis2 = leaderJoint->GetPosition(1); } auto testMimicFcn = [&](double multiplier, double offset, double reference) { // Set mimic joint constraint. - childJoint->SetMimicConstraint("universal_joint_1", multiplier, offset, reference); + followerJoint->SetMimicConstraint("universal_joint_1", "axis", multiplier, offset, reference); // Reset positions and run a few iterations so the positions reach nontrivial values. - parentJoint->SetPosition(0, 0); - parentJoint->SetPosition(1, 0); - childJoint->SetPosition(0, 0); - childJoint->SetPosition(1, 0); + leaderJoint->SetPosition(0, 0); + leaderJoint->SetPosition(1, 0); + followerJoint->SetPosition(0, 0); + followerJoint->SetPosition(1, 0); for (int _ = 0; _ < 10; _++) world->Step(output, state, input); - // Child joint's position should be equal to that of parent joint in previous timestep. - parentJointPrevPosAxis1 = parentJoint->GetPosition(0); - parentJointPrevPosAxis2 = parentJoint->GetPosition(1); + // Follower joint's position should be equal to that of leader joint in previous timestep. + leaderJointPrevPosAxis1 = leaderJoint->GetPosition(0); + leaderJointPrevPosAxis2 = leaderJoint->GetPosition(1); for (int _ = 0; _ < 10; _++) { world->Step(output, state, input); - EXPECT_FLOAT_EQ(multiplier * (parentJointPrevPosAxis1 - reference) + offset, - childJoint->GetPosition(0)); - EXPECT_FLOAT_EQ(multiplier * (parentJointPrevPosAxis2 - reference) + offset, - childJoint->GetPosition(1)); - parentJointPrevPosAxis1 = parentJoint->GetPosition(0); - parentJointPrevPosAxis2 = parentJoint->GetPosition(1); + EXPECT_FLOAT_EQ(multiplier * (leaderJointPrevPosAxis1 - reference) + offset, + followerJoint->GetPosition(0)); + EXPECT_FLOAT_EQ(multiplier * (leaderJointPrevPosAxis2 - reference) + offset, + followerJoint->GetPosition(1)); + leaderJointPrevPosAxis1 = leaderJoint->GetPosition(0); + leaderJointPrevPosAxis2 = leaderJoint->GetPosition(1); } }; @@ -351,12 +351,12 @@ TEST_F(JointMimicFeatureTest, PendulumMimicTest) // Test mimic constraint between two revolute joints. auto model = world->GetModel("pendulum_with_base"); - auto parentJoint = model->GetJoint("upper_joint_1"); - auto childJoint = model->GetJoint("upper_joint_2"); + auto leaderJoint = model->GetJoint("upper_joint_1"); + auto followerJoint = model->GetJoint("upper_joint_2"); // Ensure both joints start from zero angle. - EXPECT_EQ(parentJoint->GetPosition(0), 0); - EXPECT_EQ(childJoint->GetPosition(0), 0); + EXPECT_EQ(leaderJoint->GetPosition(0), 0); + EXPECT_EQ(followerJoint->GetPosition(0), 0); gz::physics::ForwardStep::Output output; gz::physics::ForwardStep::State state; @@ -366,32 +366,32 @@ TEST_F(JointMimicFeatureTest, PendulumMimicTest) // Let the simulation run without mimic constraint. // The positions of joints should not be equal. - double parentJointPrevPosAxis = 0; + double leaderJointPrevPosAxis = 0; for (int _ = 0; _ < 10; _++) { world->Step(output, state, input); - EXPECT_NE(parentJointPrevPosAxis, childJoint->GetPosition(0)); - parentJointPrevPosAxis = parentJoint->GetPosition(0); + EXPECT_NE(leaderJointPrevPosAxis, followerJoint->GetPosition(0)); + leaderJointPrevPosAxis = leaderJoint->GetPosition(0); } auto testMimicFcn = [&](double multiplier, double offset, double reference) { // Set mimic joint constraint. - childJoint->SetMimicConstraint("upper_joint_1", multiplier, offset, reference); + followerJoint->SetMimicConstraint("upper_joint_1", "axis" , multiplier, offset, reference); // Reset positions and run a few iterations so the positions reach nontrivial values. - parentJoint->SetPosition(0, 0); - childJoint->SetPosition(0, 0); + leaderJoint->SetPosition(0, 0); + followerJoint->SetPosition(0, 0); for (int _ = 0; _ < 10; _++) world->Step(output, state, input); - // Child joint's position should be equal to that of parent joint in previous timestep. - parentJointPrevPosAxis = parentJoint->GetPosition(0); + // Follower joint's position should be equal to that of leader joint in previous timestep. + leaderJointPrevPosAxis = leaderJoint->GetPosition(0); for (int _ = 0; _ < 10; _++) { world->Step(output, state, input); - EXPECT_FLOAT_EQ(multiplier * (parentJointPrevPosAxis - reference) + offset, - childJoint->GetPosition(0)); - parentJointPrevPosAxis = parentJoint->GetPosition(0); + EXPECT_FLOAT_EQ(multiplier * (leaderJointPrevPosAxis - reference) + offset, + followerJoint->GetPosition(0)); + leaderJointPrevPosAxis = leaderJoint->GetPosition(0); } }; From 0b4f4901aeb908de0e0a9fcc7e879b1a6071fd7e Mon Sep 17 00:00:00 2001 From: Aditya Pande Date: Wed, 10 May 2023 20:55:03 +0000 Subject: [PATCH 18/21] Changed mimicJoint to joint, minor typo correction Signed-off-by: Aditya Pande --- dartsim/src/JointFeatures.cc | 6 +++--- dartsim/src/JointFeatures.hh | 2 +- include/gz/physics/Joint.hh | 12 ++++++------ include/gz/physics/detail/Joint.hh | 4 ++-- 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index ed40022ce..a581c5016 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -727,14 +727,14 @@ Wrench3d JointFeatures::GetJointTransmittedWrenchInJointFrame( ///////////////////////////////////////////////// void JointFeatures::SetJointMimicConstraint( const Identity &_id, - const std::string _mimicJoint, + const std::string _joint, const std::string, const double _multiplier, const double _offset, const double _reference) { auto joint = this->ReferenceInterface(_id)->joint; - auto jointMimic = joint->getSkeleton()->getJoint(_mimicJoint); + auto jointMimic = joint->getSkeleton()->getJoint(_joint); // Take extra care that the value is valid. A nan can cause the DART // constraint solver to fail, which will in turn either cause a crash or @@ -756,7 +756,7 @@ void JointFeatures::SetJointMimicConstraint( // Check if the joint tries to mimic itself. if (joint == jointMimic) { - gzerr << "Leader and follower joints for the mimic contraint" + gzerr << "Leader and follower joints for the mimic constraint" << " should not be the same. The constraint will be ignored" << std::endl; } diff --git a/dartsim/src/JointFeatures.hh b/dartsim/src/JointFeatures.hh index 7e346093c..52f1248fb 100644 --- a/dartsim/src/JointFeatures.hh +++ b/dartsim/src/JointFeatures.hh @@ -209,7 +209,7 @@ class JointFeatures : // ----- Mimic joint constraint ----- public: void SetJointMimicConstraint( const Identity &_id, - const std::string _mimicJoint, + const std::string _joint, const std::string _axis, const double _multiplier, const double _offset, diff --git a/include/gz/physics/Joint.hh b/include/gz/physics/Joint.hh index 84653e91c..4612a8e60 100644 --- a/include/gz/physics/Joint.hh +++ b/include/gz/physics/Joint.hh @@ -592,12 +592,12 @@ namespace gz : public virtual Feature { /// \brief The Joint API for setting the mimic joint constraint. - /// \param[in] _mimicJoint - /// name of the joint to be mimicked, i.e. the parent joint. + /// \param[in] _joint + /// name of the joint to be mimicked, i.e. the leader joint. /// \param[in] _multiplier - /// The multiplier to be applied to poistion of parent joint. + /// The multiplier to be applied to poistion of leader joint. /// \param[in] _offset - /// The offset to be applied to position of parent joint after + /// The offset to be applied to position of leader joint after /// the multiplier is applied. public: template class Joint : public virtual Feature::Joint @@ -605,7 +605,7 @@ namespace gz public: using Scalar = typename PolicyT::Scalar; public: void SetMimicConstraint( - const std::string _mimicJoint, + const std::string _joint, const std::string _axis, Scalar _multiplier, Scalar _offset, @@ -621,7 +621,7 @@ namespace gz // See Joint::MimicConstraint above public: virtual void SetJointMimicConstraint( const Identity &_id, - const std::string _mimicJoint, + const std::string _joint, const std::string _axis, Scalar _multiplier, Scalar _offset, Scalar _reference) = 0; }; diff --git a/include/gz/physics/detail/Joint.hh b/include/gz/physics/detail/Joint.hh index c9278fa02..a20193690 100644 --- a/include/gz/physics/detail/Joint.hh +++ b/include/gz/physics/detail/Joint.hh @@ -220,12 +220,12 @@ namespace gz template void SetMimicConstraintFeature::Joint:: SetMimicConstraint( - const std::string _mimicJoint, + const std::string _joint, const std::string _axis, Scalar _multiplier, Scalar _offset, Scalar _reference) { this->template Interface() - ->SetJointMimicConstraint(this->identity, _mimicJoint, + ->SetJointMimicConstraint(this->identity, _joint, _axis, _multiplier, _offset, _reference); } From 6878701bf2fbc118b110eab141d66b7f5f1e04f2 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 12 Jul 2023 14:32:16 -0700 Subject: [PATCH 19/21] Fix spelling Signed-off-by: Steve Peters --- include/gz/physics/Joint.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/gz/physics/Joint.hh b/include/gz/physics/Joint.hh index 4612a8e60..2fe69d10d 100644 --- a/include/gz/physics/Joint.hh +++ b/include/gz/physics/Joint.hh @@ -595,7 +595,7 @@ namespace gz /// \param[in] _joint /// name of the joint to be mimicked, i.e. the leader joint. /// \param[in] _multiplier - /// The multiplier to be applied to poistion of leader joint. + /// The multiplier to be applied to position of leader joint. /// \param[in] _offset /// The offset to be applied to position of leader joint after /// the multiplier is applied. From d7f552fe0ff38086eada8fd62b435eae1bb347c8 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 12 Jul 2023 14:34:25 -0700 Subject: [PATCH 20/21] SetJointMimicConstraint: add dof argument Signed-off-by: Steve Peters --- dartsim/src/JointFeatures.cc | 1 + dartsim/src/JointFeatures.hh | 1 + include/gz/physics/Joint.hh | 6 ++++++ include/gz/physics/detail/Joint.hh | 3 ++- test/common_test/joint_mimic_features.cc | 12 ++++++------ 5 files changed, 16 insertions(+), 7 deletions(-) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index a581c5016..7c435681d 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -727,6 +727,7 @@ Wrench3d JointFeatures::GetJointTransmittedWrenchInJointFrame( ///////////////////////////////////////////////// void JointFeatures::SetJointMimicConstraint( const Identity &_id, + const std::size_t /*_dof*/, // follower dof not supported by dart API yet const std::string _joint, const std::string, const double _multiplier, diff --git a/dartsim/src/JointFeatures.hh b/dartsim/src/JointFeatures.hh index 52f1248fb..2a7ceebac 100644 --- a/dartsim/src/JointFeatures.hh +++ b/dartsim/src/JointFeatures.hh @@ -209,6 +209,7 @@ class JointFeatures : // ----- Mimic joint constraint ----- public: void SetJointMimicConstraint( const Identity &_id, + const std::size_t _dof, const std::string _joint, const std::string _axis, const double _multiplier, diff --git a/include/gz/physics/Joint.hh b/include/gz/physics/Joint.hh index 2fe69d10d..ef8e4de71 100644 --- a/include/gz/physics/Joint.hh +++ b/include/gz/physics/Joint.hh @@ -592,6 +592,10 @@ namespace gz : public virtual Feature { /// \brief The Joint API for setting the mimic joint constraint. + /// \param[in] _dof + /// The desired generalized coordinate corresponding to the follower + /// axis within this joint. Values start from 0 and stop before + /// Joint::GetDegreesOfFreedom(). /// \param[in] _joint /// name of the joint to be mimicked, i.e. the leader joint. /// \param[in] _multiplier @@ -605,6 +609,7 @@ namespace gz public: using Scalar = typename PolicyT::Scalar; public: void SetMimicConstraint( + const std::size_t _dof, const std::string _joint, const std::string _axis, Scalar _multiplier, @@ -621,6 +626,7 @@ namespace gz // See Joint::MimicConstraint above public: virtual void SetJointMimicConstraint( const Identity &_id, + const std::size_t _dof, const std::string _joint, const std::string _axis, Scalar _multiplier, Scalar _offset, Scalar _reference) = 0; diff --git a/include/gz/physics/detail/Joint.hh b/include/gz/physics/detail/Joint.hh index a20193690..6c4a453d8 100644 --- a/include/gz/physics/detail/Joint.hh +++ b/include/gz/physics/detail/Joint.hh @@ -220,12 +220,13 @@ namespace gz template void SetMimicConstraintFeature::Joint:: SetMimicConstraint( + const std::size_t _dof, const std::string _joint, const std::string _axis, Scalar _multiplier, Scalar _offset, Scalar _reference) { this->template Interface() - ->SetJointMimicConstraint(this->identity, _joint, + ->SetJointMimicConstraint(this->identity, _dof, _joint, _axis, _multiplier, _offset, _reference); } diff --git a/test/common_test/joint_mimic_features.cc b/test/common_test/joint_mimic_features.cc index fae5ac65b..84d12cba9 100644 --- a/test/common_test/joint_mimic_features.cc +++ b/test/common_test/joint_mimic_features.cc @@ -177,13 +177,13 @@ TEST_F(JointMimicFeatureTest, PrismaticRevoluteMimicTest) // prismatic_joint_1 -> revolute_joint_1 // revolute_joint_1 --> revolute_joint_2 // revolute_joint_1 --> prismatic_joint_3 - prismaticFollowerJoint1->SetMimicConstraint("prismatic_joint_1", "axis", multiplier, + prismaticFollowerJoint1->SetMimicConstraint(0, "prismatic_joint_1", "axis", multiplier, offset, reference); - revoluteFollowerJoint1->SetMimicConstraint("prismatic_joint_1", "axis" , multiplier, + revoluteFollowerJoint1->SetMimicConstraint(0, "prismatic_joint_1", "axis" , multiplier, offset, reference); - revoluteFollowerJoint2->SetMimicConstraint("revolute_joint_1", "axis", multiplier, + revoluteFollowerJoint2->SetMimicConstraint(0, "revolute_joint_1", "axis", multiplier, offset, reference); - prismaticFollowerJoint2->SetMimicConstraint("revolute_joint_1", "axis", multiplier, + prismaticFollowerJoint2->SetMimicConstraint(0, "revolute_joint_1", "axis", multiplier, offset, reference); // Reset positions and run a few iterations so the positions reach nontrivial values. @@ -287,7 +287,7 @@ TEST_F(JointMimicFeatureTest, UniversalMimicTest) auto testMimicFcn = [&](double multiplier, double offset, double reference) { // Set mimic joint constraint. - followerJoint->SetMimicConstraint("universal_joint_1", "axis", multiplier, offset, reference); + followerJoint->SetMimicConstraint(0, "universal_joint_1", "axis", multiplier, offset, reference); // Reset positions and run a few iterations so the positions reach nontrivial values. leaderJoint->SetPosition(0, 0); leaderJoint->SetPosition(1, 0); @@ -377,7 +377,7 @@ TEST_F(JointMimicFeatureTest, PendulumMimicTest) auto testMimicFcn = [&](double multiplier, double offset, double reference) { // Set mimic joint constraint. - followerJoint->SetMimicConstraint("upper_joint_1", "axis" , multiplier, offset, reference); + followerJoint->SetMimicConstraint(0, "upper_joint_1", "axis" , multiplier, offset, reference); // Reset positions and run a few iterations so the positions reach nontrivial values. leaderJoint->SetPosition(0, 0); followerJoint->SetPosition(0, 0); From a7e4513a090b19089954edb24d07ce4a9a3083d3 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 13 Jul 2023 16:15:00 -0700 Subject: [PATCH 21/21] Use const ref Signed-off-by: Steve Peters --- dartsim/src/JointFeatures.cc | 4 ++-- dartsim/src/JointFeatures.hh | 4 ++-- include/gz/physics/Joint.hh | 8 ++++---- include/gz/physics/detail/Joint.hh | 4 ++-- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index 7c435681d..568d48efb 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -728,8 +728,8 @@ Wrench3d JointFeatures::GetJointTransmittedWrenchInJointFrame( void JointFeatures::SetJointMimicConstraint( const Identity &_id, const std::size_t /*_dof*/, // follower dof not supported by dart API yet - const std::string _joint, - const std::string, + const std::string &_joint, + const std::string &/*_axis*/, const double _multiplier, const double _offset, const double _reference) diff --git a/dartsim/src/JointFeatures.hh b/dartsim/src/JointFeatures.hh index 2a7ceebac..bf3366524 100644 --- a/dartsim/src/JointFeatures.hh +++ b/dartsim/src/JointFeatures.hh @@ -210,8 +210,8 @@ class JointFeatures : public: void SetJointMimicConstraint( const Identity &_id, const std::size_t _dof, - const std::string _joint, - const std::string _axis, + const std::string &_joint, + const std::string &_axis, const double _multiplier, const double _offset, const double _reference) override; diff --git a/include/gz/physics/Joint.hh b/include/gz/physics/Joint.hh index ef8e4de71..3ce011ce1 100644 --- a/include/gz/physics/Joint.hh +++ b/include/gz/physics/Joint.hh @@ -610,8 +610,8 @@ namespace gz public: void SetMimicConstraint( const std::size_t _dof, - const std::string _joint, - const std::string _axis, + const std::string &_joint, + const std::string &_axis, Scalar _multiplier, Scalar _offset, Scalar _reference); @@ -627,8 +627,8 @@ namespace gz public: virtual void SetJointMimicConstraint( const Identity &_id, const std::size_t _dof, - const std::string _joint, - const std::string _axis, + const std::string &_joint, + const std::string &_axis, Scalar _multiplier, Scalar _offset, Scalar _reference) = 0; }; }; diff --git a/include/gz/physics/detail/Joint.hh b/include/gz/physics/detail/Joint.hh index 6c4a453d8..4cc4c3cc1 100644 --- a/include/gz/physics/detail/Joint.hh +++ b/include/gz/physics/detail/Joint.hh @@ -221,8 +221,8 @@ namespace gz void SetMimicConstraintFeature::Joint:: SetMimicConstraint( const std::size_t _dof, - const std::string _joint, - const std::string _axis, + const std::string &_joint, + const std::string &_axis, Scalar _multiplier, Scalar _offset, Scalar _reference) { this->template Interface()