Added reference to offset

Signed-off-by: Aditya <[email protected]>
gazebosim · Oct 11, 2022 · 943740c · 943740c
1 parent 1317a6d
commit 943740c
Show file tree

Hide file tree

Showing 5 changed files with 45 additions and 34 deletions.
diff --git 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<JointInfo>(_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
@@ -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
@@ -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
@@ -221,11 +221,11 @@ namespace gz
     void SetMimicConstraintFeature::Joint<PolicyT, FeaturesT>::
     SetMimicConstraint(
       const std::string _mimicJoint,
-      Scalar _multiplier, Scalar _offset)
+      Scalar _multiplier, Scalar _offset, Scalar _reference)
     {
       this->template Interface<SetMimicConstraintFeature>()
         ->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
@@ -1912,18 +1912,22 @@ 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
         // 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);
+        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);
   }
 }