Random Velocity Plugin


Overview

The Random Velocity Plugin allows you to assign a random velocity to a link in the world.

You can set the maximum and minimum permissible values of the x, y and z velocity components, and the velocity magnitude. Additionally, you can set the time period after which the velocity can change itself. Once you have set the velocity magnitude, the direction is set on its own (by random selection).

Prerequisites

Hello World Plugin

Example

  1. Change your current working directory to the location of the worlds folder in your Gazebo source (use the path to your Gazebo source).
  2. Open random_velocity.world in your default editor.

    $ cd path/worlds
    $ gedit random_velocity.world
    
  3. You will observe code like this : gazebo/worlds/random_velocity.world

World File Explained

The above code is just example usage of the Random Velocity Plugin.

First, we create a model and a link. In this case, our model is a cube of unit dimensions. The cube's surface is frictionless, since the coefficient of friction (mu) is set to zero.

    <model name="box">
      <pose>0 0 0.5 0 0 0</pose>
      <link name="link">
        <kinematic>true</kinematic>
        <collision name="collision">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>0.0</mu>
                <mu2>0.0</mu2>
              </ode>
            </friction>
          </surface>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
        </visual>
      </link>

Now comes the actual usage of the plugin.

      <plugin name="random" filename="libRandomVelocityPlugin.so">

        <!-- Name of the link in this model that receives the velocity -->
        <link>link</link>

        <!-- Initial velocity that is applied to the link -->
        <initial_velocity>0 0.5 0</initial_velocity>

        <!-- Scaling factor that is used to compute a new velocity -->
        <velocity_factor>0.5</velocity_factor>

        <!-- Time, in seconds, between new velocities -->
        <update_period>5</update_period>

        <!-- Clamp the Z velocity value to zero. You can also clamp x and
             y values -->
        <min_z>0</min_z>
        <max_z>0</max_z>
      </plugin>
  1. Plugin name is "random" and its corresponding shared object file(.so) is libRandomVelocityPlugin.so. .so files are dynamically linked at runtime.

  2. You can apply an initial velocity to the link, using the initial_velocity tag.

  3. velocity_factor is the magnitude of new velocities that will be generated after each time period equal to the update time. Direction will be random but magnitude will remain constant.

  4. Clamping indicates that a range is set. Maximum velocity in the y direction cannot exceed the max_y value, and minumum velocity cannot be less than the min_y value.

The default value for scale is 1, update time is 10, and (min_i, max_i) are (-1.79769e+308, 1.79769e+308). 1.79769e+308 is the maximum allowed value of float by the Ignition Math library.

You can run the plugin using:

gazebo worlds/random_velocity.world

You can play with the values of initial_velocity, velocity_factor, update_period etc., and observe how these values affect the simulation.

Plugin Source

The source code for this plugin is available on bitbucket.

If you have installed Gazebo from source then you can find this file where you downloaded the repository.

$ cd /YourPath/gazebo/plugins/RandomVelocityPlugin.cc

If you have installed from debian, then you can only locate header(.hh) and not (.cc). You can try finding the location using,

$ locate RandomVelocityPlugin.hh

RandomVelocityPlugin.hh contains a commented example of usage (as included above) and function declarations of the functions defined in RandomVelocityPlugin.cc, which looks like this.

Understanding the source

#include <ignition/math/Rand.hh>
#include <gazebo/common/Events.hh>
#include <gazebo/common/Assert.hh>
#include <gazebo/physics/Model.hh>
#include <gazebo/physics/Link.hh>
#include "RandomVelocityPluginPrivate.hh"
#include "RandomVelocityPlugin.hh"

RandomVelocityPluginPrivate.cc contains the private data pointer, in accordance with the PIMPL idiom implementation (opaque pointers). The default initial values of all variables are set in it only. All other #includes are necessary for various parts of code. For example:

  1. <ignition/math/Rand.hh> for ignition::math::Rand::DblUniform(-1, 1)

  2. <gazebo/common/Assert.hh> for GZ_ASSERT

  3. <gazebo/physics/Model.hh> for physics::ModelPtr _model

using namespace gazebo;

To avoid writing gazebo repeatedly, before using Gazebo routines, structures or object classes.

GZ_REGISTER_MODEL_PLUGIN(RandomVelocityPlugin)

In the hello world tutorial we learned that plugins must be registered with the simulator using the GZ_REGISTER_WORLD_PLUGIN macro.

RandomVelocityPlugin::RandomVelocityPlugin()
  : dataPtr(new RandomVelocityPluginPrivate)
{
}

/////////////////////////////////////////////////
RandomVelocityPlugin::~RandomVelocityPlugin()
{
  delete this->dataPtr;
}

