Implement reset interface in the physics system

examples/plugin/reset_plugin/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
+
+project(ResetPlugins)
+
+find_package(ignition-plugin2 REQUIRED COMPONENTS register)
+set(IGN_PLUGIN_VER ${ignition-plugin2_VERSION_MAJOR})
+
+find_package(ignition-gazebo7 REQUIRED)
+set(IGN_GAZEBO_VER ${ignition-gazebo7_VERSION_MAJOR})
+
+add_library(JointPositionRandomizer SHARED JointPositionRandomizer.cc)
+target_link_libraries(JointPositionRandomizer 
+  PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER}
+  PRIVATE ignition-gazebo${IGN_GAZEBO_VER}::core)

examples/plugin/reset_plugin/JointPositionRandomizer.cc

@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#include <ignition/math/Rand.hh>
+#include <ignition/gazebo/System.hh>
+#include <ignition/gazebo/components/Joint.hh>
+#include <ignition/gazebo/components/JointPositionReset.hh>
+#include <ignition/gazebo/components/JointPositionLimitsCmd.hh>
+#include <ignition/gazebo/components/JointType.hh>
+#include <ignition/gazebo/components/ParentEntity.hh>
+#include <ignition/plugin/Register.hh>
+
+using namespace ignition;
+using namespace gazebo;
+namespace reset_plugin
+{
+class JointPositionRandomizer : public System,
+                                public ISystemConfigure,
+                                public ISystemReset
+{
+  void Configure(const Entity &_entity,
+                 const std::shared_ptr<const sdf::Element> &,
+                 EntityComponentManager &, EventManager &) override
+  {
+    this->targetEntity = _entity;
+  }
+
+  void Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) override
+  {
+    std::cout << "Reset joints\n";
+    auto joints = _ecm.EntitiesByComponents(
+        components::ParentEntity(this->targetEntity), components::Joint());
+
+    for (auto joint : joints)
+    {
+      auto jointType = _ecm.Component<components::JointType>(joint);
+
+      double pos = 0.0; 
+
+      if (jointType->Data() == sdf::JointType::PRISMATIC)
+      {
+        pos = math::Rand::DblUniform(-0.8, 0.1);
+        std::cout << "prismatic joint (" << joint 
+          << ") pos: (" << pos << " m)"<< std::endl;
+      } 
+      else if (jointType->Data() == sdf::JointType::REVOLUTE)
+      {
+        pos = math::Rand::DblUniform(0, IGN_PI);
+        std::cout << "revolute joint (" << joint 
+          << ") pos: (" << pos << " rad)"<< std::endl;
+      }
+      _ecm.SetComponentData<components::JointPositionReset>(joint, {pos});
+    }
+  }
+
+  private: Entity targetEntity;
+};
+}  // namespace reset_plugin
+
+IGNITION_ADD_PLUGIN(reset_plugin::JointPositionRandomizer,
+                    ignition::gazebo::System,
+                    reset_plugin::JointPositionRandomizer::ISystemConfigure,
+                    reset_plugin::JointPositionRandomizer::ISystemReset)

examples/plugin/reset_plugin/README.md

@@ -0,0 +1,34 @@
+# System Reset API
+
+This example uses the JointPositionRandomizer system to randomize the joint 
+positions of a robot arm at every reset.
+
+
+## Build
+
+~~~
+cd examples/plugin/reset_plugin
+mkdir build
+cd build
+cmake ..
+make
+~~~
+
+This will generate the `JointPositionRandomizer` library under `build`.
+
+## Run
+
+Add the library to the path:
+
+~~~
+cd examples/plugin/reset_plugin
+export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=`pwd`/build
+~~~
+
+Then run a world that loads the plugin as follows:
+
+    ign gazebo -r -v 4 joint_position_randomizer.sdf
+
+In another terminal, run the following to reset the world.
+
+    ign service -s /world/default/control --reqtype ignition.msgs.WorldControl --reptype ignition.msgs.Boolean --timeout 3000 --req 'reset: {all: true}'

examples/plugin/reset_plugin/joint_position_randomizer.sdf

