Support world joints (joints inside <world> tags)

src/LevelManager.cc

@@ -21,6 +21,7 @@
 
 #include <sdf/Actor.hh>
 #include <sdf/Atmosphere.hh>
+#include <sdf/Joint.hh>
 #include <sdf/Light.hh>
 #include <sdf/Model.hh>
 #include <sdf/World.hh>
@@ -35,6 +36,7 @@
 #include "gz/sim/components/Atmosphere.hh"
 #include "gz/sim/components/Geometry.hh"
 #include "gz/sim/components/Gravity.hh"
+#include "gz/sim/components/Joint.hh"
 #include "gz/sim/components/Level.hh"
 #include "gz/sim/components/Model.hh"
 #include "gz/sim/components/Light.hh"
@@ -508,6 +510,21 @@ void LevelManager::ConfigureDefaultLevel()
       entityNamesInDefault.insert(light->Name());
     }
   }
+
+  // Joints
+  // We assume no performers are joints
+  for (uint64_t jointIndex = 0;
+       jointIndex < this->runner->sdfWorld->JointCount(); ++jointIndex)
+  {
+    auto joint = this->runner->sdfWorld->JointByIndex(jointIndex);
+
+    if (this->entityNamesInLevels.find(joint->Name()) ==
+        this->entityNamesInLevels.end())
+    {
+      entityNamesInDefault.insert(joint->Name());
+    }
+  }
+
   // Components
   this->runner->entityCompMgr.CreateComponent(
       defaultLevel, components::Level());
@@ -847,6 +864,20 @@ void LevelManager::LoadActiveEntities(const std::set<std::string> &_namesToLoad)
     }
   }
 
+  // Joints
+  for (uint64_t jointIndex = 0;
+       jointIndex < this->runner->sdfWorld->JointCount(); ++jointIndex)
+  {
+    auto joint = this->runner->sdfWorld->JointByIndex(jointIndex);
+    if (_namesToLoad.find(joint->Name()) != _namesToLoad.end())
+    {
+      Entity jointEntity = this->entityCreator->CreateEntities(joint);
+
+      this->entityCreator->SetParent(jointEntity, this->worldEntity);
+    }
+  }
+
+
   this->activeEntityNames.insert(_namesToLoad.begin(), _namesToLoad.end());
 }
 
@@ -887,6 +918,17 @@ void LevelManager::UnloadInactiveEntities(
         return true;
       });
 
+  this->runner->entityCompMgr.Each<components::Joint, components::Name>(
+      [&](const Entity &_entity, const components::Joint *,
+          const components::Name *_name) -> bool
+      {
+        if (_namesToUnload.find(_name->Data()) != _namesToUnload.end())
+        {
+          this->entityCreator->RequestRemoveEntity(_entity, true);
+        }
+        return true;
+      });
+
   for (const auto &name : _namesToUnload)
   {
     this->activeEntityNames.erase(name);

src/systems/physics/Physics.cc

@@ -623,6 +623,12 @@ class gz::sim::systems::PhysicsPrivate
             MinimumFeatureList,
             physics::sdf::ConstructSdfNestedModel>{};
 
+  //////////////////////////////////////////////////
+  // World models (used for world joints)
+  public: struct WorldModelFeatureList : physics::FeatureList<
+            MinimumFeatureList,
+            physics::WorldModelFeature>{};
+
   //////////////////////////////////////////////////
   /// \brief World EntityFeatureMap
   public: using WorldEntityMap = EntityFeatureMap3d<
@@ -633,7 +639,9 @@ class gz::sim::systems::PhysicsPrivate
           SetContactPropertiesCallbackFeatureList,
           NestedModelFeatureList,
           CollisionDetectorFeatureList,
-          SolverFeatureList>;
+          SolverFeatureList,
+          WorldModelFeatureList
+          >;
 
   /// \brief A map between world entity ids in the ECM to World Entities in
   /// gz-physics.
@@ -1035,6 +1043,15 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm,
           }
         }
 
+        // World Model proxy (used for joints directly under <world> in SDF)
+        auto worldModelFeature =
+            this->entityWorldMap.EntityCast<WorldModelFeatureList>(_entity);
+        if (worldModelFeature)
+        {
+          auto modelPtrPhys = worldModelFeature->GetWorldModel();
+          this->entityModelMap.AddEntity(_entity, modelPtrPhys);
+        }
+
         return true;
       });
 }

test/integration/physics_system.cc

@@ -121,7 +121,6 @@ TEST_F(PhysicsSystemFixture, CreatePhysicsWorld)
     server.Run(true, 1, false);
     EXPECT_FALSE(server.Running());
   }
