## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links
## note.0= [[pr2_mechanism/Tutorials/Writing a realtime joint controller|Writing a realtime joint controller]]
## descriptive title for the tutorial
## title = Create a new Controller for the Shadow Hand
## multi-line description to be displayed in search
## description = Create a new controller for the Shadow Robot Hand (EtherCAT or simulated in Gazebo).
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=
## next.1.link=
## what level user is this tutorial for
## level= AdvancedCategory
## keywords = Control, Shadow Robot
####################################
<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>
<<TableOfContents(4)>>
## AUTOGENERATED DO NOT DELETE
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE
== 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 [[https://github.com/shadow-robot/sr-ros-interface/tree/indigo-devel/sr_mechanism_controllers/example/|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.
{{{#!cplusplus block=controller_h
void SrhExampleController::update(const ros::Time& time, const ros::Duration& period)
{
assert(robot_ != NULL);
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.
{{{#!cplusplus block=controller_h
//make sure the controller has been initialised,
// to avoid sending a crazy command.
if (!initialized_)
{
starting();
initialized_ = true;
}
}}}
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.
{{{#!cplusplus block=controller_h
//compute the commanded effort you want to send
// to the motor: you can use whatever algorithm
// you want. To see more complex examples on how
// to use a pid loop / more than one loop, just
// go to the src directory, and have a look at
// srh_mixed_position_velocity_controller.cpp
//we start by computing the position error
double error_position = 0.0;
if( has_j2 )
{
//For *J0, the position error is equal to the command - (*J1 + *J2)
error_position = command_ - (joint_state_->position_ + joint_state_2->position_);
}
else
error_position = command_ - joint_state_->position_;
//Here I'm simply doing a dummy P controller, with a fixed gain.
// It can't be used in the real life obviously. That's where you
// should WRITE YOUR ALGORITHM
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.
{{{#!cplusplus block=controller_h
//Update the commanded effort.
if( has_j2 ) //The motor in *J0 is attached to the *J2
joint_state_2->commanded_effort_ = commanded_effort;
else
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.
{{{#!cplusplus block=controller_h
if(loop_count_ % 10 == 0) //publishes the joint state at 100Hz
{
if(controller_state_publisher_ && controller_state_publisher_->trylock())
{
controller_state_publisher_->msg_.header.stamp = time;
controller_state_publisher_->msg_.set_point = command_;
if( has_j2 )
{
controller_state_publisher_->msg_.process_value = joint_state_->position_ + joint_state_2->position_;
controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_ + joint_state_2->velocity_;
}
else
{
controller_state_publisher_->msg_.process_value = joint_state_->position_;
controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_;
}
controller_state_publisher_->msg_.error = error_position;
controller_state_publisher_->msg_.time_step = dt_.toSec();
controller_state_publisher_->msg_.command = commanded_effort;
controller_state_publisher_->unlockAndPublish();
}
}
loop_count_++;
last_time_ = time;
}
}
}}}
== 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:
{{{#!xml
<class name="sr_mechanism_controllers/SrhExampleController"
type="controller::SrhExampleController"
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 [[https://github.com/shadow-robot/sr-ros-interface/blob/indigo-devel/sr_description/hand/config/hand_controller_gazebo.yaml|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:
{{{#!xml
<node name="spawn_gazebo_controllers"
pkg="controller_manager" type="spawner" respawn="false" output="screen"
args="sh_ffj0_example_controller sh_ffj3_example_controller " />
}}}
* if you're using the etherCAT hand:
{{{#!xml
<node name="default_loaded_controllers_spawner"
pkg="controller_manager" type="spawner" output="screen"
args="--wait-for=/calibrated sh_ffj0_example_controller sh_ffj3_example_controller" />
}}}