@@ -0,0 +1,44 @@
+<sdf version="1.8">
+  <!-- This example uses the JointPositionRandomizer system to randomize the joint positions of a robot arm at every reset.
+
+    To reset the world, run:
+
+    ign service -s /world/default/control \-\-reqtype ignition.msgs.WorldControl \-\-reptype ignition.msgs.Boolean \-\-timeout 3000 \-\-req 'reset: {all: true}'
+  -->
+
+  <world name="default">
+    <plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics"></plugin>
+    <plugin filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands"></plugin>
+    <plugin filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster"></plugin>
+    <light name="sun" type="directional"/>
+    <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>
+          </material>
+        </visual>
+      </link>
+    </model>
+    <include>
+      <uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Simple Arm</uri>
+      <plugin filename="build/libJointPositionRandomizer.so" name="reset_plugin::JointPositionRandomizer"/>
+    </include>
+  </world>
+</sdf>

src/EntityComponentManager_TEST.cc

@@ -3174,6 +3174,7 @@ TEST_P(EntityComponentManagerFixture,
   EXPECT_EQ(1, foundEntities);
 }
 
+//////////////////////////////////////////////////
 TEST_P(EntityComponentManagerFixture, CopyEcm)
 {
   Entity entity = manager.CreateEntity();
@@ -3195,6 +3196,7 @@ TEST_P(EntityComponentManagerFixture, CopyEcm)
       });
 }
 
+//////////////////////////////////////////////////
 TEST_P(EntityComponentManagerFixture, ComputeDiff)
 {
   Entity entity1 = manager.CreateEntity();
@@ -3243,6 +3245,7 @@ TEST_P(EntityComponentManagerFixture, ComputeDiff)
   }
 }
 
+//////////////////////////////////////////////////
 TEST_P(EntityComponentManagerFixture, ResetToWithDeletedEntity)
 {
   Entity entity1 = manager.CreateEntity();
@@ -3293,6 +3296,7 @@ TEST_P(EntityComponentManagerFixture, ResetToWithDeletedEntity)
   }
 }
 
+//////////////////////////////////////////////////
 TEST_P(EntityComponentManagerFixture, ResetToWithAddedEntity)
 {
   Entity entity1 = manager.CreateEntity();

src/systems/physics/CanonicalLinkModelTracker.hh

@@ -58,6 +58,10 @@ namespace systems::physics_system
     /// \param[in] _ecm EntityComponentManager
     public: void AddNewModels(const EntityComponentManager &_ecm);
 
+    /// \brief Save mappings for all models and their canonical links
+    /// \param[in] _ecm EntityComponentManager
+    public: void AddAllModels(const EntityComponentManager &_ecm);
+
     /// \brief Get a topological ordering of models that have a particular
     /// canonical link
     /// \param[in] _canonicalLink The canonical link
@@ -79,7 +83,7 @@ namespace systems::physics_system
 
     /// \brief An empty set of models that is returned from the
     /// CanonicalLinkModels method for links that map to no models
-    private: const std::set<Entity> emptyModelOrdering{};
+    private: static inline const std::set<Entity> kEmptyModelOrdering{};
   };
 
   void CanonicalLinkModelTracker::AddNewModels(
@@ -93,6 +97,17 @@ namespace systems::physics_system
           return true;
         });
   }
+  void CanonicalLinkModelTracker::AddAllModels(
+      const EntityComponentManager &_ecm)
+  {
+    _ecm.Each<components::Model, components::ModelCanonicalLink>(
+        [this](const Entity &_model, const components::Model *,
+          const components::ModelCanonicalLink *_canonicalLinkComp)
+        {
+          this->linkModelMap[_canonicalLinkComp->Data()].insert(_model);
+          return true;
+        });
+  }
 
   const std::set<Entity> &CanonicalLinkModelTracker::CanonicalLinkModels(
       const Entity _canonicalLink) const
@@ -102,7 +117,7 @@ namespace systems::physics_system
       return it->second;
 
     // if an invalid entity was given, it maps to no models
-    return this->emptyModelOrdering;
+    return this->kEmptyModelOrdering;
   }
 
   void CanonicalLinkModelTracker::RemoveLink(const Entity &_link)