Set link velocity from set_model_state plugin

examples/worlds/set_model_state.sdf

@@ -4,9 +4,15 @@
 
   The initial joint position should be 90 degrees, and the initial joint
   velocity should be -30 degrees / second.
+
+  The initial linear velocity should be (0.9, 0.4, 0.1) m/s for the first box
+  and (0.9, -0.4, 0.1) for the second box, and initial angular velocity should be
+  (0.1, 5, 0.1) rad/s for both.
+
 -->
 <sdf version="1.11">
   <world name="set_model_state">
+    <gravity>0 0 0</gravity>
     <plugin
       filename="gz-sim-physics-system"
       name="gz::sim::systems::Physics">
@@ -208,5 +214,87 @@
         </model_state>
       </plugin>
     </model>
+
+    <model name="model_state_example_link_velocity_radians">
+      <pose>2 0 1 0 0 0</pose>
+      <link name="box_link">
+        <pose>0.0 0.0 0.0 0 0 0</pose>
+        <inertial auto="true">
+           <density>277.7</density>
+        </inertial>
+        <visual name="box_visual">
+          <pose>0.0 0.0 0.0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.1 0.4 0.9</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.1 0.4 0.9</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.9 0.4 0.1</linear_velocity>
+           <angular_velocity  degrees="false">0.1 5 0.1</angular_velocity>
+         </link_state>
+       </model_state>
+      </plugin>
+      </model>
+
+    <model name="model_state_example_link_velocity_degrees">
+      <pose>-1 0 1 0 0 0</pose>
+      <link name="box_link">
+        <pose>0.0 0.0 0.0 0 0 0</pose>
+        <inertial auto="true">
+           <density>277.7</density>
+        </inertial>
+        <visual name="box_visual">
+          <pose>0.0 0.0 0.0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.1 0.4 0.9</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.1 0.4 0.9</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.9 -0.4 0.1</linear_velocity>
+           <angular_velocity  degrees="true">5.73 286.479 5.73</angular_velocity>
+         </link_state>
+       </model_state>
+      </plugin>
+      </model>
   </world>
 </sdf>

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/LinearVelocityReset.hh"
+#include "gz/sim/components/AngularVelocityReset.hh"
 #include "gz/sim/Model.hh"
 #include "gz/sim/Util.hh"
 
@@ -60,6 +63,32 @@ namespace
     }
     return false;
   }
+
+  bool parseVectorWithDegree(math::Vector3d &vector, sdf::ElementConstPtr _elem)
+  {
+    if(_elem)
+    {
+      // parse degree attribute, default false
+      std::pair<bool, bool> degreesPair = _elem->Get<bool>("degrees", false);
+      // parse element vector, default math::Vector3d::Zero
+      std::pair<math::Vector3d, bool> vectorPair = _elem->Get<math::Vector3d>
+                                                  ("", math::Vector3d::Zero);
+      if(vectorPair.second)
+      {
+        if(degreesPair.first)
+        {
+          vector.Set(GZ_DTOR(vectorPair.first.X()),
+                 GZ_DTOR(vectorPair.first.Y()), GZ_DTOR(vectorPair.first.Z()));
+        }
+        else
+        {
+          vector = vectorPair.first;
+        }
+        return true;
+      }
+    }
+    return false;
+  }
 }
 
 class gz::sim::systems::SetModelStatePrivate
@@ -222,6 +251,58 @@ 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;
+    }
+
+    math::Vector3d linearVelocity;
+    auto linearVelocityElem = linkStateElem->FindElement("linear_velocity");
+
+    if(linearVelocityElem)
+    {
+     std::pair<math::Vector3d, bool> vectorPair =
+     linearVelocityElem->Get<math::Vector3d>("", math::Vector3d::Zero);
+      if (vectorPair.second)
+      {
+        linearVelocity = vectorPair.first;
+        _ecm.SetComponentData<components::WorldLinearVelocityReset>(
+                                          linkEntity, linearVelocity);
+      }
+    }
+
+    math::Vector3d angularVelocity;
+    auto angularVelocityElem =
+                     linkStateElem->FindElement("angular_velocity");
+
+    if(angularVelocityElem){
+      bool parsedVector = parseVectorWithDegree(angularVelocity,
+                                                angularVelocityElem);
+      if(parsedVector)
+      {
+         _ecm.SetComponentData<components::WorldAngularVelocityReset>(
+                                           linkEntity, angularVelocity);
+      }
+    }
+  }
+  // \TODO(yaswanth1701) set reset velocity components in body frame.
 }
 
 //////////////////////////////////////////////////