Back

Setting Velocity on Joints and Links

Setting Velocity on Links And Joints

This tutorial will describe how to programatically set velocities on Joints and Links in Gazebo 7. This is a common task done in a custom plugin.

Examples

Downloaded an example plugin here. Follow these steps to build and run the plugin.

cd Downloads/set_vel_plugin/
mkdir build
cd build
cmake ..
make
GAZEBO_PLUGIN_PATH=. gazebo --pause --verbose ../set_velocity.world

Finally unpause the world to see everything move. All of the methods used to set velocity are explained below.

Methods

There are three methods to set velocity:

  1. Set Instantaneous Velocity
  2. Configure a joint motor (ODE only)
  3. Create a PID controller

Set Velocity Instantaneously

Advantages

  • Supported on all physics engines
  • Simple, only one function call
  • Object moves at target velocity right away

Disadvantages

  • Object doesn't "feel" a force accellerating it

All physics engines used by gazebo support setting an instananeous velocity. Objects move at the target speed without any forces or torques being applied. This means calls to Joint::GetForceTorque(), Link::GetWorldForce() and Link::GetWorldTorque() will not show any additional forces or torques when using this method.

The object is not constrained to the velocity permanently. Forces or torques may change the speed of the object after the velocity is set. It must be set every time step to keep the object at a constant velocity forever.

Set Velocity With Joint Motors

Advantages

  • Can reach target velocity in a single update
  • Object feels the force that accellerates it
  • The exact required force is used, no under or overshooting the target velocity

Disadvantages

  • ODE only (default physics engine used by gazebo)
  • When using to set a link's velocity all other degrees of freedom are locked

Joints motors can be used to reach a velocity by applying the exact required force to a joint. Gazebo only supports this method when using the ODE physics engine (the default engine). It relies on the ODE Joint Motor feature.

Setting Velocity Using PID Controllers

Advantages

  • Supported on all physics engines
  • Object feels the force that accellerates it
  • Does not need to lock other degrees of freedom

Disadvantages

  • PID gains need to be tuned per object
  • Can under shoot or over shoot the target velocity
  • It takes multiple updates to reach the target velocity

Another method of setting velocity is to use a PID controller to apply forces and torques. It works on all physics engines, but requires tuning constants specifically for the object being moved. Unlike the joint motor method, the other degrees of freedom are not locked while the forces are being applied.

In general the velocity cannot be achieved instantaneously with a PID controller. It must exist and be active for multiple updates to achive the target velocity.

Setting Velocity on Joints

This section will show how to use the three methods to set velocity on a joint.

Note Not all joints can be commanded to move at target velocity. Revolute, revolute2, prismatic, screw, and universal joints can be set. Ball, gearbox, and fixed joints cannot be set using any method described below. However, while a gearbox joint velocity cannot be set, the parent or child joint can be set if it is one of the supported joint types.

Set Joint Velocity Instantaneously

Velocity on joints can be set instantaneously using Joint::SetVelocity(). The velocity is achieved by moving the child link. Notice at first the top link (child) on the gray (leftmost) joint moves while the bottom link is stationary. The momentum of the top link causes the whole gray object to move when the joint limit is hit.

          this->model->GetJoint("gray_joint")->SetVelocity(0, 1.0);

It takes two parameters: axis, and velocity. The axis parameter is an index, and it may be 0 or 1. Zero means the first axis on the joint, and one means the second if applicable.

Type Number of Axes
prismatic 1
revolute 1
revolute2 2
screw 1
universal 2

The second parameter is the velocity. It is meters per second for prismatic joints, and radians per second for all others.

Set Joint Velocity Using Joint Motors

Configuring a joint motor is done using Joint::SetParam().

          this->model->GetJoint("orange_joint")->SetParam("fmax", 0, 100.0);
          this->model->GetJoint("orange_joint")->SetParam("vel", 0, 1.0);

It accepts three parameters: key, axis, and value. The key parameter is a string that names the parameter to be changed. The axis parameter is an index that may be 0 or 1.

Note The value parameter must have exactly the right type. Joint motors require setting double parameters. This call will work joint->SetParam('fmax', 0, 0.0) while this will have a runtime error joint->SetParam('fmax', 0, 0).

Setting up a joint motor requires requires two calls. The first call sets the key vel to the velocity the joint should travel at. It is meters per second for prismatic joints and radians per second for all others. The other call sets the key fmax. It is the maximum force or torque a joint motor may apply during a time step. Set it larger than the force required to be at the target velocity at the next time step. Set it smaller to apply a force over many time steps until the velocity is reached. Stop applying force by setting fmax back to zero.

