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); } }