Physics: set link velocity from *VelocityReset components

include/gz/sim/components/AngularVelocityReset.hh

@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2024 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.
+ *
+ */
+#ifndef GZ_SIM_COMPONENTS_WORLDANGULARVELOCITYRESET_HH_
+#define GZ_SIM_COMPONENTS_WORLDANGULARVELOCITYRESET_HH_
+
+#include <gz/math/Vector3.hh>
+#include <gz/sim/components/Component.hh>
+#include <gz/sim/components/Factory.hh>
+#include <gz/sim/components/Serialization.hh>
+#include <gz/sim/config.hh>
+
+namespace gz
+{
+namespace sim
+{
+// Inline bracket to help doxygen filtering.
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+namespace components
+{
+  /// \brief Angular velocity of an entity, in its own frame
+  /// and in SI units (rad/s). The angular velocity is
+  //  represented by gz::math::Vector3d.
+  using AngularVelocityReset = Component<math::Vector3d,
+                                       class AngularVelocityResetTag>;
+  GZ_SIM_REGISTER_COMPONENT(
+      "gz_sim_components.AngularVelocityReset", AngularVelocityReset)
+
+  /// \brief Angular velocity of an entity in the world frame
+  /// and in SI units (rad/s). The angular velocity is
+  //  represented by gz::math::Vector3d.
+  using WorldAngularVelocityReset = Component<math::Vector3d,
+                                       class WorldAngularVelocityResetTag>;
+  GZ_SIM_REGISTER_COMPONENT(
+      "gz_sim_components.WorldAngularVelocityReset", WorldAngularVelocityReset)
+}
+}
+}
+}
+
+#endif

include/gz/sim/components/LinearVelocityReset.hh

@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2024 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.
+ *
+ */
+#ifndef GZ_SIM_COMPONENTS_WORLDLINEARVELOCITYRESET_HH_
+#define GZ_SIM_COMPONENTS_WORLDLINEARVELOCITYRESET_HH_
+
+#include <gz/math/Vector3.hh>
+#include <gz/sim/components/Component.hh>
+#include <gz/sim/components/Factory.hh>
+#include <gz/sim/components/Serialization.hh>
+#include <gz/sim/config.hh>
+
+namespace gz
+{
+namespace sim
+{
+// Inline bracket to help doxygen filtering.
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+namespace components
+{
+  /// \brief Linear velocity of an entity in its own frame
+  /// and in SI units (m/s). The linear velocity is
+  /// represented by gz::math::Vector3d.
+  using LinearVelocityReset = Component<math::Vector3d ,
+                                       class LinearVelocityResetTag>;
+  GZ_SIM_REGISTER_COMPONENT(
+      "gz_sim_components.LinearVelocityReset", LinearVelocityReset)
+
+  /// \brief Linear velocity of an entity in the world frame
+  /// and in SI units (m/s). The linear velocity is
+  /// represented by gz::math::Vector3d.
+  using WorldLinearVelocityReset = Component<math::Vector3d ,
+                                       class WorldLinearVelocityResetTag>;
+  GZ_SIM_REGISTER_COMPONENT(
+      "gz_sim_components.WorldLinearVelocityReset", WorldLinearVelocityReset)
+}
+}
+}
+}
+
+#endif

src/systems/physics/Physics.cc

@@ -100,6 +100,7 @@
 #include "gz/sim/components/AngularAcceleration.hh"
 #include "gz/sim/components/AngularVelocity.hh"
 #include "gz/sim/components/AngularVelocityCmd.hh"
+#include "gz/sim/components/AngularVelocityReset.hh"
 #include "gz/sim/components/AxisAlignedBox.hh"
 #include "gz/sim/components/BatterySoC.hh"
 #include "gz/sim/components/CanonicalLink.hh"
@@ -124,6 +125,7 @@
 #include "gz/sim/components/LinearAcceleration.hh"
 #include "gz/sim/components/LinearVelocity.hh"
 #include "gz/sim/components/LinearVelocityCmd.hh"
+#include "gz/sim/components/LinearVelocityReset.hh"
 #include "gz/sim/components/Link.hh"
 #include "gz/sim/components/Model.hh"
 #include "gz/sim/components/Name.hh"
@@ -2710,6 +2712,122 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
         return true;
       });
 
