View source for ROS:ControllingRobots

= Controlling Robots with ROS/OpenRAVE =

This tutorial explains how to control robots via the [http://www.ros.org/browse/details.php?name=openrave_robot_control openrave_robot_control] package.


By using this framework, a controller gains:

* All robot-specific settings (like masses, max velocities, limits) come from openrave xml files to allow for consistent integration with planners.
* Forward/Inverse kinematics, jacobian computation, and dynamics computation.
* Automatically published TF Frames of all the links of the robot.
* Integration for openrave to send trajectory, velocity, and torque commands.
* Controller-level self-collision checking.
* Advertisement of services through ROS along with concept of '''sessions''' managing exclusive access to robot control.
* Trajectory re-timing and smoothing by OpenRAVE.

= Setting up a Robot Example =
these instructions assume you have installed the required packages for ros and openrave.  See [[Installation]].

==Starting a ROS robot controller==

The first thing you need to do is startup up a robot controller, that advertises services that OpenRAVE can talk to.  There are several options available:

#'''Simulated Controller'''
#*The simplest thing is to start a simulated controller, that will let you see if your messages are getting through. Such a simulation exists in the openrave_robot_control package
#*It can be started by running: <br /> <pre>rosrun openrave_robot_control simulationserver --robotfile robots/schunk-lwa3.robot.xml --jointname j0 --jointname j1 --jointname j2 --jointname j3 --jointname j4 --jointname j5 --jointname j6</pre>
#*Or the same command through a launchfile:<br /><pre>  roslaunch openrave_robot_control schunkarm_sim.launch.xml</pre>
#**The robot file, just like in normal OpenRAVE, found by searching the paths defined by the environmental variable OPENRAVE_DATA
#**the joints you wish to control must all be stated explicitly with the joint name given in the kinbody XML file
#*If the physics engine on openrave is set, the robot could be torque controlled and exhibit similar behaviors as the real robot. It is also possible to load in an environment in which the robot resides for getting environment contact forces.
#'''Actual Controller'''
#*Obviously, this is going to depend on your particular robot. Here are some available controllers:
{|border="1" cellpadding="10"
!Hardware !! Package !! Executable !! Repository !! Interface !! Usage
|-
|Schunk LWA3 
| [http://www.ros.org/browse/details.php?name=schunk_motion_controllers schunk_motion_controllers] 
| shunk_server 
| [http://sourceforge.net/projects/cmu-ros-pkg/ cmu-ros-pkg] 
| Serial and CAN(ntcan lib needed) || schunk_server [--device ''can or serial''] [--moduleid id] [--robotfile openravefile] [--maxvelmult multiplier] [--serial serialport] [--can canport]
|-
|Mitsubishi PA10        
| [http://www.ros.org/browse/details.php?name=pa10controller pa10controller]            
| pa10server   
| [http://sourceforge.net/projects/cmu-ros-pkg/ cmu-ros-pkg] 
| ??             
| pa10server [--robotfile openravefile] [--manipname manipulator name] [--maxvelmult multiplier]
|-
|maxon motors
| [http://www.ros.org/browse/details.php?name=maxondrivers maxondrivers]
| eposserver
| [http://sourceforge.net/projects/cmu-ros-pkg/ cmu-ros-pkg] 
| CAN (ntcan library)
| eposserver [--can port] [--robotfile openravefile] [--manipname name] [--maxvelmult multiplier]
|-
|maxon motors + pid
| [http://www.ros.org/browse/details.php?name=doorarmcontroller doorarmcontroller]
| doorarmcontroller
| [http://sourceforge.net/projects/cmu-ros-pkg/ cmu-ros-pkg] 
| openrave_robot_control
| doorarmcontroller [--can port] [--robotfile openravefile] [--manipname name] [--maxvelmult multiplier]
|}

=== Verify the controller is running ===
You will notice the above ROS nodes advertise ROS services (They are actually sessions, which are collections of services)
You can check that they are advertising by running:
  rosservice list
  rostopic list 
For the simulated controller, you will see the additional services:
  /Brake
  /Cancel
  /Query
  /StartTorque
  /StartTrajectory
  /StartVelocity
  /Wait
  /controller_session
  /schunk_simulation/get_loggers
  /schunk_simulation/set_logger_level
and the additional topics:
  /mechanism_state
  /tf
Especially important is the '/controller_session' service.  This is how OpenRAVE will talk with the ROS node.

=== OpenRAVE Simulated Controller Example ===

Here is an example of openrave connecting to a robot simulated using '''simulationserver''' and setting random values. To start the example do:

 roslaunch openrave_robot_control wam_sim.launch.xml

The ROS launch file is:

<source lang="xml">
<!-- shows how to start a simple simulation controller on the WAM-->
<launch>
  <machine name="wam" address="localhost"/>
  <node machine="wam" name="wam" pkg="openrave_robot_control" type="simulationserver" respawn="false" output="screen" args="--robotfile robots/barrettsegway.robot.xml --manipname arm --maxvelmult 1.0">
  </node>
  <node machine="wam" name="openrave" pkg="openrave_robot_control" type="testopenravecontrol.py" respawn="false">
    <env name="PYTHONPATH" value="$(optenv PYTHONPATH):$(find openrave)/share/openrave"/>
    <env name="OPENRAVE_PLUGINS" value="$(optenv OPENRAVE_PLUGINS):$(find openrave)/share/openrave/plugins:$(find openrave_robot_control)/lib"/>
  </node>
</launch>
</source>

The simple openravepy file to set the commands is:
<source lang="python">
#!/usr/bin/env python
from openravepy import *
from numpy import *
import time
env = Environment() # create openrave environment
env.SetViewer('qtcoin')
env.Load('robots/barrettwam.robot.xml')
robot = env.GetRobots()[0] # get the first robot
manip = robot.GetManipulators()[0]
jointnames = ' '.join(robot.GetJoints()[j].GetName() for j in manip.GetArmJoints())
robot.SetController(env.CreateController('ROSOpenRAVE + trajectoryservice /controller_session '+jointnames))

lower,upper = robot.GetJointLimits()

# sending velocity command?
#robot.GetController().SendCommand("setvelocity 4 .01")

while True:
    with robot: # save the robot state and get random joint values that are collision free
        while True:
            values = lower + random.rand(len(lower))*(upper-lower)
            robot.SetJointValues(values)
            if not robot.CheckSelfCollision() and not env.CheckCollision(robot):
                break
    print 'setting: ',values
    robot.GetController().SetDesired(values)
    robot.WaitForController(0)
    time.sleep(1.0)

env.Destroy()
</source>

== Loading the Robot into the OpenRAVE Master ==
==== Add ROSOpenRAVE Controller to the Path ====
You need to add the directory holding the '''librobot_control.so''' openrave plugin to the OPENRAVE_PLUGINS environment variables. This is possible by putting the following path inside the bashrc file:

 export OPENRAVE_PLUGINS=$OPENRAVE_PLUGINS:`rospack find openrave`/share/openrave/plugins:`rospack find openrave_robot_control`/lib 

Executing 'openrave --listplugins' should show the '''ROSOpenRAVE''' controller interface.

==== Load the ROSOpenRAVE controller interface into OpenRAVE ====

Load the ROSOpenRAVE controller interface into openrave coming from the librobot_control.so and tell it to read the correct schunk service. The shared object contains a '''Controller''' interface, which has to be set on the robot in order to take effect. See [[Started:SettingControllers|Setting Controllers]] for how to do this.

Another cool thing with the ROSOpenRAVE interface is that any number of separate pieces of hardware can be trated as the same robot (for example two schunk arms + gripper). All you have to do is specify multiple '''trajectoryservice''' tags for each root service your controllers publish. For example, these are the luanch scripts for setting up a PA10 arm and a Schunk serial manipulator:

<source lang="xml">
<launch>
  <machine name="pa10" address="localhost"/>
  <machine name="schunk" address="localhost"/>
 <group ns="pa10">
   <node machine="pa10" name="pa10" pkg="pa10controller" type="pa10server" respawn="false" output="screen" args="--robotfile robots/pa10schunk.robot.xml --maxvelmult 0.1">
     <env name="OPENRAVE_DATA" value="$(optenv OPENRAVE_DATA):$(find openrave)/share/openrave"/>
   </node>
 </group>
 <group ns="schunk">
   <node machine="schunk" name="schunk" pkg="schunk_motion_controllers" type="schunk_serial_server" respawn="true" output="screen" args=" --robotfile robots/pa10schunk.robot.xml --serial /dev/ttyUSB1">
     <env name="OPENRAVE_DATA" value="$(optenv OPENRAVE_DATA):$(find openrave)/share/openrave"/>
   </node>
 </group>
</launch>
</source>

(OPENRAVE_DATA tells the system where to look for the openrave robot files. )

This will setup two namespaces with these root services
 /pa10/controller_session
 /schunk/controller_session

Then in openrave, load the ROSOpenRAVE controller interface with the following arguments

 trajectoryservice /pa10/controller_session trajectoryservice /schunk/controller_session joints S1 S2 S3 E1 E2 W1 W2 jfinger_L

This tells the controller that the joints it should control are "S1 S2 S3 E1 E2 W1 W2 jfinger_L", which come from the ROS services.

==== Using Python for Testing Controller Communication ====
When running a ROS robot controller, the '''openrave_robot_control/scripts/controlclient.py''' script allows users to  send commands to the robot controller without starting a master openrave instance.

= Creating a Controller Driver =

New robot controllers taking advantage of this framework need to depend on the '''openrave_robot_control''' package and derive from the '''openrave_robot_control::OpenRAVEController''' class. New implementations need a thread that constantly calls OpenRAVEController::_publishTF, fills and publishes OpenRAVEController::_mstate, and processes OpenRAVEController::_listCommands.

The controller offers several modes of operation, each mode has a '''start''', '''run''', and '''finish''' functions. To hook into trajectory control, implementations of these member functions are needed:
<source lang="cpp">
virtual void _startTrajectoryCommand(TrajectoryBasePtr ptraj);
virtual CommandStatus _runTrajectoryCommand(TrajectoryBasePtr ptraj, float fCommandTime);
virtual void _finishTrajectoryCommand(TrajectoryBasePtr ptraj);
</source>

<!--
== Simulating a Controller over ROS ==

The '''openrave_robot_control''' package has a simulation controller which will run the server inside openrave. Basically it is a replacement for the hardware equivalents, which can be used to test planning and programs without running the robot. For example, to simulate joints j0 through j6 on a schunk LWA3 robot do:

 rosrun openrave_robot_control simulationserver --robotfile robots/schunk-lwa3.robot.xml --jointname j0 --jointname j1 --jointname j2 --jointname j3 --jointname j4 --jointname j5 --jointname j6

If this physics engine on openrave is set, the robot could be torque controlled and exhibit similar behaviors as the real robot. It is also possible to load in an environment in which the robot resides for getting environment contact forces.
-->