Skip to content

Commit

Permalink
Set link velocity from set_model_state plugin (#2440)
Browse files Browse the repository at this point in the history 
Signed-off-by: yaswanth1701 <[email protected]>
Co-authored-by: Steve Peters <[email protected]>
  • Loading branch information
@yaswanth1701 @scpeters
yaswanth1701 and scpeters authored Aug 15, 2024
1 parent 6a32292 commit 0adf71d
Show file tree
Hide file tree
Showing 2 changed files with 169 additions and 0 deletions.
88 changes: 88 additions & 0 deletions examples/worlds/set_model_state.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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">
Expand Down Expand Up @@ -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>
81 changes: 81 additions & 0 deletions src/systems/set_model_state/SetModelState.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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.
}

//////////////////////////////////////////////////
Expand Down

0 comments on commit 0adf71d

Please sign in to comment.