Started:Scripting

From OpenRAVE

Contents 1 Introduction to Scripting 2 Communicating with OpenRAVE 3 Tutorial 3.1 Basic commands 3.2 Executing planners - A Grasping Example 3.3 Logging 4 Matlab/Octave Partial Reference

Introduction to Scripting

One of the biggest features that separates OpenRAVE from other simulation/planning environments is that it supports scripting over the network. This makes it possible to free OpenRAVE of complex GUIs. At initialization, OpenRAVE starts listening on specific ports for the commands. This allows any computer to open a socket connection with OpenRAVE, which can be running on a different computer, and communicate with it. The official release supports Matlab and Octave as the scripting engines. Note that all script commands are sent as text across the network; therefore, it is possible to do everything in Python or Perl.

The rest of this document is written using the Matlab/Octave functions found in the matlab/ directory. For any function, type 'help functionname' to get a help message describing its parameters and usage. Some formatting rules:

• All OpenRAVE functions start with or.
• All KinBody specific functions start with orBody.
• All Robot specific functions start with orRobot. Every robot can use all orBody functions.
• All general environment functions start with orEnv.
• All problem instance specific functions start with orProblem

Check out the RaveServer constructor in src/server.cpp for a complete list of commands supported.

Communicating with OpenRAVE

Usually communicating with an OpenRAVE instance running on the same computer as the scripting environment is simple. Just call the methods in the matlab folder directly without having to worry about setting ip address. If OpenRAVE is running on a different computer, the IP address will have to be set manually through a global variable orConnectionParams. To set the address of the remote OpenRAVE in Octave/Matlab instance type

global orConnectionParams
orConnectionParams.ip = 'myopenrave_ip'; 
orConnectionParams.port = 4765; 

The scripting environment can communicate with multiple OpenRAVE instances at once, just set orConnectionParams to the appropriate instance. Also, OpenRAVE can handle multiple scripting environments talking to the same instance simultaneously.

The default port openrave starts is 4765, use the -server option to change the port. For example,

./openrave -server 3000

starts openrave to listen for connections on port 3000.

Tutorial

NOTE: Remember that warnings and responses can appear both in the Matlab/Octave window and the console window where openrave is started from.

Basic commands

First start openrave and leave it in the background. Then start an Octave or Matlab instance and make sure the paths to the Octave/Matlab scripts are added to the path (Install OpenRAVE)

To load a simple scene with the Barrett WAM and Hand:

orEnvLoadScene('data/lab1.env.xml', 1)

The 1 is a flag to reset the whole OpenRAVE scene before loading lab1.env.xml. If it wasn't there, lab1.env.xml would get appended to the current OpenRAVE scene. Now let's query and display all the objects in the scene

bodies = orEnvGetBodies()
celldisp(bodies)

Every object has a unique id. Use this id for any function that performs an operation on the object. We can move the first joint of the robot 0.5 radians by

orRobotSetDOFValues(1,0.5,0)

or the first 10 joints:

orRobotSetDOFValues(1,0.5*ones(1,10),0:9)

Here '1' is the unique id of the robot.

If the third argument is not specified, then the default degrees of fredom (joints) used will be the active DOFs. The default active DOFs are all the joints of the robot. To make only the first 7 joints active type:

orRobotSetActiveDOFs(1,0:6,0)

To get the number of active dofs type:

orRobotGetActiveDOF(1)

The concept of active DOFs is very powerful. All planners search only in the active DOFs when planning. In this example, the robot is a combination of an arm and a hand. Most of the time, we would only want to plan and move the arm while ignoring the hand. To do this, set the active DOFs to the joint indices of the arm only before calling the planner. Active DOFs are not limited to just joints, they can also be the translation or rotation components of the robot itself. For example, to plan for the robot arm while it is moving on the xy plane type:

orRobotSetActiveDOFs(1,0:6,3)

To have it rotate around the z-axis at the same time, type:

orRobotSetActiveDOFs(1,0:6,11,[0 0 1])

Now,

orRobotGetActiveDOF(1)

returns 10: 7 for the arm, 2 for xy planar translation, and 1 for the rotation around z.

To query the transformations of all the links of the robot type:

L = orBodyGetLinks(1)

This will return a 12xN matrix for N links, where each column is a 3x4 transformation matrix (left 3x3 is a rotation, and last column is the translation component). For example, to extract the tranformation matrix of the second link, type:

