From bf36c0dc6c8aa2cab2133ee13e2a87a0984044a3 Mon Sep 17 00:00:00 2001
From: Aditya <aditya050995@gmail.com>
Date: Mon, 12 Dec 2022 15:43:48 -0800
Subject: [PATCH 01/14] Rough draft of approach

Signed-off-by: Aditya <aditya050995@gmail.com>
---
 src/SdfEntityCreator.cc        |  3 +++
 src/systems/physics/Physics.cc | 41 +++++++++++++++++++++++++++++++++-
 2 files changed, 43 insertions(+), 1 deletion(-)

diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc
index cc6b374980..9921b7ef5a 100644
--- a/src/SdfEntityCreator.cc
+++ b/src/SdfEntityCreator.cc
@@ -663,6 +663,7 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint)
 Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint,
     bool _resolved)
 {
+  std::cout << "sdf ec : dbg 1, creating joint entity from SDF" << std::endl;
   GZ_PROFILE("SdfEntityCreator::CreateEntities(sdf::Joint)");
 
   // Entity
@@ -686,6 +687,7 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint,
 
   if (_joint->Axis(0))
   {
+    std::cout << "Creating jt axis 0 component" << std::endl;
     auto resolvedAxis = ResolveJointAxis(*_joint->Axis(0));
     if (!resolvedAxis)
     {
@@ -700,6 +702,7 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint,
 
   if (_joint->Axis(1))
   {
+    std::cout << "Creating jt axis 1 component" << std::endl;
     auto resolvedAxis = ResolveJointAxis(*_joint->Axis(1));
     if (!resolvedAxis)
     {
diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index 1a83e6a906..f3190a3c31 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -469,7 +469,8 @@ class gz::sim::systems::PhysicsPrivate
             physics::GetJointFromModel,
             physics::GetBasicJointProperties,
             physics::GetBasicJointState,
-            physics::SetBasicJointState>{};
+            physics::SetBasicJointState,
+            physics::SetMimicConstraintFeature>{};
 
   /// \brief Feature list to construct joints
   public: struct ConstructSdfJointFeatureList : gz::physics::FeatureList<
@@ -1124,6 +1125,7 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm,
               nestedModelFeature->ConstructNestedModel(*root.Model());
             if (modelPtrPhys)
             {
+              std::cout << "modelPtrPhys constructed from SDF using ConstructNestedModel !" << std::endl;
               this->entityModelMap.AddEntity(_entity, modelPtrPhys);
               this->topLevelModelMap.insert(std::make_pair(_entity,
                   topLevelModel(_entity, _ecm)));
@@ -1131,6 +1133,7 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm,
           }
           else
           {
+            std::cout << "modelPtrPhys constructed from SDF using ConstructModel !" << std::endl;
             auto modelPtrPhys = worldPtrPhys->ConstructModel(*root.Model());
             if (modelPtrPhys)
             {
@@ -1617,6 +1620,8 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
                components::Pose, components::ThreadPitch,
                components::ParentEntity, components::ParentLinkName,
                components::ChildLinkName>(
+               /* components::JointAxis, */
+               /* components::JointAxis2>( */
       [&](const Entity &_entity,
           const components::Joint * /* _joint */,
           const components::Name *_name,
@@ -1626,7 +1631,10 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           const components::ParentEntity *_parentModel,
           const components::ParentLinkName *_parentLinkName,
           const components::ChildLinkName *_childLinkName) -> bool
+          /* const components::JointAxis *_jointAxis, */
+          /* const components::JointAxis2 *_jointAxis2) -> bool */
       {
+        std::cout << "DBG 1" << std::endl;
         // If the parent model is scheduled for recreation, then do not
         // try to create a new joint. This situation can occur when a joint
         // is added to a model from the GUI model editor.
@@ -1636,6 +1644,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           // Add this entity to the set of newly added joints to existing
           // models.
           this->jointAddedToModel.insert(_entity);
+          std::cout << "DBG 2" << std::endl;
           return true;
         }
 
@@ -1648,6 +1657,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
                     << "] marked as new, but it's already on the map."
                     << std::endl;
           }
+          std::cout << "DBG 3" << std::endl;
           return true;
         }
 
@@ -1656,6 +1666,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
         {
           gzerr << "Joint's parent model entity [" << _parentModel->Data()
                   << "] not found on model map." << std::endl;
+          std::cout << "DBG 4" << std::endl;
           return true;
         }
 
@@ -1674,6 +1685,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           }
 
           // Skip all other attempts to create joints
+          std::cout << "DBG 5" << std::endl;
           return false;
         }
 
@@ -1682,6 +1694,29 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
         {
           // No need to create this joint because it was already created when
           // parsing the model.
+          std::cout << "Add to entity joint map" << std::endl;
+          // Check if mimic joint constraint should be applied to this joint.
+          auto jointAxis = _ecm.Component<components::JointAxis>(_entity);
+          auto jointAxis2 = _ecm.Component<components::JointAxis2>(_entity);
+
+          if (jointAxis)
+          {
+            if (auto mimicJoint = jointAxis->Data().MimicJoint())
+            {
+              existingJoint->SetMimicConstraint(mimicJoint->Joint(),
+                  mimicJoint->Multiplier(), mimicJoint->Offset(), mimicJoint->Reference());
+            }
+          }
+
+          if (jointAxis2)
+          {
+            if (auto mimicJoint = jointAxis2->Data().MimicJoint())
+            {
+              existingJoint->SetMimicConstraint(mimicJoint->Joint(),
+                  mimicJoint->Multiplier(), mimicJoint->Offset(), mimicJoint->Reference());
+            }
+          }
+
           this->entityJointMap.AddEntity(_entity, existingJoint);
           this->topLevelModelMap.insert(
             std::make_pair(_entity, topLevelModel(_entity, _ecm)));
@@ -1696,9 +1731,11 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           gzerr << "Attempting to create a new joint [" << _name->Data()
                 << "], but the physics engine doesn't support constructing "
                 << "joints at runtime." << std::endl;
+          std::cout << "DBG 7" << std::endl;
           return true;
         }
 
+        std::cout << "DBG 8 : Physics.cc making joint component out of sdf" << std::endl;
         sdf::Joint joint;
         joint.SetName(_name->Data());
         joint.SetType(_jointType->Data());
@@ -1726,6 +1763,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
         {
           // Some joints may not be supported, so only add them to the map if
           // the physics entity is valid
+          std::cout << "DBG 12" << std::endl;
           this->entityJointMap.AddEntity(_entity, jointPtrPhys);
           this->topLevelModelMap.insert(std::make_pair(_entity,
               topLevelModel(_entity, _ecm)));
@@ -1813,6 +1851,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
 
           gzdbg << "Creating detachable joint [" << _entity << "]"
                  << std::endl;
+          std::cout << "DBG 10" << std::endl;
           this->entityJointMap.AddEntity(_entity, jointPtrPhys);
           this->topLevelModelMap.insert(std::make_pair(_entity,
               topLevelModel(_entity, _ecm)));

From bd9105eefec1880e4519100ef924f04ab31ec109 Mon Sep 17 00:00:00 2001
From: Aditya <aditya050995@gmail.com>
Date: Mon, 12 Dec 2022 16:09:35 -0800
Subject: [PATCH 02/14] Clean debug statements

Signed-off-by: Aditya <aditya050995@gmail.com>
---
 src/SdfEntityCreator.cc        |  3 ---
 src/systems/physics/Physics.cc | 17 +----------------
 2 files changed, 1 insertion(+), 19 deletions(-)

diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc
index 9921b7ef5a..cc6b374980 100644
--- a/src/SdfEntityCreator.cc
+++ b/src/SdfEntityCreator.cc
@@ -663,7 +663,6 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint)
 Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint,
     bool _resolved)
 {
-  std::cout << "sdf ec : dbg 1, creating joint entity from SDF" << std::endl;
   GZ_PROFILE("SdfEntityCreator::CreateEntities(sdf::Joint)");
 
   // Entity
@@ -687,7 +686,6 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint,
 
   if (_joint->Axis(0))
   {
-    std::cout << "Creating jt axis 0 component" << std::endl;
     auto resolvedAxis = ResolveJointAxis(*_joint->Axis(0));
     if (!resolvedAxis)
     {
@@ -702,7 +700,6 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint,
 
   if (_joint->Axis(1))
   {
-    std::cout << "Creating jt axis 1 component" << std::endl;
     auto resolvedAxis = ResolveJointAxis(*_joint->Axis(1));
     if (!resolvedAxis)
     {
diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index f3190a3c31..5ed0e8adfc 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -1125,7 +1125,6 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm,
               nestedModelFeature->ConstructNestedModel(*root.Model());
             if (modelPtrPhys)
             {
-              std::cout << "modelPtrPhys constructed from SDF using ConstructNestedModel !" << std::endl;
               this->entityModelMap.AddEntity(_entity, modelPtrPhys);
               this->topLevelModelMap.insert(std::make_pair(_entity,
                   topLevelModel(_entity, _ecm)));
@@ -1133,7 +1132,6 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm,
           }
           else
           {
-            std::cout << "modelPtrPhys constructed from SDF using ConstructModel !" << std::endl;
             auto modelPtrPhys = worldPtrPhys->ConstructModel(*root.Model());
             if (modelPtrPhys)
             {
@@ -1620,8 +1618,6 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
                components::Pose, components::ThreadPitch,
                components::ParentEntity, components::ParentLinkName,
                components::ChildLinkName>(
-               /* components::JointAxis, */
-               /* components::JointAxis2>( */
       [&](const Entity &_entity,
           const components::Joint * /* _joint */,
           const components::Name *_name,
@@ -1631,10 +1627,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           const components::ParentEntity *_parentModel,
           const components::ParentLinkName *_parentLinkName,
           const components::ChildLinkName *_childLinkName) -> bool
-          /* const components::JointAxis *_jointAxis, */
-          /* const components::JointAxis2 *_jointAxis2) -> bool */
       {
-        std::cout << "DBG 1" << std::endl;
         // If the parent model is scheduled for recreation, then do not
         // try to create a new joint. This situation can occur when a joint
         // is added to a model from the GUI model editor.
@@ -1644,7 +1637,6 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           // Add this entity to the set of newly added joints to existing
           // models.
           this->jointAddedToModel.insert(_entity);
-          std::cout << "DBG 2" << std::endl;
           return true;
         }
 
@@ -1657,7 +1649,6 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
                     << "] marked as new, but it's already on the map."
                     << std::endl;
           }
-          std::cout << "DBG 3" << std::endl;
           return true;
         }
 
@@ -1666,7 +1657,6 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
         {
           gzerr << "Joint's parent model entity [" << _parentModel->Data()
                   << "] not found on model map." << std::endl;
-          std::cout << "DBG 4" << std::endl;
           return true;
         }
 
@@ -1685,7 +1675,6 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           }
 
           // Skip all other attempts to create joints
-          std::cout << "DBG 5" << std::endl;
           return false;
         }
 
@@ -1694,7 +1683,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
         {
           // No need to create this joint because it was already created when
           // parsing the model.
-          std::cout << "Add to entity joint map" << std::endl;
+
           // Check if mimic joint constraint should be applied to this joint.
           auto jointAxis = _ecm.Component<components::JointAxis>(_entity);
           auto jointAxis2 = _ecm.Component<components::JointAxis2>(_entity);
@@ -1731,11 +1720,9 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           gzerr << "Attempting to create a new joint [" << _name->Data()
                 << "], but the physics engine doesn't support constructing "
                 << "joints at runtime." << std::endl;
-          std::cout << "DBG 7" << std::endl;
           return true;
         }
 
-        std::cout << "DBG 8 : Physics.cc making joint component out of sdf" << std::endl;
         sdf::Joint joint;
         joint.SetName(_name->Data());
         joint.SetType(_jointType->Data());
@@ -1763,7 +1750,6 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
         {
           // Some joints may not be supported, so only add them to the map if
           // the physics entity is valid
-          std::cout << "DBG 12" << std::endl;
           this->entityJointMap.AddEntity(_entity, jointPtrPhys);
           this->topLevelModelMap.insert(std::make_pair(_entity,
               topLevelModel(_entity, _ecm)));
@@ -1851,7 +1837,6 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
 
           gzdbg << "Creating detachable joint [" << _entity << "]"
                  << std::endl;
-          std::cout << "DBG 10" << std::endl;
           this->entityJointMap.AddEntity(_entity, jointPtrPhys);
           this->topLevelModelMap.insert(std::make_pair(_entity,
               topLevelModel(_entity, _ecm)));

From fe36965c498a6deec5c9b4f6b65340c905d3e134 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Sat, 17 Jun 2023 20:57:47 -0700
Subject: [PATCH 03/14] Use updated API to fix build

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 src/systems/physics/Physics.cc | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index 5ed0e8adfc..980b0ad16a 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -1690,19 +1690,19 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
 
           if (jointAxis)
           {
-            if (auto mimicJoint = jointAxis->Data().MimicJoint())
+            if (auto mimic = jointAxis->Data().Mimic())
             {
-              existingJoint->SetMimicConstraint(mimicJoint->Joint(),
-                  mimicJoint->Multiplier(), mimicJoint->Offset(), mimicJoint->Reference());
+              existingJoint->SetMimicConstraint(mimic->Joint(), mimic->Axis(),
+                  mimic->Multiplier(), mimic->Offset(), mimic->Reference());
             }
           }
 
           if (jointAxis2)
           {
-            if (auto mimicJoint = jointAxis2->Data().MimicJoint())
+            if (auto mimic = jointAxis2->Data().Mimic())
             {
-              existingJoint->SetMimicConstraint(mimicJoint->Joint(),
-                  mimicJoint->Multiplier(), mimicJoint->Offset(), mimicJoint->Reference());
+              existingJoint->SetMimicConstraint(mimic->Joint(), mimic->Axis(),
+                  mimic->Multiplier(), mimic->Offset(), mimic->Reference());
             }
           }
 

From 554f99dff0bfc3db356433a6c7bd5c8997336905 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Sat, 17 Jun 2023 20:58:30 -0700
Subject: [PATCH 04/14] Add example mimic_pendulum_world.sdf

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 examples/worlds/mimic_pendulum_world.sdf | 655 +++++++++++++++++++++++
 1 file changed, 655 insertions(+)
 create mode 100644 examples/worlds/mimic_pendulum_world.sdf

diff --git a/examples/worlds/mimic_pendulum_world.sdf b/examples/worlds/mimic_pendulum_world.sdf
new file mode 100644
index 0000000000..7bcbdfa428
--- /dev/null
+++ b/examples/worlds/mimic_pendulum_world.sdf
@@ -0,0 +1,655 @@
+<?xml version="1.0" ?>
+<!--
+  Gazebo Mimic constraint demo
+
+-->
+<sdf version="1.6">
+  <world name="default">
+    <physics name="1ms" type="ode">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>0</real_time_factor>
+    </physics>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <model name="pendulum_with_base">
+      <pose>0 0 0 0 0 0</pose>
+      <link name="base">
+        <inertial>
+          <mass>100</mass>
+        </inertial>
+        <visual name="vis_plate_on_ground">
+          <pose>0 0 0.01 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.8</radius>
+              <length>0.02</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <visual name="vis_pole">
+          <pose>-0.275 0 1.1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.2 0.2 2.2</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <collision name="col_plate_on_ground">
+          <pose>0 0 0.01 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.8</radius>
+              <length>0.02</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <collision name="col_pole">
+          <pose>-0.275 0 1.1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.2 0.2 2.2</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+
+      <!-- upper link 1, length 1, IC -90 degrees -->
+      <link name="upper_link_1">
+        <pose>0 0 2.1 -1.5708 0 0</pose>
+        <self_collide>0</self_collide>
+        <inertial>
+          <mass>1.0</mass>
+          <pose>0 0 0.5 0 0 0</pose>
+          <inertia>
+            <ixx>0.087</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.087</iyy>
+            <iyz>0</iyz>
+            <izz>0.006</izz>
+          </inertia>
+        </inertial>
+        <visual name="vis_upper_joint">
+          <pose>-0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <visual name="vis_cylinder">
+          <pose>0 0 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>1.0</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <collision name="col_upper_joint">
+          <pose>-0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <collision name="col_cylinder">
+          <pose>0 0 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>1.0</length>
+            </cylinder>
+          </geometry>
+        </collision>
+      </link>
+
+      <!-- upper link 2, length 0.5, IC -90 degrees -->
+      <link name="upper_link_2">
+        <pose>-0.5 0 2.1 -1.5708 0 0</pose>
+        <self_collide>0</self_collide>
+        <inertial>
+          <mass>1.0</mass>
+          <pose>0 0 0.25 0 0 0</pose>
+          <inertia>
+            <ixx>0.024</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.024</iyy>
+            <iyz>0</iyz>
+            <izz>0.006</izz>
+          </inertia>
+        </inertial>
+        <visual name="vis_upper_joint_2">
+          <pose>0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <visual name="vis_cylinder_2">
+          <pose>0 0 0.25 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.5</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <collision name="col_upper_joint_2">
+          <pose>0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <collision name="col_cylinder_2">
+          <pose>0 0 0.2 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.5</length>
+            </cylinder>
+          </geometry>
+        </collision>
+      </link>
+
+      <joint name="upper_joint_1" type="revolute">
+        <parent>base</parent>
+        <child>upper_link_1</child>
+        <axis>
+          <xyz>1.0 0 0</xyz>
+        </axis>
+      </joint>
+
+      <joint name="upper_joint_2" type="revolute">
+        <parent>base</parent>
+        <child>upper_link_2</child>
+        <axis>
+          <xyz>1.0 0 0</xyz>
+        </axis>
+      </joint>
+
+    </model>
+
+    <model name="pendulum_with_base_mimic_big_follows_small">
+      <pose>0 3 0 0 0 0</pose>
+      <link name="base">
+        <inertial>
+          <mass>100</mass>
+        </inertial>
+        <visual name="vis_plate_on_ground">
+          <pose>0 0 0.01 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.8</radius>
+              <length>0.02</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <visual name="vis_pole">
+          <pose>-0.275 0 1.1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.2 0.2 2.2</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <collision name="col_plate_on_ground">
+          <pose>0 0 0.01 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.8</radius>
+              <length>0.02</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <collision name="col_pole">
+          <pose>-0.275 0 1.1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.2 0.2 2.2</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+
+      <!-- upper link 1, length 1, IC -90 degrees -->
+      <link name="upper_link_1">
+        <pose>0 0 2.1 -1.5708 0 0</pose>
+        <self_collide>0</self_collide>
+        <inertial>
+          <mass>1.0</mass>
+          <pose>0 0 0.5 0 0 0</pose>
+          <inertia>
+            <ixx>0.087</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.087</iyy>
+            <iyz>0</iyz>
+            <izz>0.006</izz>
+          </inertia>
+        </inertial>
+        <visual name="vis_upper_joint">
+          <pose>-0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <visual name="vis_cylinder">
+          <pose>0 0 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>1.0</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <collision name="col_upper_joint">
+          <pose>-0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <collision name="col_cylinder">
+          <pose>0 0 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>1.0</length>
+            </cylinder>
+          </geometry>
+        </collision>
+      </link>
+
+      <!-- upper link 2, length 0.5, IC -90 degrees -->
+      <link name="upper_link_2">
+        <pose>-0.5 0 2.1 -1.5708 0 0</pose>
+        <self_collide>0</self_collide>
+        <inertial>
+          <mass>1.0</mass>
+          <pose>0 0 0.25 0 0 0</pose>
+          <inertia>
+            <ixx>0.024</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.024</iyy>
+            <iyz>0</iyz>
+            <izz>0.006</izz>
+          </inertia>
+        </inertial>
+        <visual name="vis_upper_joint_2">
+          <pose>0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <visual name="vis_cylinder_2">
+          <pose>0 0 0.25 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.5</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <collision name="col_upper_joint_2">
+          <pose>0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <collision name="col_cylinder_2">
+          <pose>0 0 0.2 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.5</length>
+            </cylinder>
+          </geometry>
+        </collision>
+      </link>
+
+      <joint name="upper_joint_1" type="revolute">
+        <parent>base</parent>
+        <child>upper_link_1</child>
+        <axis>
+          <xyz>1.0 0 0</xyz>
+          <mimic joint="upper_joint_2" axis="axis">
+            <multiplier>1</multiplier>
+          </mimic>
+        </axis>
+      </joint>
+
+      <joint name="upper_joint_2" type="revolute">
+        <parent>base</parent>
+        <child>upper_link_2</child>
+        <axis>
+          <xyz>1.0 0 0</xyz>
+        </axis>
+      </joint>
+
+    </model>
+
+    <model name="pendulum_with_base_mimic_small_follows_big">
+      <pose>0 -3 0 0 0 0</pose>
+      <link name="base">
+        <inertial>
+          <mass>100</mass>
+        </inertial>
+        <visual name="vis_plate_on_ground">
+          <pose>0 0 0.01 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.8</radius>
+              <length>0.02</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <visual name="vis_pole">
+          <pose>-0.275 0 1.1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.2 0.2 2.2</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <collision name="col_plate_on_ground">
+          <pose>0 0 0.01 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.8</radius>
+              <length>0.02</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <collision name="col_pole">
+          <pose>-0.275 0 1.1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.2 0.2 2.2</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+
+      <!-- upper link 1, length 1, IC -90 degrees -->
+      <link name="upper_link_1">
+        <pose>0 0 2.1 -1.5708 0 0</pose>
+        <self_collide>0</self_collide>
+        <inertial>
+          <mass>1.0</mass>
+          <pose>0 0 0.5 0 0 0</pose>
+          <inertia>
+            <ixx>0.087</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.087</iyy>
+            <iyz>0</iyz>
+            <izz>0.006</izz>
+          </inertia>
+        </inertial>
+        <visual name="vis_upper_joint">
+          <pose>-0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <visual name="vis_cylinder">
+          <pose>0 0 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>1.0</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <collision name="col_upper_joint">
+          <pose>-0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <collision name="col_cylinder">
+          <pose>0 0 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>1.0</length>
+            </cylinder>
+          </geometry>
+        </collision>
+      </link>
+
+      <!-- upper link 2, length 0.5, IC -90 degrees -->
+      <link name="upper_link_2">
+        <pose>-0.5 0 2.1 -1.5708 0 0</pose>
+        <self_collide>0</self_collide>
+        <inertial>
+          <mass>1.0</mass>
+          <pose>0 0 0.25 0 0 0</pose>
+          <inertia>
+            <ixx>0.024</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.024</iyy>
+            <iyz>0</iyz>
+            <izz>0.006</izz>
+          </inertia>
+        </inertial>
+        <visual name="vis_upper_joint_2">
+          <pose>0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <visual name="vis_cylinder_2">
+          <pose>0 0 0.25 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.5</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <collision name="col_upper_joint_2">
+          <pose>0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <collision name="col_cylinder_2">
+          <pose>0 0 0.2 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.5</length>
+            </cylinder>
+          </geometry>
+        </collision>
+      </link>
+
+      <joint name="upper_joint_1" type="revolute">
+        <parent>base</parent>
+        <child>upper_link_1</child>
+        <axis>
+          <xyz>1.0 0 0</xyz>
+        </axis>
+      </joint>
+
+      <joint name="upper_joint_2" type="revolute">
+        <parent>base</parent>
+        <child>upper_link_2</child>
+        <axis>
+          <xyz>1.0 0 0</xyz>
+          <mimic joint="upper_joint_1" axis="axis">
+            <multiplier>1</multiplier>
+          </mimic>
+        </axis>
+      </joint>
+
+    </model>
+
+  </world>
+</sdf>

From 16ca03346196bddc816c784c3812b83ac4e8f99a Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Fri, 14 Jul 2023 10:03:36 -0700
Subject: [PATCH 05/14] Rename example world and links, joints

Based on changes made in gz-physics test world.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 ...df => mimic_fast_slow_pendulums_world.sdf} | 92 +++++++++----------
 1 file changed, 46 insertions(+), 46 deletions(-)
 rename examples/worlds/{mimic_pendulum_world.sdf => mimic_fast_slow_pendulums_world.sdf} (89%)

diff --git a/examples/worlds/mimic_pendulum_world.sdf b/examples/worlds/mimic_fast_slow_pendulums_world.sdf
similarity index 89%
rename from examples/worlds/mimic_pendulum_world.sdf
rename to examples/worlds/mimic_fast_slow_pendulums_world.sdf
index 7bcbdfa428..9d70386485 100644
--- a/examples/worlds/mimic_pendulum_world.sdf
+++ b/examples/worlds/mimic_fast_slow_pendulums_world.sdf
@@ -103,8 +103,8 @@
         </collision>
       </link>
 
-      <!-- upper link 1, length 1, IC -90 degrees -->
-      <link name="upper_link_1">
+      <!-- slow link, length 1, IC -90 degrees -->
+      <link name="slow_link">
         <pose>0 0 2.1 -1.5708 0 0</pose>
         <self_collide>0</self_collide>
         <inertial>
@@ -119,7 +119,7 @@
             <izz>0.006</izz>
           </inertia>
         </inertial>
-        <visual name="vis_upper_joint">
+        <visual name="vis_slow_joint">
           <pose>-0.05 0 0 0 1.5708 0</pose>
           <geometry>
             <cylinder>
@@ -147,7 +147,7 @@
             <specular>0.8 0.8 0.8 1</specular>
           </material>
         </visual>
-        <collision name="col_upper_joint">
+        <collision name="col_slow_joint">
           <pose>-0.05 0 0 0 1.5708 0</pose>
           <geometry>
             <cylinder>
@@ -167,8 +167,8 @@
         </collision>
       </link>
 
-      <!-- upper link 2, length 0.5, IC -90 degrees -->
-      <link name="upper_link_2">
+      <!-- fast link, length 0.5, IC -90 degrees -->
+      <link name="fast_link">
         <pose>-0.5 0 2.1 -1.5708 0 0</pose>
         <self_collide>0</self_collide>
         <inertial>
@@ -183,7 +183,7 @@
             <izz>0.006</izz>
           </inertia>
         </inertial>
-        <visual name="vis_upper_joint_2">
+        <visual name="vis_fast_joint">
           <pose>0.05 0 0 0 1.5708 0</pose>
           <geometry>
             <cylinder>
@@ -211,7 +211,7 @@
             <specular>0.8 0.8 0.8 1</specular>
           </material>
         </visual>
-        <collision name="col_upper_joint_2">
+        <collision name="col_fast_joint">
           <pose>0.05 0 0 0 1.5708 0</pose>
           <geometry>
             <cylinder>
@@ -231,25 +231,25 @@
         </collision>
       </link>
 
-      <joint name="upper_joint_1" type="revolute">
+      <joint name="slow_joint" type="revolute">
         <parent>base</parent>
-        <child>upper_link_1</child>
+        <child>slow_link</child>
         <axis>
-          <xyz>1.0 0 0</xyz>
+          <xyz>-1.0 0 0</xyz>
         </axis>
       </joint>
 
-      <joint name="upper_joint_2" type="revolute">
+      <joint name="fast_joint" type="revolute">
         <parent>base</parent>
-        <child>upper_link_2</child>
+        <child>fast_link</child>
         <axis>
-          <xyz>1.0 0 0</xyz>
+          <xyz>-1.0 0 0</xyz>
         </axis>
       </joint>
 
     </model>
 
-    <model name="pendulum_with_base_mimic_big_follows_small">
+    <model name="pendulum_with_base_mimic_slow_follows_fast">
       <pose>0 3 0 0 0 0</pose>
       <link name="base">
         <inertial>
@@ -301,8 +301,8 @@
         </collision>
       </link>
 
-      <!-- upper link 1, length 1, IC -90 degrees -->
-      <link name="upper_link_1">
+      <!-- slow link, length 1, IC -90 degrees -->
+      <link name="slow_link">
         <pose>0 0 2.1 -1.5708 0 0</pose>
         <self_collide>0</self_collide>
         <inertial>
@@ -317,7 +317,7 @@
             <izz>0.006</izz>
           </inertia>
         </inertial>
-        <visual name="vis_upper_joint">
+        <visual name="vis_slow_joint">
           <pose>-0.05 0 0 0 1.5708 0</pose>
           <geometry>
             <cylinder>
@@ -345,7 +345,7 @@
             <specular>0.8 0.8 0.8 1</specular>
           </material>
         </visual>
-        <collision name="col_upper_joint">
+        <collision name="col_slow_joint">
           <pose>-0.05 0 0 0 1.5708 0</pose>
           <geometry>
             <cylinder>
@@ -365,8 +365,8 @@
         </collision>
       </link>
 
-      <!-- upper link 2, length 0.5, IC -90 degrees -->
-      <link name="upper_link_2">
+      <!-- fast link, length 0.5, IC -90 degrees -->
+      <link name="fast_link">
         <pose>-0.5 0 2.1 -1.5708 0 0</pose>
         <self_collide>0</self_collide>
         <inertial>
@@ -381,7 +381,7 @@
             <izz>0.006</izz>
           </inertia>
         </inertial>
-        <visual name="vis_upper_joint_2">
+        <visual name="vis_fast_joint">
           <pose>0.05 0 0 0 1.5708 0</pose>
           <geometry>
             <cylinder>
@@ -409,7 +409,7 @@
             <specular>0.8 0.8 0.8 1</specular>
           </material>
         </visual>
-        <collision name="col_upper_joint_2">
+        <collision name="col_fast_joint">
           <pose>0.05 0 0 0 1.5708 0</pose>
           <geometry>
             <cylinder>
@@ -429,28 +429,28 @@
         </collision>
       </link>
 
-      <joint name="upper_joint_1" type="revolute">
+      <joint name="slow_joint" type="revolute">
         <parent>base</parent>
-        <child>upper_link_1</child>
+        <child>slow_link</child>
         <axis>
-          <xyz>1.0 0 0</xyz>
-          <mimic joint="upper_joint_2" axis="axis">
+          <xyz>-1.0 0 0</xyz>
+          <mimic joint="fast_joint" axis="axis">
             <multiplier>1</multiplier>
           </mimic>
         </axis>
       </joint>
 
-      <joint name="upper_joint_2" type="revolute">
+      <joint name="fast_joint" type="revolute">
         <parent>base</parent>
-        <child>upper_link_2</child>
+        <child>fast_link</child>
         <axis>
-          <xyz>1.0 0 0</xyz>
+          <xyz>-1.0 0 0</xyz>
         </axis>
       </joint>
 
     </model>
 
-    <model name="pendulum_with_base_mimic_small_follows_big">
+    <model name="pendulum_with_base_mimic_fast_follows_slow">
       <pose>0 -3 0 0 0 0</pose>
       <link name="base">
         <inertial>
@@ -502,8 +502,8 @@
         </collision>
       </link>
 
-      <!-- upper link 1, length 1, IC -90 degrees -->
-      <link name="upper_link_1">
+      <!-- slow link, length 1, IC -90 degrees -->
+      <link name="slow_link">
         <pose>0 0 2.1 -1.5708 0 0</pose>
         <self_collide>0</self_collide>
         <inertial>
@@ -518,7 +518,7 @@
             <izz>0.006</izz>
           </inertia>
         </inertial>
-        <visual name="vis_upper_joint">
+        <visual name="vis_slow_joint">
           <pose>-0.05 0 0 0 1.5708 0</pose>
           <geometry>
             <cylinder>
@@ -546,7 +546,7 @@
             <specular>0.8 0.8 0.8 1</specular>
           </material>
         </visual>
-        <collision name="col_upper_joint">
+        <collision name="col_slow_joint">
           <pose>-0.05 0 0 0 1.5708 0</pose>
           <geometry>
             <cylinder>
@@ -566,8 +566,8 @@
         </collision>
       </link>
 
-      <!-- upper link 2, length 0.5, IC -90 degrees -->
-      <link name="upper_link_2">
+      <!-- fast link, length 0.5, IC -90 degrees -->
+      <link name="fast_link">
         <pose>-0.5 0 2.1 -1.5708 0 0</pose>
         <self_collide>0</self_collide>
         <inertial>
@@ -582,7 +582,7 @@
             <izz>0.006</izz>
           </inertia>
         </inertial>
-        <visual name="vis_upper_joint_2">
+        <visual name="vis_fast_joint">
           <pose>0.05 0 0 0 1.5708 0</pose>
           <geometry>
             <cylinder>
@@ -610,7 +610,7 @@
             <specular>0.8 0.8 0.8 1</specular>
           </material>
         </visual>
-        <collision name="col_upper_joint_2">
+        <collision name="col_fast_joint">
           <pose>0.05 0 0 0 1.5708 0</pose>
           <geometry>
             <cylinder>
@@ -630,20 +630,20 @@
         </collision>
       </link>
 
-      <joint name="upper_joint_1" type="revolute">
+      <joint name="slow_joint" type="revolute">
         <parent>base</parent>
-        <child>upper_link_1</child>
+        <child>slow_link</child>
         <axis>
-          <xyz>1.0 0 0</xyz>
+          <xyz>-1.0 0 0</xyz>
         </axis>
       </joint>
 
-      <joint name="upper_joint_2" type="revolute">
+      <joint name="fast_joint" type="revolute">
         <parent>base</parent>
-        <child>upper_link_2</child>
+        <child>fast_link</child>
         <axis>
-          <xyz>1.0 0 0</xyz>
-          <mimic joint="upper_joint_1" axis="axis">
+          <xyz>-1.0 0 0</xyz>
+          <mimic joint="slow_joint" axis="axis">
             <multiplier>1</multiplier>
           </mimic>
         </axis>

From 9039a221f50d5dc3859ae63f85b924bdf2993b74 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Fri, 14 Jul 2023 16:25:51 -0700
Subject: [PATCH 06/14] Call API with correct dof argument

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 src/systems/physics/Physics.cc | 16 ++++++++++++----
 1 file changed, 12 insertions(+), 4 deletions(-)

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index 76f1e612a5..f0a5aefb54 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -1710,8 +1710,12 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           {
             if (auto mimic = jointAxis->Data().Mimic())
             {
-              existingJoint->SetMimicConstraint(mimic->Joint(), mimic->Axis(),
-                  mimic->Multiplier(), mimic->Offset(), mimic->Reference());
+              existingJoint->SetMimicConstraint(0,
+                  mimic->Joint(),
+                  mimic->Axis(),
+                  mimic->Multiplier(),
+                  mimic->Offset(),
+                  mimic->Reference());
             }
           }
 
@@ -1719,8 +1723,12 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           {
             if (auto mimic = jointAxis2->Data().Mimic())
             {
-              existingJoint->SetMimicConstraint(mimic->Joint(), mimic->Axis(),
-                  mimic->Multiplier(), mimic->Offset(), mimic->Reference());
+              existingJoint->SetMimicConstraint(1,
+                  mimic->Joint(),
+                  mimic->Axis(),
+                  mimic->Multiplier(),
+                  mimic->Offset(),
+                  mimic->Reference());
             }
           }
 

From dfcd35e91607f470c1cac715d67bb4b2fedbe975 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Fri, 14 Jul 2023 17:52:59 -0700
Subject: [PATCH 07/14] Set real-time factor to 1

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 examples/worlds/mimic_fast_slow_pendulums_world.sdf | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/worlds/mimic_fast_slow_pendulums_world.sdf b/examples/worlds/mimic_fast_slow_pendulums_world.sdf
index 9d70386485..c0254fc18d 100644
--- a/examples/worlds/mimic_fast_slow_pendulums_world.sdf
+++ b/examples/worlds/mimic_fast_slow_pendulums_world.sdf
@@ -7,7 +7,7 @@
   <world name="default">
     <physics name="1ms" type="ode">
       <max_step_size>0.001</max_step_size>
-      <real_time_factor>0</real_time_factor>
+      <real_time_factor>1</real_time_factor>
     </physics>
 
     <light type="directional" name="sun">

From 16788614f66f48a1cb9b7ce94413733458195f0d Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Fri, 21 Jul 2023 21:28:28 -0700
Subject: [PATCH 08/14] Physics: make mimic constraint an optional feature

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 src/systems/physics/Physics.cc | 73 +++++++++++++++++++++++++++-------
 1 file changed, 58 insertions(+), 15 deletions(-)

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index f0a5aefb54..5372a0c7b2 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -470,14 +470,18 @@ class gz::sim::systems::PhysicsPrivate
             physics::GetJointFromModel,
             physics::GetBasicJointProperties,
             physics::GetBasicJointState,
-            physics::SetBasicJointState,
-            physics::SetMimicConstraintFeature>{};
+            physics::SetBasicJointState>{};
 
   /// \brief Feature list to construct joints
   public: struct ConstructSdfJointFeatureList : gz::physics::FeatureList<
             JointFeatureList,
             gz::physics::sdf::ConstructSdfJoint>{};
 
+  /// \brief Feature list for mimic constraints
+  public: struct MimicConstraintJointFeatureList : gz::physics::FeatureList<
+            JointFeatureList,
+            physics::SetMimicConstraintFeature>{};
+
   //////////////////////////////////////////////////
   // Detachable joints
 
@@ -681,6 +685,7 @@ class gz::sim::systems::PhysicsPrivate
             physics::Joint,
             JointFeatureList,
             DetachableJointFeatureList,
+            MimicConstraintJointFeatureList,
             JointVelocityCommandFeatureList,
             JointGetTransmittedWrenchFeatureList,
             JointPositionLimitsCommandFeatureList,
@@ -1702,7 +1707,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           // No need to create this joint because it was already created when
           // parsing the model.
 
-          // Check if mimic joint constraint should be applied to this joint.
+          // Check if mimic constraint should be applied to this joint's axes.
           auto jointAxis = _ecm.Component<components::JointAxis>(_entity);
           auto jointAxis2 = _ecm.Component<components::JointAxis2>(_entity);
 
@@ -1710,12 +1715,31 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           {
             if (auto mimic = jointAxis->Data().Mimic())
             {
-              existingJoint->SetMimicConstraint(0,
-                  mimic->Joint(),
-                  mimic->Axis(),
-                  mimic->Multiplier(),
-                  mimic->Offset(),
-                  mimic->Reference());
+              auto jointPtrMimic = this->entityJointMap
+                  .EntityCast<MimicConstraintJointFeatureList>(existingJoint);
+              if (jointPtrMimic)
+              {
+                jointPtrMimic->SetMimicConstraint(0,
+                    mimic->Joint(),
+                    mimic->Axis(),
+                    mimic->Multiplier(),
+                    mimic->Offset(),
+                    mimic->Reference());
+              }
+              else
+              {
+                static bool informed{false};
+                if (!informed)
+                {
+                  gzerr << "Attempting to create a mimic constraint for joint ["
+                        <<_name->Data()
+                        << "] but the chosen physics engine does not support "
+                        << "mimic constraints, so no constraint will be "
+                        << "created."
+                        << std::endl;
+                  informed = true;
+                }
+              }
             }
           }
 
@@ -1723,12 +1747,31 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           {
             if (auto mimic = jointAxis2->Data().Mimic())
             {
-              existingJoint->SetMimicConstraint(1,
-                  mimic->Joint(),
-                  mimic->Axis(),
-                  mimic->Multiplier(),
-                  mimic->Offset(),
-                  mimic->Reference());
+              auto jointPtrMimic = this->entityJointMap
+                  .EntityCast<MimicConstraintJointFeatureList>(existingJoint);
+              if (jointPtrMimic)
+              {
+                jointPtrMimic->SetMimicConstraint(1,
+                    mimic->Joint(),
+                    mimic->Axis(),
+                    mimic->Multiplier(),
+                    mimic->Offset(),
+                    mimic->Reference());
+              }
+              else
+              {
+                static bool informed{false};
+                if (!informed)
+                {
+                  gzerr << "Attempting to create a mimic constraint for joint ["
+                        <<_name->Data()
+                        << "] but the chosen physics engine does not support "
+                        << "mimic constraints, so no constraint will be "
+                        << "created."
+                        << std::endl;
+                  informed = true;
+                }
+              }
             }
           }
 

From 1ccabce8aad6f0c44b231f22187708170e8e4aca Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Thu, 10 Aug 2023 11:14:59 -0700
Subject: [PATCH 09/14] Deduplicate code

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 src/systems/physics/Physics.cc | 45 ++++++++++------------------------
 1 file changed, 13 insertions(+), 32 deletions(-)

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index 5372a0c7b2..a8fd5ad3df 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -479,7 +479,6 @@ class gz::sim::systems::PhysicsPrivate
 
   /// \brief Feature list for mimic constraints
   public: struct MimicConstraintJointFeatureList : gz::physics::FeatureList<
-            JointFeatureList,
             physics::SetMimicConstraintFeature>{};
 
   //////////////////////////////////////////////////
@@ -1708,50 +1707,32 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
           // parsing the model.
 
           // Check if mimic constraint should be applied to this joint's axes.
+          using AxisIndex = std::size_t;
+          std::map<AxisIndex, sdf::JointAxis> jointAxisByIndex;
           auto jointAxis = _ecm.Component<components::JointAxis>(_entity);
           auto jointAxis2 = _ecm.Component<components::JointAxis2>(_entity);
 
           if (jointAxis)
           {
-            if (auto mimic = jointAxis->Data().Mimic())
-            {
-              auto jointPtrMimic = this->entityJointMap
-                  .EntityCast<MimicConstraintJointFeatureList>(existingJoint);
-              if (jointPtrMimic)
-              {
-                jointPtrMimic->SetMimicConstraint(0,
-                    mimic->Joint(),
-                    mimic->Axis(),
-                    mimic->Multiplier(),
-                    mimic->Offset(),
-                    mimic->Reference());
-              }
-              else
-              {
-                static bool informed{false};
-                if (!informed)
-                {
-                  gzerr << "Attempting to create a mimic constraint for joint ["
-                        <<_name->Data()
-                        << "] but the chosen physics engine does not support "
-                        << "mimic constraints, so no constraint will be "
-                        << "created."
-                        << std::endl;
-                  informed = true;
-                }
-              }
-            }
+            jointAxisByIndex[0] = jointAxis->Data();
           }
 
           if (jointAxis2)
           {
-            if (auto mimic = jointAxis2->Data().Mimic())
+            jointAxisByIndex[1] = jointAxis2->Data();
+          }
+
+          for (auto &axisByIndex : jointAxisByIndex)
+          {
+            auto axisIndex = axisByIndex.first;
+            auto axis = axisByIndex.second;
+            if (auto mimic = axis.Mimic())
             {
               auto jointPtrMimic = this->entityJointMap
                   .EntityCast<MimicConstraintJointFeatureList>(existingJoint);
               if (jointPtrMimic)
               {
-                jointPtrMimic->SetMimicConstraint(1,
+                jointPtrMimic->SetMimicConstraint(axisIndex,
                     mimic->Joint(),
                     mimic->Axis(),
                     mimic->Multiplier(),
@@ -1764,7 +1745,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
                 if (!informed)
                 {
                   gzerr << "Attempting to create a mimic constraint for joint ["
-                        <<_name->Data()
+                        << _name->Data()
                         << "] but the chosen physics engine does not support "
                         << "mimic constraints, so no constraint will be "
                         << "created."

From e1448b451d1c5f82e1ce8d0d1710a3e57eb00efd Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Thu, 10 Aug 2023 13:19:31 -0700
Subject: [PATCH 10/14] Add to entityJointMap before adding mimics

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 src/systems/physics/Physics.cc | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index a8fd5ad3df..3980f2980d 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -1705,6 +1705,9 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
         {
           // No need to create this joint because it was already created when
           // parsing the model.
+          this->entityJointMap.AddEntity(_entity, existingJoint);
+          this->topLevelModelMap.insert(
+            std::make_pair(_entity, topLevelModel(_entity, _ecm)));
 
           // Check if mimic constraint should be applied to this joint's axes.
           using AxisIndex = std::size_t;
@@ -1756,9 +1759,6 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
             }
           }
 
-          this->entityJointMap.AddEntity(_entity, existingJoint);
-          this->topLevelModelMap.insert(
-            std::make_pair(_entity, topLevelModel(_entity, _ecm)));
           return true;
         }
 

From a3e88d8cbba80afad6499fcfa39327ec2f6e28a3 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Thu, 24 Aug 2023 23:04:42 -0700
Subject: [PATCH 11/14] Set example world SDFormat version to 1.11

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 examples/worlds/mimic_fast_slow_pendulums_world.sdf | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/worlds/mimic_fast_slow_pendulums_world.sdf b/examples/worlds/mimic_fast_slow_pendulums_world.sdf
index c0254fc18d..9d48636f30 100644
--- a/examples/worlds/mimic_fast_slow_pendulums_world.sdf
+++ b/examples/worlds/mimic_fast_slow_pendulums_world.sdf
@@ -3,7 +3,7 @@
   Gazebo Mimic constraint demo
 
 -->
-<sdf version="1.6">
+<sdf version="1.11">
   <world name="default">
     <physics name="1ms" type="ode">
       <max_step_size>0.001</max_step_size>

From 3efb2d6e88fc2eea334deda2e959ce0844b7720c Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Tue, 29 Aug 2023 12:38:30 -0700
Subject: [PATCH 12/14] Simplify variable definition in for loop

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 src/systems/physics/Physics.cc | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index 640cff1e86..57174d4af8 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -1705,10 +1705,8 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
             jointAxisByIndex[1] = jointAxis2->Data();
           }
 
-          for (auto &axisByIndex : jointAxisByIndex)
+          for (const auto &[axisIndex, axis] : jointAxisByIndex)
           {
-            auto axisIndex = axisByIndex.first;
-            auto axis = axisByIndex.second;
             if (auto mimic = axis.Mimic())
             {
               auto jointPtrMimic = this->entityJointMap

From e943e319bf0e925778a3d9bc0f1474bb95034b70 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Tue, 29 Aug 2023 12:38:49 -0700
Subject: [PATCH 13/14] Pass joint pointer to mimic API

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 src/systems/physics/Physics.cc | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index 57174d4af8..fb3837934d 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -1713,9 +1713,16 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm,
                   .EntityCast<MimicConstraintJointFeatureList>(existingJoint);
               if (jointPtrMimic)
               {
+                const auto leaderJoint =
+                    basicModelPtrPhys->GetJoint(mimic->Joint());
+                std::size_t leaderAxis = 0;
+                if (mimic->Axis() == "axis2")
+                {
+                  leaderAxis = 1;
+                }
                 jointPtrMimic->SetMimicConstraint(axisIndex,
-                    mimic->Joint(),
-                    mimic->Axis(),
+                    leaderJoint,
+                    leaderAxis,
                     mimic->Multiplier(),
                     mimic->Offset(),
                     mimic->Reference());

From e9b22fcc76a510fd0b31395c81ddbc2a16ad69e2 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Wed, 30 Aug 2023 13:32:39 -0700
Subject: [PATCH 14/14] Add example gz sim command in CDATA block

Put example command in CDATA block because -- is not allowed
in <!-- --> comment blocks. Put the CDATA and comment block
just inside the <sdf> tag.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 .../worlds/mimic_fast_slow_pendulums_world.sdf     | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

diff --git a/examples/worlds/mimic_fast_slow_pendulums_world.sdf b/examples/worlds/mimic_fast_slow_pendulums_world.sdf
index 9d48636f30..b42c8bc73a 100644
--- a/examples/worlds/mimic_fast_slow_pendulums_world.sdf
+++ b/examples/worlds/mimic_fast_slow_pendulums_world.sdf
@@ -1,9 +1,15 @@
 <?xml version="1.0" ?>
-<!--
-  Gazebo Mimic constraint demo
-
--->
 <sdf version="1.11">
+  <!--
+    Gazebo Mimic constraint demo
+
+    Currently requires bullet-feathersone plugin and can be run with the
+    command in the following CDATA block:
+  -->
+  <![CDATA[
+    gz sim -v 4 mimic_fast_slow_pendulums_world.sdf \
+           --physics-engine gz-physics-bullet-featherstone-plugin
+  ]]>
   <world name="default">
     <physics name="1ms" type="ode">
       <max_step_size>0.001</max_step_size>