Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

velocity_control: allow non-persistent initial velocity commands #2265

Closed
wants to merge 3 commits into from
Closed

velocity_control: allow non-persistent initial velocity commands #2265

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
c2b98dd
velocity_control: set model velocity cmds directly
scpeters Nov 10, 2023
607004a
Support non-persistent initial velocity
scpeters Nov 15, 2023
fdf9438
initial_velocity.sdf example world
scpeters Nov 15, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
232 changes: 232 additions & 0 deletions examples/worlds/initial_velocity.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,232 @@
<?xml version="1.0" ?>
<!--
Gazebo velocity control plugin demo

Try sending commands:

gz topic -t "/model/vehicle_blue/cmd_vel" -m gz.msgs.Twist -p "linear: {x: 0.5}, angular: {z: 0.05}"

gz topic -t "/model/vehicle_green/cmd_vel" -m gz.msgs.Twist -p "linear: {x: 1.0}, angular: {z: -0.1}"

-->
<sdf version="1.11">
<world name="velocity_control">

<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>

<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>1 1 1 1</diffuse>
<specular>0.5 0.5 0.5 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>

<model name="default_persistent_zero_velocity">
<pose>0 0 1.0 0 0 0</pose>
<link name="box_1x4x9">
<inertial auto="true"/>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.4 0.9</size>
</box>
</geometry>
<material>
<ambient>0.5 0.5 1.0 1</ambient>
<diffuse>0.5 0.5 1.0 1</diffuse>
<specular>0.0 0.0 1.0 1</specular>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.4 0.9</size>
</box>
</geometry>
</collision>
</link>
<plugin
filename="gz-sim-velocity-control-system"
name="gz::sim::systems::VelocityControl">
</plugin>
</model>

<model name="persistent_zero_velocity">
<pose>0 1 1.0 0 0 0</pose>
<link name="box_1x4x9">
<inertial auto="true"/>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.4 0.9</size>
</box>
</geometry>
<material>
<ambient>0.5 0.5 1.0 1</ambient>
<diffuse>0.5 0.5 1.0 1</diffuse>
<specular>0.0 0.0 1.0 1</specular>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.4 0.9</size>
</box>
</geometry>
</collision>
</link>
<plugin
filename="gz-sim-velocity-control-system"
name="gz::sim::systems::VelocityControl">
<initial_angular>0 0 0</initial_angular>
<initial_linear>0 0 0</initial_linear>
</plugin>
</model>

<model name="explicit_persistent_zero_velocity">
<pose>0 2 1.0 0 0 0</pose>
<link name="box_1x4x9">
<inertial auto="true"/>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.4 0.9</size>
</box>
</geometry>
<material>
<ambient>0.5 0.5 1.0 1</ambient>
<diffuse>0.5 0.5 1.0 1</diffuse>
<specular>0.0 0.0 1.0 1</specular>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.4 0.9</size>
</box>
</geometry>
</collision>
</link>
<plugin
filename="gz-sim-velocity-control-system"
name="gz::sim::systems::VelocityControl">
<initial_angular persistent="true">0 0 0</initial_angular>
<initial_linear persistent="true">0 0 0</initial_linear>
</plugin>
</model>

<model name="initial_zero_velocity">
<pose>0 3 1.0 0 0 0</pose>
<link name="box_1x4x9">
<inertial auto="true"/>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.4 0.9</size>
</box>
</geometry>
<material>
<ambient>0.5 0.5 1.0 1</ambient>
<diffuse>0.5 0.5 1.0 1</diffuse>
<specular>0.0 0.0 1.0 1</specular>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.4 0.9</size>
</box>
</geometry>
</collision>
</link>
<plugin
filename="gz-sim-velocity-control-system"
name="gz::sim::systems::VelocityControl">
<initial_angular persistent="false">0 0 0</initial_angular>
<initial_linear persistent="false">0 0 0</initial_linear>
</plugin>
</model>

<model name="initial_nonzero_velocity">
<pose>0 -1 1.0 0 0 0</pose>
<link name="box_1x4x9">
<inertial auto="true"/>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.4 0.9</size>
</box>
</geometry>
<material>
<ambient>0.5 0.5 1.0 1</ambient>
<diffuse>0.5 0.5 1.0 1</diffuse>
<specular>0.0 0.0 1.0 1</specular>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.4 0.9</size>
</box>
</geometry>
</collision>
</link>
<plugin
filename="gz-sim-velocity-control-system"
name="gz::sim::systems::VelocityControl">
<initial_angular persistent="false">0.1 5 0.1</initial_angular>
<initial_linear persistent="false">4 -2 10</initial_linear>
</plugin>
</model>

</world>
</sdf>
97 changes: 54 additions & 43 deletions src/systems/velocity_control/VelocityControl.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@

#include <map>
#include <mutex>
#include <optional>
#include <string>
#include <vector>
#include <unordered_map>
Expand Down Expand Up @@ -51,13 +52,6 @@ class gz::sim::systems::VelocityControlPrivate
public: void OnLinkCmdVel(const msgs::Twist &_msg,
const transport::MessageInfo &_info);

/// \brief Update the linear and angular velocities.
/// \param[in] _info System update information.
/// \param[in] _ecm The EntityComponentManager of the given simulation
/// instance.
public: void UpdateVelocity(const UpdateInfo &_info,
const EntityComponentManager &_ecm);

/// \brief Update link velocity.
/// \param[in] _info System update information.
/// \param[in] _ecm The EntityComponentManager of the given simulation
Expand All @@ -71,14 +65,19 @@ class gz::sim::systems::VelocityControlPrivate
/// \brief Model interface
public: Model model{kNullEntity};