T = reshape(L(:,2),[3 4])

To set the transformation of the base robot do

orBodySetTransform(1,[0 0 1],[0.707 0.707 0 0])

This will set the translation to (0,0,1). The third parameter is a rotation around the X axis by 90 degrees in quaternion form. It is also possible to directly input a 3x4 transformation matrix T by:

orBodySetTransform(1,reshape(T,[1 12]))

To add a cup on top of the table named 'table' type:

destTransform = orBodyGetTransform(orEnvGetBody('table'));
destPosition  = destTransform(10:12);
cupPosition   = destPosition + [0;0;0.1];
cupid         = orEnvCreateKinBody('MyCup','data/mug1.kinbody.xml');
orBodySetTransform(cupid, cupPosition,[0.707 0.707 0 0]);

Executing planners - A Grasping Example

In this part, we'll be loading the manipulation plugin and using its various features to plan in the lab1.env.xml workspace with the BarrettWAM. The manipulation plugin exposes a Manipulation ProblemInstance and the Inverse Kinematics functions for the Barrett WAM arm.

First load

orEnvLoadScene('data/lab1.env.xml', 1)
robotid = orEnvGetBody('BarrettWAM')
manipid = orEnvCreateProblem('BaseManipulation', 'BarrettWAM')

Each ProblemInstance supports a 'SendCommand' function that is used to receive commands from the script server and send back responses. Matlab can communicate with this function by the 'orProblemSendCommand' function.

To move the hand safely to a particular preshape do:

manips = orRobotGetManipulators(robotid);
orBodySetJointValues(robotid,[0 0 0 pi/2],manips{1}.handjoints);

Alternative: Can move the robot safely to a preshape using planners by:

orRobotSetActiveDOFs(robotid,manips{1}.armjoints);
handjoints = sprintf('%d ',manips{1}.handjoints);
orProblemSendCommand(['MoveUnsyncJoints handjoints 4 0 0 0 1.57 ' handjoints],manipid);
orBodySetJointValues(robotid,[0 0 0 pi/2],manips{1}.handjoints);

To plan for a configuration space goal for the arm joints of the robot, type:

orProblemSendCommand('MoveManipulator armvals -0.005617 1.07 0.233 2.096 -4.194 -0.235 1.302520',manipid)

Can squeeze the fingers of the hand by:

orProblemSendCommand('CloseFingers',manipid)

Grab the body by:

orProblemSendCommand('GrabBody name mug6',manipid)

Move to the table:

tableTrans = [  -0.035588  -0.995502  -0.087805  -0.466866
                -0.994116   0.026269   0.105090   0.869644
                -0.102311   0.091028  -0.990579   1.000000 ];
orProblemSendCommand(['MoveToHandPosition matrix ' sprintf('%f ',tableTrans(1:3,1:4))],manipid)

Move the hand down a little:

orProblemSendCommand(['MoveHandStraight direction 0 0 -1 maxdist 0.3 matrix ' sprintf('%f ',tableTrans(1:3,1:4))],manipid)

Finally, to release the fingers use the ReleaseFingers command. ReleaseFingers is a little more general than CloseFingers because the fingers to move and the direction of movement can be specified.

orRobotSetActiveDOFs(1,[7 8 9]); % want to move 3 joints
orProblemSendCommand('releasefingers target mug6',manipid);

Can now plan to original position:

orProblemSendCommand('MoveManipulator armvals 0 0 0 0 0 0 0',manipid);

Can also test if an IK solution exists by giving the transformation matrix T of the wrist:

s = orProblemSendCommand(['IKtest trans ', num2str(T(:,4)'), ' rot ', num2str(T(1:9))],manipid)
if( ~isempty(s) )
    orRobotSetDOFValues(1, sscanf(s, '%f'),0:6)
end

Logging

A scene can be easily saved by using the logging plugin. Once a scene is loaded and all the objects are in place, do the following commands:

logid = orEnvCreateProblem('logging')
orProblemSendCommand('savescene filename myscene.env.xml',logid)

Matlab/Octave Partial Reference

This is not a complete list and use this only to introduce yourself to the functions, the real reference and usages can be found in the help files of each function by typing help function_name.

• orBodyDestroy(bodyid)

Destroys a body of id bodyid. bodyid can also be a robot.

• success = orBodyEnable(bodyid, enable)

Enables or disables the body. If a body is disabled, 
collision detection and physics will will be turned off for it.

• aabb = orBodyGetAABB(bodyid)

Returns an axis-aligned boudning box of the body in world coordinates
aabb is a 3x2  vector where the first column is the position of the
box and the second is the extents.

• aabbs = orBodyGetAABBs(bodyid)

returns the axis-aligned boudning boxes of all the links of the body in world coordinates
aabbs is a 6xn vector where each column describes the box for all n links.
The first 3 values in each column describe the position of the aabb, and the next
3 values describe the extents (half width/length/height) on each of the axes.

• dof = orBodyGetDOF(robotid)

Returns the number of active joints of the body.

• values = orBodyGetDOFValues(robotid, indices)

Gets the body's dof values in a Nx1 vector where N is the DOF
bodyid - unique id of the robot
indices [optional]- The indices of the joints whose values should be returned.
                    If not specified, all joints are returned

• values = orBodyGetLinks(bodyid)

Returns the transformations of all the body's links in a 12 x L matrix. Where L
is the number of links and each column is a 3x4 transformation
(use T=reshape(., [3 4]) to recover).
T * [X;1] = Xnew

• orBodySetJointValues(bodyid, values, indices)

Set the raw joint values of a body. If bodyid is a robot, sets the robot's
joints ignoring its current active degrees of freedom. If a controller on
the robot is running, this function might not have any effect. Instead
use orRobotSetDOFValues
indices [optional] - array specifying the indices to control

• orBodySetTransform

orBodySetTransform(bodyid, translation, quaternion)
orBodySetTransform(bodyid, [quaternion translation])
orBodySetTransform(bodyid, transform matrix) (12x1, 1x12, or 3x4)
Set the affine transformation of the body. The transformation actually
describes the first link of the body. The rest of the links are derived by
the joint angles. A quaternion is related to axis and angle via: [cos(theta/2);sin(theta/2)*axis]

• [collision, colbody] = orEnvCheckCollision(bodyid,excludeid)

Check collision of the robot with the environment. collision is 1 if the robot
is colliding, colbodyid is the id of the object that body collided with

• orEnvClose(figureids)

closes the figures and plots figureids - array of ids returned from orEnvPlot or other plotting functions

• bodyid = orEnvCreateKinBody(name, xmlfile)
• plannerid = orEnvCreatePlanner(plannertype)
• orEnvCreateProblem

problemid = orEnvCreateProblem(problemname, args)
problemid = orEnvCreateProblem([problemname, args])

• problemid = orEnvCreateProblem(problemname, args, destroyduplicates)

Creates an instance of a problem and returns its id for future communicate with it
problemname - the problem name
args - a string of arguments to send to the problem's main function
destroyduplicates [optional] - if 1, will destroy any previous problems with the same problem name.
                              If 0, will not destroy anything.
                              The default value is 1. 

• robotid = orEnvCreateRobot(robotname, xmlfile, type)

Creates a robot of the given type. If type is not specified, creates a generic robot

• orEnvDestroyProblem(problemid)

Destroys problem instance whose id is problemid.

• bodies = orEnvGetBodies()

bodies is a cell array of all body objects in the scene
every cell contains a struct with the following parameters
id - bodyid
filename - filename used to initialize the body with
name - human robot name
type - xml type of body

• id = orEnvGetBody(bodyname)

returns the id of the body that corresponds to bodyname

• robots = orEnvGetRobots()

robots is a cell array of robots
every cell contains a struct with the following parameters
id - robotid
filename - filename used to initialize the robot with
name - human robot name
type - type of robot

• success = orEnvLoadPlugin(filename)

Loads a plugin.
filename - the relative path of the plugin to load. (*.so for linux, *.dll for windows)

• orEnvLoadScene(filename, [ClearScene])

Loads a new environment.
filename - The filename of the scene to load. If a relative file
           is specified, note that it is relative to the current direction
           of the OpenRAVE executable.
ClearScene - If 1, then clears the scene before loading. Else leaves the 
             scene alone and loads in addition to it.

• figureid = orEnvPlot(points,...)

plots points or lines in the openrave viewer
points - Nx3 vector of xyz positions
optional arguments include 'size', 'color', and 'line'
  color - Nx3 vector of RGB values between 0 and 1
  size - Nx1 vector of the sizes in pixels of each point/line
  line (or linestrip) - if specified, then openrave renders a line strip
  linelist - if specified, openrave renders a line for every two points
  trilist - if specified, openrave renders a triangle for every three
            vertices should be specified in counter-clockwise order
  sphere - if specified, openrave renders each point as a sphere
  transparency - [0,1], set transparency of plotted objects (0 is opaque)

• [collision, colinfo] = orEnvRayCollision(rays)

performs ray collision checks and returns the position and normals
where all the rays collide
rays - a 6xN matrix where the first 3
rows are the ray position and last 3 are the ray direction
collision - N dim vector that is 1 for colliding rays and 0
for non-colliding rays colinfo is a 6xN vector that describes 
where the ray hit and the normal to the surface of the hit point
where the first 3 columns are position and last 3 are normals
if bodyid is specified, only checks collisions with that body

• orEnvSetOptions('publishanytime 1')

Current options:
- simulation [start/stop] [time_step] - toggles the internal simulation loop, ie all the calls to SimulationStep. 
If time_step is specified, will set the simulation time step for all objects.
Note that this is not tied to real time at all, how fast the  simulation goes in reality depends on complexity
of the scene and the physics engine being used.
- physics engine_name - switches the physics engine to another one with id 'engine_name'
- gravity [x y z] - changes to gravity vector
- publishanytime [1/0] - switch between publishing the body transformations
         to the GUI anytime or only between stepsimulation and server  messsages.
         When publishing anytime, the GUI will reflect the body movements after every
         move. This is useful when visualizing internal C++ states. When off, the GUI
         will only reflect the state of robots after all calls to stepsimulation and
         server send messages have been done. The default is off.
- debug [debug level] - toggles debugging messages by RAVELOG.
                       0  - only RAVEPRINT statements show
                       1+ - RAVELOG statements with various debug levels show
- quit - closes the openrave instance

• [tripoints, triindices] = orEnvTriangulate(inclusive, ids)

Returns the triangulation of various objects in the scenes controlled by name and options
Arguments:
  inclusive - if 1, will only triangulate the bodies pointed to by ids
              if 0, will triangulate all objects except the bodies pointed to by ids
              default value is 0.
  ids (optional) - the ids to include or exclude in the triangulation
To triangulate everything, just do orEnvTriangulate(0,[]), or orEnvTriangulate()

Output:
  tripoints - 3xN matrix of 3D points
  tripoints - 3xK matrix of indices into tripoints for every triangle.

• success = orEnvWait(robotid, robot_timeout)

wait until all previously sent commands to matlab are finished.
Since problems are meant to last for a long time orEnvWait waits
until the problem's main function finishes.

robotid - optional argument. If a robot id is specified, will wait until
the robot finishes with its trajectory.

robot_timeout (s) - function will return with success set to 0 if robot
did not finish its commands by robot_timeout ms. If not specified, orEnvWait
will not return until robot completes.

• success = orPlannerInit(planner, robot, parameters)

Initialize a planner to plan for a robot and give some parameters

• trajectory = orPlannerPlan(planner)

Start planning. The planner returns a trajectory when successful (otherwise returns an empty matrix)
trajectory - (DOF+1)xN matrix where N is the number of points in the trajectory.
             The first row are the time values of each trajectory point.

• output = orProblemSendCommand(cmd, problemid, dosync)

Sends a command to the problem. The function doesn't return until
ProblemInstance::SendCommand returns.
cmd - the string command to send the problem
problemid [optional] - returned id of the problem, if not specified, then
                      command is sent to all problems
dosync [optional] - If 1, the SendCommand is called in the main thread, in sync
                      with the rest of the primitives. If 0, called in a different thread.
output - the concatenated output of all the problems that the command is sent to

• orRender(cmd)

Controls rendering properties. Cmd can be
start - starts the GUI to update the internal openrave state
stop - stops the GUI from updating the internal openrave state (can be used to speed up loading)

• success = orRobotControllerSend(robotid, controllercmd)

sends a command to the current controller the robot is connected to.
OpenRAVE sends directly to ControllerBase::SendCmd,
ControllerBase::SupportsCmd is also used to check for support.

success - 1 if command was accepted, 0 if not

• success = orRobotControllerSet(robotid, controllername, controllerargs)

Sets a new robot controller and destroys the old.
controllername - name used to query a controller
controllerargs [optional] - the arguments to ControllerBase::Init

• dof = orRobotGetActiveDOF(robotid)

returns the robot's active degrees of freedom used for planning (not necessary corresponding to joints).

• sensors = orRobotGetAttachedSensors(robotid)

sensors is a cell array describing the attached sensors of the robot
Each cell is a struct with fields:
  name - name of the attached sensor
  link - zero-based index of link sensor is attached to
  Trelative - 3x4 matrix of the relative transform of the camera with respect to the robot
  Tglobal - 3x4 matrix of the global transform of the sensor of the current robot
            Tglobal = Tlink * Trelative
  type - the xml id of the sensor that is attached

• values = orRobotGetDOFLimits(robotid)

Gets the robot's dof limits in a Nx2 vector where N is the DOF, the first column
is the low limit and the second column is the upper limit.

• values = orRobotGetDOFValues(robotid, indices)

Gets the robot's dof values in a Nx1 vector where N is the DOF
robotid - unique id of the robot
indices [optional]- The indices of the joints whose values should be returned.
                    If not specified, the active degreees of freedeom set by
                    orRobotSetActiveDOFs will be used.

• manipulators = orRobotGetManipulators(robotid)

manipulators is a cell array describes the manipulators of the robot
Each cell is a struct with fields
  baselink - zero-based index of base link manipulator is attached to
  eelink - zero-based index of link defining the end-effector
  Tgrasp - 3x4 matrix of the grasp frame relative to the end effector link,
           Tglobalgrasp = Tendeffector*Tgrasp
  joints - 1xK zero-based joint indices of the hand attached to the end effector
  armjoints - 1xN zero-based manipulator joint indices that have an
              effect on the end effector
  iksolvername - name of ik solver to use

• data = orRobotSensorGetData(robotid, sensorindex)

Gets the sensor data. The format returned is dependent on the type
of sensor. Look at the different data SensorData implementations in rave.h.
Although the data returned is not necessarily one of them.
options [optional] - options that specify what type of data to request (0 is default)
data.type - contains the id of the data type (see SensorBase::SensorType)
For laser data
 data.laserrange - 3xN array where each column is the direction * distance
 data.laserpos - 3xN array where each column is the corresponding origin of each range measurement
 data.laserint - 1xN optional laser intensity array
For image data
 data.KK - 3x3 intrinsic matrix
 data.T - 3x4 camera matrix (to project a point multiply by KK*inv(T))
 data.I - the rgb image size(I) = [height width 3]

• out = orRobotSensorSend(robotid, sensorindex, controllercmd)

sends a command to a sensor attached to the robot 
OpenRAVE sends directly to SensorBase::SendCmd,
SensorBase::SupportsCmd is used to check for command support.

robotid - unique id of the robot
sensorindex - zero-based index of sensor into robot's attached sensor array
out - the output of the command

• orRobotSetActiveDOfs(robotid, indices, affinedofs, rotationaxis)

robotid - unique id of the robot
indices - zero based indices of the robot joints to activate
affinedofs [optional] - is a mask of affine transformation for planning
      1 - X, 2 - Y, 4 - Z, 8 - RotationAxis (rotationaxis has to be avlid, 16 - full 3D rotation
rotationaxis [optional] - the rotation axis (if the RotationAxis bit is set in affinedofs)

• orRobotSetDofValues(robotid, values, indices)

Sets the DOF values of the robot
robotid - unique id of the robot
values - the joint values of the robot
indices [optional] - the indices of the dofs to set of the robot. 
                     If indices is not specified the active degrees of freedom
                     set by previous calls to orRobotSetActiveDOFs will be used.
                     Note that specifying indices will not change the active dofs of the robot.

• orRobotStartActiveTrajectory(robotid, jointvalues, timestamps, transformations)

Starts/Queues a robot trajectory of the robot where the size of
each trajectory point is the current active degrees of freedom
of the robot (others are held constants)
D is the number of active degrees of freedom.
N is the number of points of the trajectory
robotid - unique id of the robot
jointvalues - DxN matrix of the joint values of each point in the trajrectory.
timestamps [optional] - the time stamps in seconds of each trajectory point.
transformations [optional] - 12xN or 7xN matrix. The base link transformations of
                             each trajectory point.
                             If the column size is 12, then it is a 3x4 matrix
                             in column first order
                             If the column size is 7, then it is a quaterion and a translation.
                             If active degrees of freedom contains a affine transformation component
                             it is overwritten with the transformations matrices