The first function is a constructor function that initializes the data objects using Private class RandomVelocityPluginPrivate. The second function is a destructor function that deletes the pointer pointing to this instance's data.

Similar to what we did in Hello World, we create function RandomVelocityPlugin::Load() Parameters passed in are:

  1. physics::ModelPtr: A model is a collection of links, joints, and plugins.

  2. sdf::ElementPtr: A pointer to the SDF element for the plugin in the world file.

The function checks that ModelPtr is not null and sdf::ElementPtr has the elements elements which are accepted by the plugin.

  // Get x clamping values
  if (_sdf->HasElement("min_x"))
    this->dataPtr->xRange.X(_sdf->Get<double>("min_x"));
  if (_sdf->HasElement("max_x"))
    this->dataPtr->xRange.Y(_sdf->Get<double>("max_x"));

If min_x exists for _sdf then, xRange.X() is set to min_x. The same goes for max_y. [x/y/z]Range.X() indicate min value and [x/y/z]Range.Y() indicate max value. Their default values are set in RandomVelocityPluginPrivate.cc.

  // Set the initial velocity, if present
  if (_sdf->HasElement("initial_velocity"))
  {
    this->dataPtr->velocity =
      _sdf->Get<ignition::math::Vector3d>("initial_velocity");
  }
  // Set the velocity factor
  if (_sdf->HasElement("velocity_factor"))
    this->dataPtr->velocityFactor = _sdf->Get<double>("velocity_factor");

  // Set the update period
  if (_sdf->HasElement("update_period"))
    this->dataPtr->updatePeriod = _sdf->Get<double>("update_period");

You can learn about ignition::math::Vector3d here. The other two are simple setter functions.

this->dataPtr->updateConnection = event::Events::ConnectWorldUpdateBegin(
      std::bind(&RandomVelocityPlugin::Update, this, std::placeholders::_1));

This makes a connection between the plugin and world update events. An Event class is used to get notifications for simulator events. The worldUpdateBegin event is fired at each physics iteration. ConnectWorldUpdateBegin takes in the callback to this event and returns a connection.

bind is a C++ standard function; it generates a forward calling wrapper. Calling bind is equivalent to invoking Update, with arguments as placeholders::_1. The std::placeholders namespace contains the placeholder objects [_1, . . . _N] where N is an implementation-defined maximum number.

void RandomVelocityPlugin::Update(const common::UpdateInfo &_info)
{
  GZ_ASSERT(this->dataPtr->link, "<link> in RandomVelocity plugin is null");

  // Short-circuit in case the link is invalid.
  if (!this->dataPtr->link)
    return;

  // Change direction when enough time has elapsed
  if (_info.simTime - this->dataPtr->prevUpdate > this->dataPtr->updatePeriod)
  {
    // Get a random velocity value.
    this->dataPtr->velocity.Set(
        ignition::math::Rand::DblUniform(-1, 1),
        ignition::math::Rand::DblUniform(-1, 1),
        ignition::math::Rand::DblUniform(-1, 1));

    // Apply scaling factor
    this->dataPtr->velocity.Normalize();
    this->dataPtr->velocity *= this->dataPtr->velocityFactor;

    // Clamp X value
    this->dataPtr->velocity.X(ignition::math::clamp(this->dataPtr->velocity.X(),
        this->dataPtr->xRange.X(), this->dataPtr->xRange.Y()));

    // Clamp Y value
    this->dataPtr->velocity.Y(ignition::math::clamp(this->dataPtr->velocity.Y(),
        this->dataPtr->yRange.X(), this->dataPtr->yRange.Y()));

    // Clamp Z value
    this->dataPtr->velocity.Z(ignition::math::clamp(this->dataPtr->velocity.Z(),
        this->dataPtr->zRange.X(), this->dataPtr->zRange.Y()));

    this->dataPtr->prevUpdate = _info.simTime;
  }

  // Apply velocity
  this->dataPtr->link->SetLinearVel(this->dataPtr->velocity);
}

This is the update function invoked above. UpdateInfo &_info primarily contains three pieces of information:

  1. Real time (realTime)

  2. Current simulation time (simTime)

  3. Name of the world (worldName)

Other important functions and classes used are:

  1. DblUniform : Gets a random double value from a uniform distribution.

  2. Normalize() : Normalizes the vector length by returning a unit length vector.

  3. ignition::math::vector3d

  4. Clamp

  5. SetLinearVelocity

You can modify the plugin by involving angular acceleration, linear acceleration, and you can have keep them random or fixed. You can also play with the relative velocity of two links by making it constant, and applying random velocity to one link and to see how the velocity of the other link changes to maintain that relative velocity. You could also involve external force factors.