/// \brief Angular velocity of a model
public: math::Vector3d angularVelocity{0, 0, 0};
/// \brief Model angular velocity command, initialized to zero.
public: std::optional<math::Vector3d> angularVelocity = math::Vector3d::Zero;

/// \brief Flag indicating whether the model angular velocity command should
/// persist across multiple time steps.
public: bool persistentAngularVelocity = true;

/// \brief Linear velocity of a model
public: math::Vector3d linearVelocity{0, 0, 0};
/// \brief Model linear velocity command, initialized to zero.
public: std::optional<math::Vector3d> linearVelocity = math::Vector3d::Zero;

/// \brief Last target velocity requested.
public: msgs::Twist targetVel;
/// \brief Flag indicating whether the model linear velocity command should
/// persist across multiple time steps.
public: bool persistentLinearVelocity = true;

/// \brief A mutex to protect the model velocity command.
public: std::mutex mutex;
Expand Down Expand Up @@ -122,21 +121,26 @@ void VelocityControl::Configure(const Entity &_entity,

if (_sdf->HasElement("initial_linear"))
{
this->dataPtr->linearVelocity = _sdf->Get<math::Vector3d>("initial_linear");
msgs::Set(this->dataPtr->targetVel.mutable_linear(),
this->dataPtr->linearVelocity);
auto elem = _sdf->FindElement("initial_linear");
this->dataPtr->linearVelocity = elem->Get<math::Vector3d>();
gzmsg << "Linear velocity initialized to ["
<< this->dataPtr->linearVelocity << "]" << std::endl;
<< *this->dataPtr->linearVelocity << "]" << std::endl;
if (elem->HasAttribute("persistent"))
{
this->dataPtr->persistentLinearVelocity = elem->Get<bool>("persistent");
}
}

if (_sdf->HasElement("initial_angular"))
{
this->dataPtr->angularVelocity =
_sdf->Get<math::Vector3d>("initial_angular");
msgs::Set(this->dataPtr->targetVel.mutable_angular(),
this->dataPtr->angularVelocity);
auto elem = _sdf->FindElement("initial_angular");
this->dataPtr->angularVelocity = elem->Get<math::Vector3d>();
gzmsg << "Angular velocity initialized to ["
<< this->dataPtr->angularVelocity << "]" << std::endl;
<< *this->dataPtr->angularVelocity << "]" << std::endl;
if (elem->HasAttribute("persistent"))
{
this->dataPtr->persistentAngularVelocity = elem->Get<bool>("persistent");
}
}

// Subscribe to model commands
Expand Down Expand Up @@ -200,13 +204,31 @@ void VelocityControl::PreUpdate(const UpdateInfo &_info,
if (_info.paused)
return;

// update angular velocity of model
_ecm.SetComponentData<components::AngularVelocityCmd>(
this->dataPtr->model.Entity(), {this->dataPtr->angularVelocity});
{
std::lock_guard<std::mutex> lock(this->dataPtr->mutex);

// update linear velocity of model
_ecm.SetComponentData<components::LinearVelocityCmd>(
this->dataPtr->model.Entity(), {this->dataPtr->linearVelocity});
// update angular velocity of model
if (this->dataPtr->angularVelocity)
{
_ecm.SetComponentData<components::AngularVelocityCmd>(
this->dataPtr->model.Entity(), {*this->dataPtr->angularVelocity});
if (!this->dataPtr->persistentAngularVelocity)
{
this->dataPtr->angularVelocity.reset();
}
}

// update linear velocity of model
if (this->dataPtr->linearVelocity)
{
_ecm.SetComponentData<components::LinearVelocityCmd>(
this->dataPtr->model.Entity(), {*this->dataPtr->linearVelocity});
if (!this->dataPtr->persistentLinearVelocity)
{
this->dataPtr->linearVelocity.reset();
}
}
}

// If there are links, create link components
// If the link hasn't been identified yet, look for it
Expand Down Expand Up @@ -275,24 +297,10 @@ void VelocityControl::PostUpdate(const UpdateInfo &_info,
if (_info.paused)
return;

// update model velocities
this->dataPtr->UpdateVelocity(_info, _ecm);
// update link velocities
this->dataPtr->UpdateLinkVelocity(_info, _ecm);
}

//////////////////////////////////////////////////
void VelocityControlPrivate::UpdateVelocity(
const UpdateInfo &/*_info*/,
const EntityComponentManager &/*_ecm*/)
{
GZ_PROFILE("VeocityControl::UpdateVelocity");

std::lock_guard<std::mutex> lock(this->mutex);
this->linearVelocity = msgs::Convert(this->targetVel.linear());
this->angularVelocity = msgs::Convert(this->targetVel.angular());
}

//////////////////////////////////////////////////
void VelocityControlPrivate::UpdateLinkVelocity(
const UpdateInfo &/*_info*/,
Expand All @@ -315,7 +323,10 @@ void VelocityControlPrivate::UpdateLinkVelocity(
void VelocityControlPrivate::OnCmdVel(const msgs::Twist &_msg)
{
std::lock_guard<std::mutex> lock(this->mutex);
this->targetVel = _msg;
this->linearVelocity = msgs::Convert(_msg.linear());
this->angularVelocity = msgs::Convert(_msg.angular());
this->persistentLinearVelocity = true;
this->persistentAngularVelocity = true;
}

//////////////////////////////////////////////////
Expand Down
Loading