diff --git a/examples/worlds/set_model_state.sdf b/examples/worlds/set_model_state.sdf
index 0c00b7b535..d8896d1057 100644
--- a/examples/worlds/set_model_state.sdf
+++ b/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.
+
-->
+ 0 0 0
@@ -208,5 +214,87 @@
+
+
+ 2 0 1 0 0 0
+
+ 0.0 0.0 0.0 0 0 0
+
+ 277.7
+
+
+ 0.0 0.0 0.0 0 0 0
+
+
+ 0.1 0.4 0.9
+
+
+
+ 1.0 0.0 0.0 1
+ 1.0 0.0 0.0 1
+ 1.0 0.0 0.0 1
+
+
+
+ 0.0 0.0 0.0 0 0 0
+
+
+ 0.1 0.4 0.9
+
+
+
+
+
+
+
+ 0.9 0.4 0.1
+ 0.1 5 0.1
+
+
+
+
+
+
+ -1 0 1 0 0 0
+
+ 0.0 0.0 0.0 0 0 0
+
+ 277.7
+
+
+ 0.0 0.0 0.0 0 0 0
+
+
+ 0.1 0.4 0.9
+
+
+
+ 1.0 0.0 0.0 1
+ 1.0 0.0 0.0 1
+ 1.0 0.0 0.0 1
+
+
+
+ 0.0 0.0 0.0 0 0 0
+
+
+ 0.1 0.4 0.9
+
+
+
+
+
+
+
+ 0.9 -0.4 0.1
+ 5.73 286.479 5.73
+
+
+
+
diff --git a/src/systems/set_model_state/SetModelState.cc b/src/systems/set_model_state/SetModelState.cc
index 332fc458e0..d41eb4f384 100644
--- a/src/systems/set_model_state/SetModelState.cc
+++ b/src/systems/set_model_state/SetModelState.cc
@@ -23,11 +23,14 @@
#include
#include
+#include
#include
#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 degreesPair = _elem->Get("degrees", false);
+ // parse element vector, default math::Vector3d::Zero
+ std::pair vectorPair = _elem->Get
+ ("", 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 namePair =
+ linkStateElem->Get("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 vectorPair =
+ linearVelocityElem->Get("", math::Vector3d::Zero);
+ if (vectorPair.second)
+ {
+ linearVelocity = vectorPair.first;
+ _ecm.SetComponentData(
+ linkEntity, linearVelocity);
+ }
+ }
+
+ math::Vector3d angularVelocity;
+ auto angularVelocityElem =
+ linkStateElem->FindElement("angular_velocity");
+
+ if(angularVelocityElem){
+ bool parsedVector = parseVectorWithDegree(angularVelocity,
+ angularVelocityElem);
+ if(parsedVector)
+ {
+ _ecm.SetComponentData(
+ linkEntity, angularVelocity);
+ }
+ }
+ }
+ // \TODO(yaswanth1701) set reset velocity components in body frame.
}
//////////////////////////////////////////////////