Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Create a new Controller for the Shadow Hand

Overview

On the etherCAT hand or the simulated hand, it's possible (and quite easy) to implement your own controller, which can then be loaded dynamically by the controller manager.

Implementing the controller

The full code for this example can be found in sr_mechanism_controllers/example.

In this tutorial, we'll take a closer look at the update function: this function is the brain of your controller. It is called for each controller typically at 1kHz. It is where you'll update the force command you want to send to the motor, based on the current state of the robot.

Let's have a look at the code:

The function takes the current time and the sampling period as arguments. The period is the time difference used in a PID control loop. Inside the function we first make sure the pointers to the robot and the controlled joint state are valid.

   1  void SrhExampleController::update(const ros::Time& time, const ros::Duration& period)
   2   {
   3     assert(robot_ != NULL);
   4     assert(joint_state_->joint_);

We also make sure the controller has been initialised. If it's not the case, we reset the command to be sensible by calling the starting() function.

   1     //make sure the controller has been initialised,
   2     // to avoid sending a crazy command.
   3     if (!initialized_)
   4     {
   5       starting();
   6 
   7       initialized_ = true;
   8     }

We now compute the commanded effort we want to send to the motor. This is typically where you'll write your algorithm. The joint 0s, which are composed of the proximal and middle joints are treated differently as the other joints.

   1     //compute the commanded effort you want to send
   2     // to the motor: you can use whatever algorithm
   3     // you want. To see more complex examples on how
   4     // to use a pid loop / more than one loop, just
   5     // go to the src directory, and have a look at
   6     // srh_mixed_position_velocity_controller.cpp
   7 
   8     //we start by computing the position error
   9     double error_position = 0.0;
  10     if( has_j2 )
  11     {
  12       //For *J0, the position error is equal to the command - (*J1 + *J2)
  13       error_position = command_ - (joint_state_->position_ + joint_state_2->position_);
  14     }
  15     else
  16       error_position = command_ - joint_state_->position_;
  17 
  18     //Here I'm simply doing a dummy P controller, with a fixed gain.
  19     // It can't be used in the real life obviously. That's where you
  20     // should WRITE YOUR ALGORITHM
  21     commanded_effort = 10* error_position;

We update the commanded_effort_ variable for the relevant joint_state. This value will then be sent to the motor (or to the simulated motor) by the main loop.

   1     //Update the commanded effort.
   2     if( has_j2 ) //The motor in *J0 is attached to the *J2
   3       joint_state_2->commanded_effort_ = commanded_effort;
   4     else
   5       joint_state_->commanded_effort_ = commanded_effort;

We can publish useful information at 100Hz. This information can then be plotted using rxplot or recorded with rxbag to be able to tune the controllers, or debug them.

   1     if(loop_count_ % 10 == 0) //publishes the joint state at 100Hz
   2     {
   3       if(controller_state_publisher_ && controller_state_publisher_->trylock())
   4       {
   5         controller_state_publisher_->msg_.header.stamp = time;
   6         controller_state_publisher_->msg_.set_point = command_;
   7 
   8         if( has_j2 )
   9         {
  10           controller_state_publisher_->msg_.process_value = joint_state_->position_ + joint_state_2->position_;
  11           controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_ + joint_state_2->velocity_;
  12         }
  13         else
  14         {
  15           controller_state_publisher_->msg_.process_value = joint_state_->position_;
  16           controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_;
  17         }
  18 
  19         controller_state_publisher_->msg_.error = error_position;
  20         controller_state_publisher_->msg_.time_step = dt_.toSec();
  21         controller_state_publisher_->msg_.command = commanded_effort;
  22 
  23         controller_state_publisher_->unlockAndPublish();
  24       }
  25     }
  26     loop_count_++;
  27 
  28     last_time_ = time;
  29   }
  30 }

Compiling the controller

To be able to compile the controller, you should put the .hpp file in the include/sr_mechanism_controllers directory and the .cpp file in src.

You now have two different files to edit:

• controller_plugins.xml: This file is used by pluginlib to be able to load the controllers you've written. Add those lines (modified accordingly) to this file:

   1   <class name="sr_mechanism_controllers/SrhExampleController"
   2          type="controller::SrhExampleController"
   3          base_class_type="controller_interface::ControllerBase" />

• CMakeLists.txt: you also need to compile your controller. Just add the path to your .cpp file to the rosbuild_add_library command.

Loading the controller

Defining the controllers

To be able to load your controllers, you need to define which joint is controlled by which controller. For this, you need to write a yaml file which is going to be loaded to the parameter server.

Just specify in the file the name of the controller, the joint it controls and the type of controller you want to use. If you have more options to set for the controller (e.g. PID values, etc...), just add them to the file, and make sure you read them during the initialisation of the controller. The file need to be loaded to the parameter server as well (using a launch file for example).

Here we simply define the controller for FFJ0 and FFJ3, using our example controller.

sh_ffj0_example_controller:
  joint: FFJ0
  type: sr_mechanism_controllers/SrhExampleController

sh_ffj3_example_controller:
  joint: FFJ3
  type: sr_mechanism_controllers/SrhExampleController

A complete example of this file can be found in sr_description/hand/config/hand_controller_gazebo.yaml.

Using the controllers

Once you've started the robot, you need to start the controllers you want. The controllers are defined in your parameter server, and are now really to be started. To start them, you need to use the services made available by the controller manager.

You can add to your launch file:

• if you're using the simulated hand:

   1  <node name="spawn_gazebo_controllers"
   2           pkg="controller_manager" type="spawner" respawn="false" output="screen"
   3           args="sh_ffj0_example_controller sh_ffj3_example_controller " />

• if you're using the etherCAT hand:

   1  <node name="default_loaded_controllers_spawner"
   2         pkg="controller_manager" type="spawner" output="screen"
   3         args="--wait-for=/calibrated sh_ffj0_example_controller sh_ffj3_example_controller" />