-  // TODO(addisu) add useful EXPECT calls
 }
 
 ////////////////////////////////////////////////
@@ -2539,3 +2538,86 @@ TEST_F(PhysicsSystemFixture,
         << "World Angular Velocity should be increasing.";
   }
 }
+
+//////////////////////////////////////////////////
+/// Tests that joints inside <world> are supported by computing the position of
+/// a pendulum bob from the joint angle. This also tests that commands such as
+/// JointPositionReset work as expected.
+TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(JointsInWorld))
+{
+  ServerConfig serverConfig;
+
+  const auto sdfFile = common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds",
+                                         "joints_in_world.sdf");
+
+  serverConfig.SetSdfFile(sdfFile);
+  Server server(serverConfig);
+  server.SetUpdatePeriod(1ns);
+
+  const int kIters = 1000;
+  test::Relay testSystem;
+  testSystem.OnPreUpdate(
+      [&](const UpdateInfo &_info, EntityComponentManager &_ecm)
+      {
+        _ecm.EachNew<components::Joint, components::ParentEntity>(
+            [&](const Entity &_entity, const components::Joint *,
+                const components::ParentEntity *) -> bool
+            {
+              enableComponent<components::JointPosition>(_ecm, _entity);
+              enableComponent<components::JointVelocity>(_ecm, _entity);
+              return true;
+            });
+
+        if (_info.iterations == kIters / 2)
+        {
+          // After half the iterations, reset the joint position and velocity so
+          // that the bob at its equilibrium point and at rest.
+          auto jointEntity = _ecm.EntityByComponents(components::Name("j1"),
+                                                     components::Joint());
+          ASSERT_NE(jointEntity, kNullEntity);
+          _ecm.SetComponentData<components::JointVelocityReset>(jointEntity,
+                                                                {0});
+          _ecm.SetComponentData<components::JointPositionReset>(jointEntity,
+                                                                {GZ_PI_2});
+        }
+      });
+  testSystem.OnPostUpdate(
+      [&](const UpdateInfo &_info, const EntityComponentManager &_ecm)
+      {
+        auto jointEntity = _ecm.EntityByComponents(components::Name("j1"),
+                                                   components::Joint());
+        ASSERT_NE(jointEntity, kNullEntity);
+
+        auto m2Entity = _ecm.EntityByComponents(components::Name("m2"),
+                                                components::Model());
+        ASSERT_NE(m2Entity, kNullEntity);
+
+        math::Pose3d m2Pose = worldPose(m2Entity, _ecm);
+
+        auto jointPos =
+            _ecm.ComponentData<components::JointPosition>(jointEntity);
+        ASSERT_TRUE(jointPos.has_value());
+        ASSERT_EQ(1u, jointPos->size());
+
+        if (_info.iterations < kIters / 2)
+        {
+          // If the joint is properly working, the position of the model can be
+          // determined from the joint angle with the equations:
+          // x = L*cos(θ)
+          // z = -L*sin(θ)
+          // where L is the length of the model from the pivot (2m for this
+          // test).
+          const double theta = (*jointPos)[0];
+          const double kLength = 2.0;
+          EXPECT_NEAR(m2Pose.X(), kLength * cos(theta), 1e-2);
+          EXPECT_NEAR(m2Pose.Z(), -kLength * sin(theta), 1e-2);
+        }
+        else
+        {
+          EXPECT_NEAR((*jointPos)[0], GZ_PI_2, 1e-2);
+        }
+      });
+
+  server.AddSystem(testSystem.systemPtr);
+  server.Run(true, 1000, false);
+}

test/worlds/joints_in_world.sdf

@@ -0,0 +1,44 @@
+<?xml version="1.0" ?>
+<sdf version="1.10">
+  <world name="joints_in_world">
+    <joint name="fix_to_world" type="fixed">
+      <parent>world</parent>
+      <child>m1</child>
+    </joint>
+
+    <model name="m1">
+      <link name="link1">
+        <visual name="visual">
+          <geometry>
+            <sphere>
+              <radius>0.5</radius>
+            </sphere>
+          </geometry>
+        </visual>
+      </link>
+    </model>
+
+    <joint name="j1" type="revolute">
+      <pose relative_to="m1"/>
+      <parent>m1</parent>
+      <child>m2</child>
+      <axis>
+        <xyz>0 1 0</xyz>
+      </axis>
+    </joint>
+
+    <model name="m2">
+      <pose>2 0 0   0 0 0</pose>
+      <link name="link2">
+        <visual name="visual">
+          <geometry>
+            <sphere>
+              <radius>0.5</radius>
+            </sphere>
+          </geometry>
+        </visual>
+      </link>
+    </model>
+
+  </world>
+</sdf>