+  // Reset link linear velocity in world frame
+  _ecm.Each<components::Link, components::WorldLinearVelocityReset>(
+      [&](const Entity &_entity, const components::Link *,
+          const components::WorldLinearVelocityReset *_worldlinearvelocityreset)
+      {
+        if (!this->entityLinkMap.HasEntity(_entity))
+        {
+          gzwarn << "Failed to find link [" << _entity
+                  << "]." << std::endl;
+          return true;
+        }
+
+        auto linkPtrPhys = this->entityLinkMap.Get(_entity);
+        if (nullptr == linkPtrPhys)
+          return true;
+
+        auto freeGroup = linkPtrPhys->FindFreeGroup();
+        if (!freeGroup)
+          return true;
+
+        auto rootLinkPtr = freeGroup->RootLink();
+        if (rootLinkPtr != linkPtrPhys)
+        {
+          gzdbg << "Attempting to set linear velocity for link [ " << _entity
+                 << " ] which is not root link of the FreeGroup."
+                 << "Velocity won't be set."
+                 << std::endl;
+
+          return true;
+        }
+
+        this->entityFreeGroupMap.AddEntity(_entity, freeGroup);
+
+        auto worldLinearVelFeature = this->entityFreeGroupMap
+                .EntityCast<WorldVelocityCommandFeatureList>(_entity);
+        if (!worldLinearVelFeature)
+        {
+          static bool informed{false};
+          if (!informed)
+          {
+            gzdbg << "Attempting to set link linear velocity, but the "
+                   << "physics engine doesn't support velocity commands. "
+                   << "Velocity won't be set."
+                   << std::endl;
+            informed = true;
+          }
+          return true;
+        }
+
+        // Linear velocity in world frame
+        math::Vector3d worldLinearVel = _worldlinearvelocityreset->Data();
+
+        worldLinearVelFeature->SetWorldLinearVelocity(
+            math::eigen3::convert(worldLinearVel));
+
+        return true;
+      });
+
+  // Reset link angular velocity in world frame
+  _ecm.Each<components::Link, components::WorldAngularVelocityReset>(
+      [&](const Entity &_entity, const components::Link *,
+          const components::WorldAngularVelocityReset
+          *_worldangularvelocityreset)
+      {
+        if (!this->entityLinkMap.HasEntity(_entity))
+        {
+          gzwarn << "Failed to find link [" << _entity
+                  << "]." << std::endl;
+          return true;
+        }
+
+        auto linkPtrPhys = this->entityLinkMap.Get(_entity);
+        if (nullptr == linkPtrPhys)
+          return true;
+
+        auto freeGroup = linkPtrPhys->FindFreeGroup();
+        if (!freeGroup)
+          return true;
+
+        auto rootLinkPtr = freeGroup->RootLink();
+        if(rootLinkPtr != linkPtrPhys)
+        {
+          gzdbg << "Attempting to set angular velocity for link [ " << _entity
+                 << " ] which is not root link of the FreeGroup."
+                 << "Velocity won't be set."
+                 << std::endl;
+
+          return true;
+        }
+
+        this->entityFreeGroupMap.AddEntity(_entity, freeGroup);
+
+        auto worldAngularVelFeature = this->entityFreeGroupMap
+                .EntityCast<WorldVelocityCommandFeatureList>(_entity);
+
+        if (!worldAngularVelFeature)
+        {
+          static bool informed{false};
+          if (!informed)
+          {
+            gzdbg << "Attempting to set link angular velocity, but the "
+                  << "physics engine doesn't support velocity commands. "
+                  << "Velocity won't be set."
+                  << std::endl;
+            informed = true;
+          }
+          return true;
+        }
+        // Angular velocity in world frame
+        math::Vector3d worldAngularVel = _worldangularvelocityreset->Data();
+
+        worldAngularVelFeature->SetWorldAngularVelocity(
+            math::eigen3::convert(worldAngularVel));
+
+        return true;
+      });
 
   // Populate bounding box info
   // Only compute bounding box if component exists to avoid unnecessary
@@ -3645,6 +3763,34 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm,
     _ecm.RemoveComponent<components::JointVelocityReset>(entity);
   }
 
+  std::vector<Entity> entitiesLinearVelocityReset;
+  _ecm.Each<components::WorldLinearVelocityReset>(
+      [&](const Entity &_entity,
+      components::WorldLinearVelocityReset *) -> bool
+      {
+        entitiesLinearVelocityReset.push_back(_entity);
+        return true;
+      });
+
+  for (const auto entity : entitiesLinearVelocityReset)
+  {
+    _ecm.RemoveComponent<components::WorldLinearVelocityReset>(entity);
+  }
+
+  std::vector<Entity> entitiesAngularVelocityReset;
+  _ecm.Each<components::WorldAngularVelocityReset>(
+      [&](const Entity &_entity,
+      components::WorldAngularVelocityReset *) -> bool
+      {
+        entitiesAngularVelocityReset.push_back(_entity);
+        return true;
+      });
+
+  for (const auto entity : entitiesAngularVelocityReset)
+  {
+    _ecm.RemoveComponent<components::WorldAngularVelocityReset>(entity);
+  }
+
   std::vector<Entity> entitiesCustomContactSurface;
   _ecm.Each<components::EnableContactSurfaceCustomization>(
       [&](const Entity &_entity,