added link velocity components and changes to physics systems

Signed-off-by: yaswanth1701 <[email protected]>

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_WORLDANGULARELOCITYRESET_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 Link initial angular velocity in its own frame and
+  /// in SI units (rad/s).The angular velocity of entity is
+  /// represented by gz::math::vector3d.
+  using AngularVelocityReset = Component<math::Vector3d,
+                                       class AngularVelocityResetTag>;
+  GZ_SIM_REGISTER_COMPONENT(
+      "gz_sim_components.AngularVelocityReset", AngularVelocityReset)
+
+  /// \brief Link initial angular velocity in world frame and
+  /// in SI units (rad/s). The angular velocity of entity 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 Entity initial linear velocity in its own frame and
+  /// in SI units (m/s). The linear velocity of entity is
+  /// represented by gz::math::vector3d.
+  using LinearVelocityReset = Component<math::Vector3d ,
+                                       class LinearVelocityResetTag>;
+  GZ_SIM_REGISTER_COMPONENT(
+      "gz_sim_components.LinearVelocityReset", LinearVelocityReset)
+
+  /// \brief Entity initial linear velocity in world frame and
+  /// in SI units (m/s). The linear velocity of entity 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

@@ -99,6 +99,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"
@@ -123,6 +124,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"
@@ -2643,6 +2645,123 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
         return true;
       });
 
+  // Set initial link linear velocity
+  _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)
+        {
+          gzwarn << "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;
+      });
+
+   // Set initial link angular velocity
+   _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)
+        {
+          gzwarn << "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
@@ -3578,6 +3697,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,
@@ -3628,58 +3775,34 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm,
         return true;
       });
 
-  {
-    std::vector<Entity> entitiesJointVelocityCmd;
-    _ecm.Each<components::JointVelocityCmd>(
-        [&](const Entity &_entity, components::JointVelocityCmd *) -> bool
-        {
-          entitiesJointVelocityCmd.push_back(_entity);
-          return true;
-        });
-
-    for (const auto entity : entitiesJointVelocityCmd)
-    {
-      _ecm.RemoveComponent<components::JointVelocityCmd>(entity);
-    }
-  }
+  _ecm.Each<components::JointVelocityCmd>(
+      [&](const Entity &, components::JointVelocityCmd *_vel) -> bool
+      {
+        std::fill(_vel->Data().begin(), _vel->Data().end(), 0.0);
+        return true;
+      });
 
   _ecm.Each<components::SlipComplianceCmd>(
       [&](const Entity &, components::SlipComplianceCmd *_slip) -> bool
       {
         std::fill(_slip->Data().begin(), _slip->Data().end(), 0.0);
         return true;
       });
+  GZ_PROFILE_END();
 
-  {
-    std::vector<Entity> entitiesAngularVelocityCmd;
-    _ecm.Each<components::AngularVelocityCmd>(
-        [&](const Entity &_entity, components::AngularVelocityCmd *) -> bool
-        {
-          entitiesAngularVelocityCmd.push_back(_entity);
-          return true;
-        });
-
-    for (const auto entity : entitiesAngularVelocityCmd)
-    {
-      _ecm.RemoveComponent<components::AngularVelocityCmd>(entity);
-    }
-  }
-
-  {
-    std::vector<Entity> entitiesLinearVelocityCmd;
-    _ecm.Each<components::LinearVelocityCmd>(
-        [&](const Entity &_entity, components::LinearVelocityCmd *) -> bool
-        {
-          entitiesLinearVelocityCmd.push_back(_entity);
-          return true;
-        });
+  _ecm.Each<components::AngularVelocityCmd>(
+      [&](const Entity &, components::AngularVelocityCmd *_vel) -> bool
+      {
+        _vel->Data() = math::Vector3d::Zero;
+        return true;
+      });
 
-    for (const auto entity : entitiesLinearVelocityCmd)
-    {
-      _ecm.RemoveComponent<components::LinearVelocityCmd>(entity);
-    }
-  }
-  GZ_PROFILE_END();
+  _ecm.Each<components::LinearVelocityCmd>(
+      [&](const Entity &, components::LinearVelocityCmd *_vel) -> bool
+      {
+        _vel->Data() = math::Vector3d::Zero;
+        return true;
+      });
 
   // Update joint positions
   GZ_PROFILE_BEGIN("Joints");