ROS:ControllingRobots

From OpenRAVE
Jump to: navigation, search

Controlling Robots with ROS/OpenRAVE

This tutorial explains how to control robots via the 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

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:

  1. 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: 
      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
    • Or the same command through a launchfile:
        roslaunch openrave_robot_control schunkarm_sim.launch.xml
      • 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.
  2. Actual Controller
    • Obviously, this is going to depend on your particular robot. Here are some available controllers:
Hardware Package Executable Repository Interface Usage
Schunk LWA3 schunk_motion_controllers shunk_server 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 pa10controller pa10server cmu-ros-pkg  ?? pa10server [--robotfile openravefile] [--manipname manipulator name] [--maxvelmult multiplier]
maxon motors maxondrivers eposserver cmu-ros-pkg CAN (ntcan library) eposserver [--can port] [--robotfile openravefile] [--manipname name] [--maxvelmult multiplier]
maxon motors + pid armrobot armrobot cmu-ros-pkg openrave_robot_control armrobot [--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"> <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="OPENRAVE_PLUGINS" value="$(optenv OPENRAVE_PLUGINS):$(find openrave_robot_control)/lib"/>
 </node>

</launch> </source>

The simple openravepy file to set the commands is: <source lang="python">

  1. !/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()

  1. sending velocity command?
  2. 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 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>


Retrieved from "http://openrave.programmingvision.com/wiki/index.php?title=ROS:ControllingRobots&oldid=226"