Set Joint Velocity Using PID controllers

A PID controller can be used to apply forces on the joint axes. The class physics::JointController can manager the PID controllers for you.

          this->jointController.reset(new physics::JointController(
                this->model));
          this->jointController->AddJoint(model->GetJoint("purple_joint"));
          std::string name = model->GetJoint("purple_joint")->GetScopedName();
          this->jointController->SetVelocityPID(name, common::PID(100, 0, 0));
          this->jointController->SetVelocityTarget(name, 1.0);

The controller gains must be configured for each object being moved. The velocity target is meters per second for prismatic joints, and radians per second for all others. JointController::Update() must be called every time step to apply forces

          this->jointController->Update();

Setting Velocity on Links

This section will show how to use the three methods to set velocity on a link.

Set Link Velocity Instantaneously

          // Link velocity instantaneously without applying forces
          model->GetLink("white_link_0")->SetLinearVel({0, 1, 0});
          model->GetLink("white_link_1")->SetLinearVel({0, 1, 0});
          model->GetLink("white_link_1")->SetAngularVel({1, 0, 0});
          model->GetLink("white_link_2")->SetAngularVel({1, 0, 0});

Linear velocity on links can be set with Link::SetLinearVel(). Angular velocity on links can be set with Link::SetAngularVel(). Both accept a three dimensional vector with the target linear velocity. The velocity must be expressed in the world frame in meters per second or radians per second.

Set Link Velocity Using Joint Motors

Joint motors can be used to move links by creating a joint connecting the link to the world. It is critical that the joints are created when the velocity is to be applied, and deleted afterwards.

Linear velocity can be set by creating a prismatic joint between the world and the link to be moved. Then a joint moter can be configured as described above. The link will not rotate or move off the prismatic joint axis until the joint is detached.

        // create prismatic joint with the world as a parent
        physics::ModelPtr model = _link->GetModel();
        physics::WorldPtr world = physics::get_world("default");
        physics::PhysicsEnginePtr engine = world->GetPhysicsEngine();
        this->joint = engine->CreateJoint("prismatic");
        this->joint->SetName(model->GetName() + "__perfect_lin_joint__");
        physics::LinkPtr worldLink = boost::dynamic_pointer_cast<physics::Link>(
            world->GetByName("world"));
        math::Pose jointOrigin;
        this->joint->Load(worldLink, _link, jointOrigin);
        this->joint->Init();
        double magnitude = _vel.GetLength();
        this->joint->SetAxis(0, _vel.Normalize());
        this->joint->SetParam("fmax", 0, _maxForce);
        this->joint->SetParam("vel", 0, magnitude);

Angular velocity can be set by creating a revolute joint between the world and the link to be moved. The link will only be able to rotate about the revolute joint axis until the joint is detached.

        // create revolute joint with the world as a parent
        physics::ModelPtr model = _link->GetModel();
        physics::WorldPtr world = physics::get_world("default");
        physics::PhysicsEnginePtr engine = world->GetPhysicsEngine();
        this->joint = engine->CreateJoint("revolute");
        this->joint->SetName(model->GetName() + "__perfect_ang_joint__");
        physics::LinkPtr worldLink =
          boost::dynamic_pointer_cast<physics::Link>(world->GetByName("world"));
        math::Pose jointOrigin;
        this->joint->Load(worldLink, _link, jointOrigin);
        this->joint->Init();
        double magnitude = _vel.GetLength();
        this->joint->SetAxis(0, _vel.Normalize());
        this->joint->SetParam("fmax", 0, _maxTorque);
        this->joint->SetParam("vel", 0, magnitude);

