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. } //////////////////////////////////////////////////