Set link velocity from set_model_state plugin

examples/worlds/set_model_state.sdf

@@ -7,6 +7,7 @@
 -->
 <sdf version="1.6">
   <world name="set_model_state">
+    <gravity>0 0 0</gravity>
     <plugin
       filename="gz-sim-physics-system"
       name="gz::sim::systems::Physics">
@@ -248,5 +249,54 @@
         </model_state>
       </plugin>
     </model>
+
+    <model name="model_state_example_link_velocity">
+      <pose>2 0 1 0 0 0</pose>
+      <link name="box_link">
+        <pose>0.0 0.0 0.0 0 0 0</pose>
+        <inertial>
+        <inertia>
+            <ixx>5</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>5</iyy>
+            <iyz>0</iyz>
+            <izz>5</izz>
+          </inertia>
+          <mass>120.0</mass>
+        </inertial>
+        <visual name="box_visual">
+          <pose>0.0 0.0 0.0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.5 0.5 0.5</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>1.0 0.0 0.0 1</ambient>
+            <diffuse>1.0 0.0 0.0 1</diffuse>
+            <specular>1.0 0.0 0.0 1</specular>
+          </material>
+        </visual>
+        <collision name="box_collision">
+          <pose>0.0 0.0 0.0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.5 0.5 0.5</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+      <plugin
+        filename="gz-sim-set-model-state-system"
+        name="gz::sim::systems::SetModelState">
+        <model_state>
+         <link_state name="box_link">
+           <linear_velocity>0.1 -0.1 0.1</linear_velocity>
+           <angular_velocity>1 2 1</angular_velocity>
+         </link_state>
+       </model_state>
+      </plugin>
+      </model>
   </world>
 </sdf>

include/gz/sim/components/WorldAngularVelocityReset.hh

