Align Physics system

.github/workflows/cicd.yml

@@ -20,9 +20,9 @@ jobs:
           - User
           - Developer
         ignition:
-          - edifice
-          - dome
           # - citadel
+          # - dome
+          - edifice
 
     env:
       CCACHE_DIR: ${{ github.workspace }}/.ccache

cpp/scenario/CMakeLists.txt

@@ -9,6 +9,9 @@ set(SCENARIO_PRIVATE_DEPENDENCIES "")
 
 if(SCENARIO_USE_IGNITION)
 
+    option(ENABLE_PROFILER "Enable Ignition Profiler" OFF)
+    mark_as_advanced(ENABLE_PROFILER)
+
     add_subdirectory(gazebo)
     add_subdirectory(plugins)
     add_subdirectory(controllers)

cpp/scenario/gazebo/CMakeLists.txt

@@ -34,7 +34,6 @@ set(EXTRA_COMPONENTS_PUBLIC_HEADERS
     include/scenario/gazebo/components/BaseWorldAccelerationTarget.h
     include/scenario/gazebo/components/JointControlMode.h
     include/scenario/gazebo/components/JointController.h
-    include/scenario/gazebo/components/WorldVelocityCmd.h
     include/scenario/gazebo/components/JointPositionTarget.h
     include/scenario/gazebo/components/JointVelocityTarget.h
     include/scenario/gazebo/components/JointAccelerationTarget.h

cpp/scenario/gazebo/include/scenario/gazebo/helpers.h

@@ -165,17 +165,17 @@ namespace scenario::gazebo::utils {
     sdf::JointType toSdf(const scenario::core::JointType type);
     scenario::core::JointType fromSdf(const sdf::JointType sdfType);
 
-    std::pair<ignition::math::Vector3d, ignition::math::Vector3d>
-    fromModelToBaseVelocity(const ignition::math::Vector3d& linModelVelocity,
-                            const ignition::math::Vector3d& angModelVelocity,
-                            const ignition::math::Pose3d& M_H_B,
-                            const ignition::math::Quaterniond& W_R_B);
-
-    std::pair<ignition::math::Vector3d, ignition::math::Vector3d>
-    fromBaseToModelVelocity(const ignition::math::Vector3d& linBaseVelocity,
-                            const ignition::math::Vector3d& angBaseVelocity,
-                            const ignition::math::Pose3d& M_H_B,
-                            const ignition::math::Quaterniond& W_R_B);
+    ignition::math::Vector3d fromModelToBaseLinearVelocity(
+        const ignition::math::Vector3d& linModelVelocity,
+        const ignition::math::Vector3d& angModelVelocity,
+        const ignition::math::Pose3d& M_H_B,
+        const ignition::math::Quaterniond& W_R_B);
+
+    ignition::math::Vector3d fromBaseToModelLinearVelocity(
+        const ignition::math::Vector3d& linBaseVelocity,
+        const ignition::math::Vector3d& angBaseVelocity,
+        const ignition::math::Pose3d& M_H_B,
+        const ignition::math::Quaterniond& W_R_B);
 
     std::shared_ptr<World> getParentWorld(const GazeboEntity& gazeboEntity);
 

cpp/scenario/gazebo/src/GazeboSimulator.cpp

@@ -39,6 +39,7 @@
 #include <ignition/gazebo/Server.hh>
 #include <ignition/gazebo/ServerConfig.hh>
 #include <ignition/gazebo/components/Name.hh>
+#include <ignition/gazebo/components/Pose.hh>
 #include <ignition/gazebo/components/World.hh>
 #include <ignition/transport/Node.hh>
 #include <ignition/transport/Publisher.hh>
@@ -233,12 +234,45 @@ bool GazeboSimulator::run(const bool paused)
         iterations = 1;
     }
 
+    // Recent versions of Ignition Gazebo optimize the streaming of pose updates
+    // in order to reduce the bandwidth between server and client.
+    // However, it does not take into account paused steps.
+    // Here below we force all the Pose components to be streamed by manually
+    // setting them as changed.
+    if (paused) {
+        // Get the singleton
+        auto& ecmSingleton =
+            scenario::plugins::gazebo::ECMSingleton::Instance();
+
+        // Process all worlds
+        for (const auto& worldName : this->worldNames()) {
+            assert(ecmSingleton.hasWorld(worldName));
+            assert(ecmSingleton.valid(worldName));
+            assert(ecmSingleton.getECM(worldName));
+
+            // Get the ECM
+            auto* ecm = ecmSingleton.getECM(worldName);
+
+            // Mark all all entities with Pose component as Changed
+            ecm->Each<ignition::gazebo::components::Pose>(
+                [&](const ignition::gazebo::Entity& entity,
+                    ignition::gazebo::components::Pose*) -> bool {
+                    ecm->SetChanged(
+                        entity,
+                        ignition::gazebo::components::Pose::typeId,
+                        ignition::gazebo::ComponentState::OneTimeChange);
+                    return true;
+                });
+        }
+    }
+
+    // Paused simulation run
     if (paused && !server->RunOnce(/*paused=*/true)) {
         sError << "The server couldn't execute the paused step" << std::endl;
         return false;
     }
 
-    // Run the simulation
+    // Unpaused simulation run
     if (!paused
         && !server->Run(/*blocking=*/deterministic,
                         /*iterations=*/iterations,

cpp/scenario/gazebo/src/Model.cpp

@@ -34,15 +34,16 @@
 #include "scenario/gazebo/components/BaseWorldVelocityTarget.h"
 #include "scenario/gazebo/components/JointControllerPeriod.h"
 #include "scenario/gazebo/components/Timestamp.h"
-#include "scenario/gazebo/components/WorldVelocityCmd.h"
 #include "scenario/gazebo/exceptions.h"
 #include "scenario/gazebo/helpers.h"
 
 #include <ignition/common/Event.hh>
 #include <ignition/gazebo/Events.hh>
 #include <ignition/gazebo/Model.hh>
+#include <ignition/gazebo/components/AngularVelocityCmd.hh>
 #include <ignition/gazebo/components/CanonicalLink.hh>
 #include <ignition/gazebo/components/Joint.hh>
+#include <ignition/gazebo/components/LinearVelocityCmd.hh>
 #include <ignition/gazebo/components/Link.hh>
 #include <ignition/gazebo/components/Name.hh>
 #include <ignition/gazebo/components/ParentEntity.hh>
@@ -300,49 +301,13 @@ bool Model::resetBaseOrientation(const std::array<double, 4>& orientation)
 
 bool Model::resetBaseWorldLinearVelocity(const std::array<double, 3>& linear)
 {
-    // Check if the velocity was not already reset in this simulation run,
-    // otherwise the previous target would get overridden
-    if (!this->m_ecm->EntityHasComponentType(
-            this->m_entity,
-            ignition::gazebo::components::WorldVelocityCmd::typeId)) {
+    // Note: there could be a rigid transformation between the base frame and
+    //       the canonical frame. The Physics system processes velocity commands
+    //       in the canonical frame, but this method receives velocity commands
+    //       of in the base frame (all expressed in world coordinates).
+    //       Therefore, we need to compute the base linear velocity from the
+    //       base frame to the canonical frame.
 
-        return this->resetBaseWorldVelocity(linear,
-                                            this->baseWorldAngularVelocity());
-    }
-
-    // Get the existing cmd
-    const auto& velocityCmd = utils::getExistingComponentData<
-        ignition::gazebo::components::WorldVelocityCmd>(m_ecm, m_entity);
-
-    // Override only the linear velocity
-    return this->resetBaseWorldVelocity(
-        linear, utils::fromIgnitionVector(velocityCmd.angular));
-}
-
-bool Model::resetBaseWorldAngularVelocity(const std::array<double, 3>& angular)
-{
-    // Check if the velocity was not already reset in this simulation run,
-    // otherwise the previous target would get overridden
-    if (!this->m_ecm->EntityHasComponentType(
-            this->m_entity,
-            ignition::gazebo::components::WorldVelocityCmd::typeId)) {
-
-        return this->resetBaseWorldVelocity(this->baseWorldLinearVelocity(),
-                                            angular);
-    }
-
-    // Get the existing cmd
-    const auto& velocityCmd = utils::getExistingComponentData<
-        ignition::gazebo::components::WorldVelocityCmd>(m_ecm, m_entity);
-
-    // Override only the angular velocity
-    return this->resetBaseWorldVelocity(
-        utils::fromIgnitionVector(velocityCmd.linear), angular);
-}
-
-bool Model::resetBaseWorldVelocity(const std::array<double, 3>& linear,
-                                   const std::array<double, 3>& angular)
-{
     // Get the entity of the canonical (base) link
     const auto canonicalLinkEntity = m_ecm->EntityByComponents(
         ignition::gazebo::components::Link(),
@@ -359,23 +324,40 @@ bool Model::resetBaseWorldVelocity(const std::array<double, 3>& linear,
     const auto& W_R_B = utils::toIgnitionQuaternion(
         this->getLink(this->baseFrame())->orientation());
 
-    // Create the new model velocity
-    ignition::gazebo::WorldVelocity baseWorldVelocity;
-
-    // Compute the mixed velocity of the base link
-    std::tie(baseWorldVelocity.linear, baseWorldVelocity.angular) =
-        utils::fromModelToBaseVelocity(utils::toIgnitionVector3(linear),
-                                       utils::toIgnitionVector3(angular),
-                                       M_H_B,
-                                       W_R_B);
+    // Compute the linear part of the base link mixed velocity
+    const ignition::math::Vector3d baseLinearWorldVelocity =
+        utils::fromModelToBaseLinearVelocity(
+            utils::toIgnitionVector3(linear),
+            utils::toIgnitionVector3(this->baseWorldAngularVelocity()),
+            M_H_B,
+            W_R_B);
 
     // Store the new velocity
-    utils::setComponentData<ignition::gazebo::components::WorldVelocityCmd>(
-        m_ecm, m_entity, baseWorldVelocity);
+    utils::setComponentData<ignition::gazebo::components::LinearVelocityCmd>(
+        m_ecm, m_entity, baseLinearWorldVelocity);
+
+    return true;
+}
+
+bool Model::resetBaseWorldAngularVelocity(const std::array<double, 3>& angular)
+{
+    // Note: the angular part of the velocity does not change between the base
+    //       link and the canonical link (as the linear part).
+    //       In fact, the angular velocity is invariant if there's a rigid
+    //       transformation between the two frames, like in this case.
+    utils::setComponentData<ignition::gazebo::components::AngularVelocityCmd>(
+        m_ecm, m_entity, utils::toIgnitionVector3(angular));
 
     return true;
 }
 
+bool Model::resetBaseWorldVelocity(const std::array<double, 3>& linear,
+                                   const std::array<double, 3>& angular)
+{
+    return this->resetBaseWorldLinearVelocity(linear)
+           && this->resetBaseWorldAngularVelocity(angular);
+}
+
 bool Model::valid() const
 {
     return this->validEntity() && pImpl->model.Valid(*m_ecm);
@@ -1050,7 +1032,7 @@ std::array<double, 3> Model::baseWorldLinearVelocity() const
             this->getLink(this->baseFrame())->worldAngularVelocity());
 
     // Convert the base velocity to the model mixed velocity
-    auto [modelLinearVelocity, _] = utils::fromBaseToModelVelocity( //
+    const auto& modelLinearVelocity = utils::fromBaseToModelLinearVelocity( //
         canonicalLinkLinearVelocity,
         canonicalLinkAngularVelocity,
         M_H_B,

cpp/scenario/gazebo/src/helpers.cpp

@@ -504,46 +504,41 @@ scenario::core::JointType utils::fromSdf(const sdf::JointType sdfType)
     return type;
 }
 
-std::pair<ignition::math::Vector3d, ignition::math::Vector3d>
-utils::fromModelToBaseVelocity(const ignition::math::Vector3d& linModelVelocity,
-                               const ignition::math::Vector3d& angModelVelocity,
-                               const ignition::math::Pose3d& M_H_B,
-                               const ignition::math::Quaterniond& W_R_B)
+ignition::math::Vector3d utils::fromModelToBaseLinearVelocity(
+    const ignition::math::Vector3d& linModelVelocity,
+    const ignition::math::Vector3d& angModelVelocity,
+    const ignition::math::Pose3d& M_H_B,
+    const ignition::math::Quaterniond& W_R_B)
 {
-    ignition::math::Vector3d linBaseVelocity;
-    const ignition::math::Vector3d& angBaseVelocity = angModelVelocity;
-
     // Extract the rotation and the position of the model wrt to the base
     auto B_R_M = M_H_B.Rot().Inverse();
     auto M_o_B = M_H_B.Pos();
     auto B_o_M = -B_R_M * M_o_B;
 
     // Compute the base linear velocity
-    linBaseVelocity = linModelVelocity - angModelVelocity.Cross(W_R_B * B_o_M);
+    const ignition::math::Vector3d linBaseVelocity =
+        linModelVelocity - angModelVelocity.Cross(W_R_B * B_o_M);
 
-    // Return the mixed velocity of the base
-    return {linBaseVelocity, angBaseVelocity};
+    // Return the linear part of the mixed velocity of the base
+    return linBaseVelocity;
 }
 
-std::pair<ignition::math::Vector3d, ignition::math::Vector3d>
-utils::fromBaseToModelVelocity(const ignition::math::Vector3d& linBaseVelocity,
-                               const ignition::math::Vector3d& angBaseVelocity,
-                               const ignition::math::Pose3d& M_H_B,
-                               const ignition::math::Quaterniond& W_R_B)
+ignition::math::Vector3d utils::fromBaseToModelLinearVelocity(
+    const ignition::math::Vector3d& linBaseVelocity,
+    const ignition::math::Vector3d& angBaseVelocity,
+    const ignition::math::Pose3d& M_H_B,
+    const ignition::math::Quaterniond& W_R_B)
 {
-    ignition::math::Vector3d linModelVelocity;
-    const ignition::math::Vector3d& angModelVelocity = angBaseVelocity;
-
     // Extract the rotation and the position of the model wrt to the base
     auto B_R_M = M_H_B.Rot().Inverse();
     auto M_o_B = M_H_B.Pos();
 
     // Compute the model linear velocity
-    linModelVelocity =
+    const ignition::math::Vector3d linModelVelocity =
         linBaseVelocity - angBaseVelocity.Cross(W_R_B * B_R_M * M_o_B);
 
-    // Return the mixed velocity of the model
-    return {linModelVelocity, angModelVelocity};
+    // Return the linear part of the mixed velocity of the model
+    return linModelVelocity;
 }
 
 std::shared_ptr<World> utils::getParentWorld(const GazeboEntity& gazeboEntity)