Controlling both linear and angular velocity at the same time requires an additional link. The link should be programmatically created with the same origin as the link whose velocity is to be set. Create a prismatic joint with the world as the parent and the phantom link as the child. Then create a revolute joint with the phantom link as the parent and the link to be moved as the child. Finally create joint motors on both joints to reach the target velocity.

        // Create a phantom link
        this->phantomLink = model->CreateLink("__perfect_phantom_link__");
        sdf::ElementPtr config(new sdf::Element);
        config->Copy(_link->GetSDF()->Clone());
        config->GetAttribute("name")->Set("__perfect_phantom_link__");
        // Remove visuals/collisions/inertials
        while (config->HasElement("visual"))
        {
          config->RemoveChild(config->GetElement("visual"));
        }
        while (config->HasElement("collision"))
        {
          config->RemoveChild(config->GetElement("collision"));
        }
        while (config->HasElement("inertial"))
        {
          config->RemoveChild(config->GetElement("inertial"));
        }
        this->phantomLink->Load(config);
        this->phantomLink->Init();

        // create prismatic joint with parent "world" and child phantomLink
        this->prismaticJoint = engine->CreateJoint("prismatic");
        this->prismaticJoint->SetName(
            model->GetName() + "__perfect_vel_lin_joint__");
        physics::LinkPtr worldLink = boost::dynamic_pointer_cast<physics::Link>(
            world->GetByName("world"));
        math::Pose prismaticJointOrigin;
        this->prismaticJoint->Load(worldLink, this->phantomLink,
            prismaticJointOrigin);
        this->prismaticJoint->Init();
        double linearMagnitude = _linearVel.GetLength();
        this->prismaticJoint->SetAxis(0, _linearVel.Normalize());
        this->prismaticJoint->SetParam("fmax", 0, _maxForce);
        this->prismaticJoint->SetParam("vel", 0, linearMagnitude);

        // create revolute joint with parent phantomLink and child _link
        this->revoluteJoint = engine->CreateJoint("revolute");
        this->revoluteJoint->SetName(
            model->GetName() + "__perfect_vel_ang_joint__");
        math::Pose revoluteJointOrigin;
        this->revoluteJoint->Load(this->phantomLink, _link,
            revoluteJointOrigin);
        this->revoluteJoint->Init();
        double angularMagnitude = _angularVel.GetLength();
        this->revoluteJoint->SetAxis(0, _angularVel.Normalize());
        this->revoluteJoint->SetParam("fmax", 0, _maxTorque);
        this->revoluteJoint->SetParam("vel", 0, angularMagnitude);

Set Link Velocity Using PID controllers

A PID controller can be used to set a velocity by appling forces or torques. This requires tuning constants for each object whose velocity is to be set.

      // Hard coded gains. Tune these for your own application!
      double linear_p = 100.0;
      double linear_i = 0.0;
      double linear_d = 0.0;
      double linear_imax = 123456789.0;
      double angular_p = 100.0;
      double angular_i = 0.0;
      double angular_d = 0.0;
      double angular_imax = 123456789.0;

Each degree of freedom (x, y, z, roll, pitch, yaw) must have it's own PID controller. Fewer controllers can be used if it is permissable for the link to move freely on some degrees of freedom. For example, setting a translational velocity while allowing the object to rotate requries only 3 PID controllers: x, y, z.

      // Add a PID controller for each DoF
      for (int i = 0; i < 3; i++)
      {
        common::PID controller_translation(linear_p, linear_i, linear_d,
            linear_imax, -linear_imax, _maxForce, -_maxForce);
        common::PID controller_rotation(angular_p, angular_i, angular_d,
            angular_imax, -angular_imax, _maxTorque, -_maxTorque);
        this->controllers.push_back(controller_translation);
        this->controllers.push_back(controller_rotation);
      }

Every physics update the error between the actual velocity and the target velocity needs to be given to the controller.

      // Calculate the error between actual and target velocity
      math::Vector3 curLinearVel = this->link->GetWorldLinearVel();
      math::Vector3 curAngularVel = this->link->GetWorldAngularVel();
      math::Vector3 linearError = curLinearVel - this->targetLinearVel;
      math::Vector3 angularError = curAngularVel - this->targetAngularVel;

      // Get forces to apply from controllers
      math::Vector3 worldForce;
      math::Vector3 worldTorque;
      worldForce.x = this->controllers[0].Update(linearError.x, dt);
      worldTorque.x = this->controllers[1].Update(angularError.x, dt);
      worldForce.y = this->controllers[2].Update(linearError.y, dt);
      worldTorque.y = this->controllers[3].Update(angularError.y, dt);
      worldForce.z = this->controllers[4].Update(linearError.z, dt);
      worldTorque.z = this->controllers[5].Update(angularError.z, dt);

The controllers will output forces and torques that should be applied to the link to correct for the current error.

      // Add those forces to the body
      this->link->AddForce(worldForce);
      this->link->AddTorque(worldTorque);

The object will move at the desired velocity. The amount of velocity error depends on the PID gains chosen. Tune these until you get acceptable performance.