@@ -0,0 +1,47 @@
+/*
+ * 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 angular velocity in world frame in SI units (rad/s).
+  ///
+  /// The component wraps a std::vector of size equal to the degrees of freedom
+  /// of the joint.
+  using WorldAngularVelocityReset = Component<math::Vector3d,
+                                       class WorldAngularVelocityResetTag>;
+  GZ_SIM_REGISTER_COMPONENT(
+      "gz_sim_components.WorldAngularVelocityReset", WorldAngularVelocityReset)
+}
+}
+}
+}
+
+#endif

include/gz/sim/components/WorldLinearVelocityReset.hh

@@ -0,0 +1,47 @@
+/*
+ * 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 Link linear velocity in world frame in SI units (m/s).
+  ///
+  /// The component wraps a std::vector of size equal to the degrees of freedom
+  /// of the joint.
+  using WorldLinearVelocityReset = Component<math::Vector3d ,
+                                       class WorldLinearVelocityResetTag>;
+  GZ_SIM_REGISTER_COMPONENT(
+      "gz_sim_components.WorldLinearVelocityReset", WorldLinearVelocityReset)
+}
+}
+}
+}
+
+#endif

src/systems/physics/Physics.cc

@@ -123,6 +123,8 @@
 #include "gz/sim/components/LinearAcceleration.hh"
 #include "gz/sim/components/LinearVelocity.hh"
 #include "gz/sim/components/LinearVelocityCmd.hh"
+#include "gz/sim/components/WorldLinearVelocityReset.hh"
+#include "gz/sim/components/WorldAngularVelocityReset.hh"
 #include "gz/sim/components/Link.hh"
 #include "gz/sim/components/Model.hh"
 #include "gz/sim/components/Name.hh"
@@ -372,6 +374,14 @@ class gz::sim::systems::PhysicsPrivate
   /// \brief Pointer to the underlying gz-physics Engine entity.
   public: EnginePtrType engine = nullptr;
 
+  /// \brief Flag indicating wheather link linear velocity component is
+  /// already set.
+  public: bool linearVelocityResetFlag = false;
+
+  /// \brief Flag indicating wheather link angular velocity compi340onent
+  /// is already set.
+  public: bool angularVelocityResetFlag = false;
+
   /// \brief Vector3d equality comparison function.
   public: std::function<bool(const math::Vector3d &, const math::Vector3d &)>
           vec3Eql { [](const math::Vector3d &_a, const math::Vector3d &_b)
@@ -2643,6 +2653,131 @@ 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();
+
+        if(!linearVelocityResetFlag)
+        {
+          worldLinearVelFeature->SetWorldLinearVelocity(
+                                 math::eigen3::convert(worldLinearVel));
+          this->linearVelocityResetFlag = true;
+        }
+
+        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();
+
+        if(!angularVelocityResetFlag)
+        {
+          worldAngularVelFeature->SetWorldAngularVelocity(
+              math::eigen3::convert(worldAngularVel));
+          this->angularVelocityResetFlag = true;
+        }
+
+        return true;
+      });
+
 
   // Populate bounding box info
   // Only compute bounding box if component exists to avoid unnecessary

src/systems/set_model_state/SetModelState.cc

@@ -23,11 +23,14 @@
 
 #include <gz/common/Profiler.hh>
 #include <gz/math/Angle.hh>
+#include <gz/math/Vector3.hh>
 #include <gz/plugin/Register.hh>
 
 #include "gz/sim/components/JointAxis.hh"
 #include "gz/sim/components/JointPositionReset.hh"
 #include "gz/sim/components/JointVelocityReset.hh"
+#include "gz/sim/components/WorldLinearVelocityReset.hh"
+#include "gz/sim/components/WorldAngularVelocityReset.hh"
 #include "gz/sim/Model.hh"
 #include "gz/sim/Util.hh"
 
@@ -222,6 +225,64 @@ void SetModelState::Configure(const Entity &_entity,
                                                             jointVelocity);
     }
   }
+  for (auto linkStateElem = modelStateElem->FindElement("link_state");
+       linkStateElem != nullptr;
+       linkStateElem = linkStateElem->GetNextElement("link_state"))
+  {
+
+    std::pair<std::string, bool> namePair =
+         linkStateElem->Get<std::string>("name", "");
+    if (!namePair.second)
+    {
+      gzerr << "No name specified for link_state, skipping.\n";
+      continue;
+    }
+    const auto &linkName = namePair.first;
+
+    Entity linkEntity = this->dataPtr->model.LinkByName(_ecm, linkName);
+    if (linkEntity == kNullEntity)
+    {
+      gzerr << "Unable to find link with name [" << linkName << "] "
+            << "in model with name [" << modelName << "], skipping.\n";
+      continue;
+    }
+
+    // Default velocity is initialsed as zero vector.
+    math::Vector3d defaultVelocity;
+    math::Vector3d linearVelocity;
+    math::Vector3d angularVelocity;
+
+    auto linearVelocityElem = linkStateElem->FindElement("linear_velocity");
+
+    if(linearVelocityElem)
+    {
+     std::pair<math::Vector3d, bool> vectorPair =
+     linearVelocityElem->Get<math::Vector3d>("", defaultVelocity);
+      if (vectorPair.second)
+      {
+        linearVelocity = vectorPair.first;
+        _ecm.SetComponentData<components::WorldLinearVelocityReset>(
+                                          linkEntity,
+                                          linearVelocity);
+      }
+    }
+
+    auto angularVelocityElem =
+                     linkStateElem->FindElement("angular_velocity");
+
+    if(angularVelocityElem){
+      std::pair<math::Vector3d, bool> vectorPair =
+           angularVelocityElem->Get<math::Vector3d>("", defaultVelocity);
+      if(vectorPair.second)
+      {
+         angularVelocity = vectorPair.first;
+         _ecm.SetComponentData<components::WorldAngularVelocityReset>(
+                                           linkEntity,
+                                           angularVelocity);
+
+      }
+    }
+  }
 }
 
 //////////////////////////////////////////////////