OpenRAVE Documentation

Quick examples in openravepy

Short snippets of code to help get the feel for OpenRAVE functionality. Code can be directly executed inside the python interpreter.

Simple Environment Loading

"""Loads up an environment, attaches a viewer, loads a scene, and requests information about the robot.
"""
from openravepy import *
env = Environment() # create openrave environment
env.SetViewer('qtcoin') # attach viewer (optional)
env.Load('data/lab1.env.xml') # load a simple scene
robot = env.GetRobots()[0] # get the first robot

with env: # lock the environment since robot will be used
    raveLogInfo("Robot "+robot.GetName()+" has "+repr(robot.GetDOF())+" joints with values:\n"+repr(robot.GetDOFValues()))
    robot.SetDOFValues([0.5],[0]) # set joint 0 to value 0.5
    T = robot.GetLinks()[1].GetTransform() # get the transform of link 1
    raveLogInfo("The transformation of link 1 is:\n"+repr(T))

Rotating Body

"""Rotates all bodies along world z-direction by 45 degrees:
"""
from openravepy import *
import numpy
env = Environment() # create openrave environment
env.SetViewer('qtcoin') # attach viewer (optional)
env.Load('data/lab1.env.xml') # load a simple scene

Tz = matrixFromAxisAngle([0,0,numpy.pi/4])
with env:
    for body in env.GetBodies():
       body.SetTransform(numpy.dot(Tz,body.GetTransform()))

Using a BiRRT Planner

"""Use a planner to get a collision free path to a configuration space goal.
"""
from openravepy import *
env = Environment() # create the environment
env.SetViewer('qtcoin') # start the viewer
env.Load('data/lab1.env.xml') # load a scene
robot = env.GetRobots()[0] # get the first robot
RaveSetDebugLevel(DebugLevel.Debug) # set output level to debug

manipprob = interfaces.BaseManipulation(robot) # create the interface for basic manipulation programs
manipprob.MoveManipulator(goal=[-0.75,1.24,-0.064,2.33,-1.16,-1.548,1.19]) # call motion planner with goal joint angles
robot.WaitForController(0) # wait

Inverse Kinematics: Transform6D

"""Shows how to get all 6D IK solutions.
"""
from openravepy import *
import numpy, time
env = Environment() # create the environment
env.SetViewer('qtcoin') # start the viewer
env.Load('data/pr2test1.env.xml') # load a scene
robot = env.GetRobots()[0] # get the first robot

manip = robot.SetActiveManipulator('leftarm_torso') # set the manipulator to leftarm
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
    ikmodel.autogenerate()

with env: # lock environment
    Tgoal = numpy.array([[0,-1,0,-0.21],[-1,0,0,0.04],[0,0,-1,0.92],[0,0,0,1]])
    sol = manip.FindIKSolution(Tgoal, IkFilterOptions.CheckEnvCollisions) # get collision-free solution
    with robot: # save robot state
        robot.SetDOFValues(sol,manip.GetArmIndices()) # set the current solution
        Tee = manip.GetEndEffectorTransform()
        env.UpdatePublishedBodies() # allow viewer to update new robot
        time.sleep(10)
    
    raveLogInfo('Tee is: '+repr(Tee))

Inverse Kinematics: Translation3D

"""Moves a robot in a random position, gets the end effector transform, and calls IK on it.
"""
from openravepy import *
import time
env = Environment() # create the environment
env.SetViewer('qtcoin') # start the viewer
env.Load('data/katanatable.env.xml') # load a scene
robot = env.GetRobots()[0] # get the first robot

manip = robot.GetActiveManipulator()
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.Translation3D)
if not ikmodel.load():
    ikmodel.autogenerate()

with robot: # lock environment and save robot state
    robot.SetDOFValues([2.58, 0.547, 1.5, -0.7],[0,1,2,3]) # set the first 4 dof values
    Tee = manip.GetEndEffectorTransform() # get end effector
    ikparam = IkParameterization(Tee[0:3,3],ikmodel.iktype) # build up the translation3d ik query
    sols = manip.FindIKSolutions(ikparam, IkFilterOptions.CheckEnvCollisions) # get all solutions

h = env.plot3(Tee[0:3,3],10) # plot one point
with robot: # save robot state
    raveLogInfo('%d solutions'%len(sols))
    for sol in sols: # go through every solution
        robot.SetDOFValues(sol,manip.GetArmIndices()) # set the current solution
        env.UpdatePublishedBodies() # allow viewer to update new robot
        time.sleep(10.0/len(sols))

raveLogInfo('restored dof values: '+repr(robot.GetDOFValues())) # robot state is restored to original

Plan to End Effector Position

"""Use a planner to get a collision free path to a workspace goal of the end effector.
"""
from openravepy import *
import numpy
env = Environment() # create the environment
env.SetViewer('qtcoin') # start the viewer
env.Load('data/pr2test1.env.xml') # load a scene
robot = env.GetRobots()[0] # get the first robot

robot.SetActiveManipulator('leftarm_torso') # set the manipulator to leftarm + torso
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
    ikmodel.autogenerate()

manipprob = interfaces.BaseManipulation(robot) # create the interface for basic manipulation programs
Tgoal = numpy.array([[0,-1,0,-0.21],[-1,0,0,0.04],[0,0,-1,0.92],[0,0,0,1]])
res = manipprob.MoveToHandPosition(matrices=[Tgoal],seedik=10) # call motion planner with goal joint angles
robot.WaitForController(0) # wait

Grabbing Object with Planner

"""Shows how to use a planner to close and open a gripper using planning.
"""
from openravepy import *
import numpy
env = Environment() # create openrave environment
env.SetViewer('qtcoin') # attach viewer (optional)
env.Load('data/lab1.env.xml') # load a simple scene

robot=env.GetRobots()[0]
manip = robot.GetActiveManipulator()
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
    ikmodel.autogenerate()

manipprob = interfaces.BaseManipulation(robot) # create the interface for basic manipulation programs
Tgoal = numpy.array([[0,-1,0,-0.23],[-1,0,0,-0.1446],[0,0,-1,0.85],[0,0,0,1]])
res = manipprob.MoveToHandPosition(matrices=[Tgoal],seedik=10) # call motion planner with goal joint angles
robot.WaitForController(0) # wait

taskprob = interfaces.TaskManipulation(robot) # create the interface for task manipulation programs
taskprob.CloseFingers() # close fingers until collision
robot.WaitForController(0) # wait
with env:
    robot.Grab(env.GetKinBody('mug4'))
    # move manipulator to all zeros, set jitter to 0.04 since cup is initially colliding with table
    manipprob.MoveManipulator(numpy.zeros(len(manip.GetArmIndices())),jitter=0.04)
robot.WaitForController(0) # wait

Custom IK Filters

"""Set a custom IK filter to abort computation after 100ms.
"""
from openravepy import *
import numpy, time
env=Environment()
env.Load('data/pr2test1.env.xml')

robot=env.GetRobots()[0]
manip = robot.SetActiveManipulator('leftarm_torso')
lower,upper = robot.GetDOFLimits(manip.GetArmIndices()) # get the limits of just the arm indices
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
    ikmodel.autogenerate()

maxtime = 0.1 # 100ms
for i in range(10):
    with env:
        robot.SetDOFValues(lower+numpy.random.rand(len(lower))*(upper-lower),manip.GetArmIndices()) # set a random values to just the arm
        incollision = not env.CheckCollision(robot) and not robot.CheckSelfCollision()
        starttime = time.time()
        def timeoutfilter(values, manip, ikparam):
            return IkReturnAction.Quit if time.time()-starttime > maxtime else IkReturnAction.Success
        
        handle=manip.GetIkSolver().RegisterCustomFilter(0,timeoutfilter)
        success = manip.FindIKSolution(manip.GetIkParameterization(IkParameterization.Type.Transform6D),IkFilterOptions.CheckEnvCollisions)
        raveLogInfo('in collision: %d, real success: %d, time passed: %f'%(incollision,success is not None,time.time()-starttime))

Sending Torques to a Physics Engine

"""Shows how to set a physics engine and send torque commands to the robot
"""
from openravepy import *
import numpy, time
env=Environment()
env.Load('data/lab1.env.xml')
env.SetViewer('qtcoin')
with env:
    # set a physics engine
    physics = RaveCreatePhysicsEngine(env,'ode')
    env.SetPhysicsEngine(physics)
    physics.SetGravity(numpy.array((0,0,-9.8)))
    
    robot = env.GetRobots()[0]
    robot.GetLinks()[0].SetStatic(True)
    env.StopSimulation()
    env.StartSimulation(timestep=0.001)

for itry in range(5):
    torques = 100*(numpy.random.rand(robot.GetDOF())-0.5)
    for i in range(100):
        # have to lock environment when calling robot methods
        with env:
            robot.SetDOFTorques(torques,True)
        time.sleep(0.01)

# reset the physics engine
env.SetPhysicsEngine(None)

Testing a Grasp

"""Loads the grasping model and moves the robot to the first grasp found
"""
from openravepy import *
import numpy, time
env=Environment()
env.Load('data/lab1.env.xml')
env.SetViewer('qtcoin')
robot = env.GetRobots()[0]
target = env.GetKinBody('mug1')
gmodel = databases.grasping.GraspingModel(robot,target)
if not gmodel.load():
    gmodel.autogenerate()

validgrasps, validindicees = gmodel.computeValidGrasps(returnnum=1)
gmodel.moveToPreshape(validgrasps[0])
Tgoal = gmodel.getGlobalGraspTransform(validgrasps[0],collisionfree=True)
basemanip = interfaces.BaseManipulation(robot)
basemanip.MoveToHandPosition(matrices=[Tgoal])
robot.WaitForController(0)
taskmanip = interfaces.TaskManipulation(robot)
taskmanip.CloseFingers()
robot.WaitForController(0)

Returning a Trajectory

"""Get a trajectory to a grasp before executing it.
"""
from openravepy import *
import numpy, time
env=Environment()
env.Load('data/lab1.env.xml')
env.SetViewer('qtcoin')
robot = env.GetRobots()[0]
target = env.GetKinBody('mug1')
gmodel = databases.grasping.GraspingModel(robot,target)
if not gmodel.load():
    gmodel.autogenerate()

validgrasps, validindicees = gmodel.computeValidGrasps(returnnum=1)
basemanip = interfaces.BaseManipulation(robot)
with robot:
    grasp = validgrasps[0]
    gmodel.setPreshape(grasp)
    T = gmodel.getGlobalGraspTransform(grasp,collisionfree=True)
    traj = basemanip.MoveToHandPosition(matrices=[T],execute=False,outputtrajobj=True)

raveLogInfo('traj has %d waypoints, last waypoint is: %s'%(traj.GetNumWaypoints(),repr(traj.GetWaypoint(-1))))
robot.GetController().SetPath(traj)
robot.WaitForController(0)

Saving Viewer Image

"""Save a 640x480 image from the viewer.
"""
from openravepy import *
import scipy
import time
env = Environment() # create openrave environment
env.SetViewer('qtcoin')
env.Load('data/lab1.env.xml') # load a simple scene
time.sleep(1) # wait for viewer to initialize

env.GetViewer().SendCommand('SetFiguresInCamera 1') # also shows the figures in the image
I = env.GetViewer().GetCameraImage(640,480,  env.GetViewer().GetCameraTransform(),[640,640,320,240])
scipy.misc.imsave('openrave.jpg',I)

Recording Videos

"""Start and stop recording videos using the Python API.
"""
from openravepy import *
import time

env = Environment() # create openrave environment
env.SetViewer('qtcoin')
env.Load('data/lab1.env.xml') # load a simple scene

recorder = RaveCreateModule(env,'viewerrecorder')
env.AddModule(recorder,'')
codecs = recorder.SendCommand('GetCodecs') # linux only
filename = 'openrave.mpg'
codec = 13 # mpeg4
recorder.SendCommand('Start 640 480 30 codec %d timing realtime filename %s\nviewer %s'%(codec,filename,env.GetViewer().GetName()))
time.sleep(5)
recorder.SendCommand('Stop') # stop the video
env.Remove(recorder) # remove the recorder

Parabolic Retiming

"""Do parabolic segment retiming to a goal position without checking collisions.
"""
from openravepy import *
import numpy
env = Environment() # create openrave environment
env.SetViewer('qtcoin')
env.Load('robots/barrettwam.robot.xml') # load a simple scene
robot=env.GetRobots()[0]
with env:  
    lower,upper = robot.GetActiveDOFLimits()
    goalvalues = numpy.random.rand(len(lower))*(upper-lower)+lower
    traj = RaveCreateTrajectory(env,'')
    traj.Init(robot.GetActiveConfigurationSpecification())
    traj.Insert(0,robot.GetActiveDOFValues())
    traj.Insert(1,goalvalues)
    planningutils.RetimeActiveDOFTrajectory(traj,robot,hastimestamps=False,maxvelmult=1,maxaccelmult=1,plannername='ParabolicTrajectoryRetimer')
    print 'duration',traj.GetDuration()

robot.GetController().SetPath(traj)
robot.WaitForController(0)

Scene Logging

"""Save the current scene in COLLADA format
"""
from openravepy import *
env = Environment() # create openrave environment
env.Load('data/lab1.env.xml') # load a simple scene
# save in a zip archive
env.Save('newlab.zae',Environment.SelectionOptions.Everything)

Plan Multiple Robots

"""Set multiple robots in one configuration to allow for simultaneously planning
"""
from openravepy import *
env = Environment() # create the environment
env.SetViewer('qtcoin') # start the viewer
with env:
    # init scene
    robot1 = env.ReadRobotURI('robots/barrettwam.robot.xml')
    env.Add(robot1,True)
    robot2 = env.ReadRobotURI('robots/barrettwam.robot.xml')
    env.Add(robot2,True)
    Trobot = robot2.GetTransform()
    Trobot[0,3] += 0.5
    robot2.SetTransform(Trobot)
    RaveSetDebugLevel(DebugLevel.Debug) # set output level to debug
    
    # create planner parmaeters
    params = Planner.PlannerParameters()
    params.SetConfigurationSpecification(env, robot1.GetActiveManipulator().GetArmConfigurationSpecification() + robot2.GetActiveManipulator().GetArmConfigurationSpecification())
    params.SetGoalConfig([  2.16339636e+00,  -3.67548731e-01,  -1.84983003e+00, 1.76388705e+00,  -1.27624984e-07,   7.65325147e-09, 0.00000000e+00,  -7.27862052e-01,  -6.52626197e-01, -8.10210670e-09,   1.34978628e+00,  -1.21644879e-08, 2.77047240e-08,   0.00000000e+00])
    
    # start planner
    traj = RaveCreateTrajectory(env,'')
    planner = RaveCreatePlanner(env,'birrt')
    planner.InitPlan(None,params)
    status = planner.PlanPath(traj)

# set new traj to robot controllers
robot1.GetController().SetPath(traj)
robot2.GetController().SetPath(traj)
robot1.WaitForController(0) # wait
robot2.WaitForController(0) # wait

Directly Launching Planners

"""Launch a planner directly by creating its interface and configuring the PlannerParameters structures.
"""
from openravepy import *
from numpy import pi
env = Environment() # create openrave environment
env.SetViewer('qtcoin')
env.Load('data/lab1.env.xml')
robot = env.GetRobots()[0]

robot.SetActiveDOFs(range(4)) # set joints the first 4 dofs
params = Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetGoalConfig([0,pi/2,pi/2,pi/2]) # set goal to all ones
# forces parabolic planning with 40 iterations
params.SetExtraParameters("""<_postprocessing planner="parabolicsmoother">
    <_nmaxiterations>40</_nmaxiterations>
</_postprocessing>""")

planner=RaveCreatePlanner(env,'birrt')
planner.InitPlan(robot, params)

traj = RaveCreateTrajectory(env,'')
planner.PlanPath(traj)

for i in range(traj.GetNumWaypoints()):
    # get the waypoint values, this holds velocites, time stamps, etc
    data=traj.GetWaypoint(i)
    # extract the robot joint values only
    dofvalues = traj.GetConfigurationSpecification().ExtractJointValues(data,robot,robot.GetActiveDOFIndices())
    raveLogInfo('waypint %d is %s'%(i,dofvalues))

Sampling Trajectory

"""Show how to sample the trajectory objects and move the robot
"""
from openravepy import *
from numpy import arange
import time
env = Environment() # create the environment
env.SetViewer('qtcoin') # start the viewer
env.Load('data/lab1.env.xml') # load a scene
robot = env.GetRobots()[0] # get the first robot
manipprob = interfaces.BaseManipulation(robot) # create the interface for basic manipulation programs
traj=manipprob.MoveManipulator(goal=[-0.75,1.24,-0.064,2.33,-1.16,-1.548,1.19],outputtrajobj=True,execute=False) # call motion planner with goal joint angles
spec=traj.GetConfigurationSpecification() # get the configuration specification of the trajrectory
for i in range(5):
    starttime = time.time()
    while time.time()-starttime < traj.GetDuration():
        curtime = time.time()-starttime
        with env: # have to lock environment since accessing robot
            trajdata=traj.Sample(curtime)
            values=spec.ExtractJointValues(trajdata,robot,range(robot.GetDOF()),0)
            robot.SetDOFValues(values)
        time.sleep(0.01)

Creating/Updating a Body

"""Creates a box and then update the box's geometry dynamically.
"""
from openravepy import *
import numpy, time
env = Environment() # create openrave environment
env.SetViewer('qtcoin') # attach viewer (optional)
with env:
    body = RaveCreateKinBody(env,'')
    body.SetName('testbody')
    body.InitFromBoxes(numpy.array([[0,0,0,0.1,0.2,0.3]]),True) # set geometry as one box of extents 0.1, 0.2, 0.3
    env.Add(body,True)

time.sleep(2) # sleep 2 seconds
with env:
    env.Remove(body)
    body.InitFromBoxes(numpy.array([[-0.4,0,0,0.1,0.2,0.3],[0.4,0,0,0.1,0.2,0.9]]),True) # set geometry as two boxes
    env.Add(body,True)

time.sleep(2) # sleep 2 seconds

# create from soup of cylinders
with env:
    infocylinder = KinBody.GeometryInfo()
    infocylinder._type = GeometryType.Cylinder
    infocylinder._t[0,3] = 0.1
    infocylinder._vGeomData = [0.1,0.4]
    infocylinder._bVisible = True
    infocylinder._fTransparency = 0.5
    infocylinder._vDiffuseColor = [1,0,0]
    infocylinder2 = KinBody.GeometryInfo()
    infocylinder2._type = GeometryType.Cylinder
    infocylinder2._t[0,3] = -0.1
    infocylinder2._vGeomData = [0.2,0.4]
    infocylinder2._bVisible = True
    infocylinder2._fTransparency = 0.5
    infocylinder2._vDiffuseColor = [0.5,0.5,0]
    k3 = RaveCreateKinBody(env,'')
    k3.InitFromGeometries([infocylinder,infocylinder2])
    k3.SetName('tempcylinder')
    env.Add(k3,True)

Creating Custom Kinematics Body

"""Creates a custom kinematics body with two links and one joint
"""
from openravepy import *
import numpy, time
env = Environment() # create openrave environment
env.SetViewer('qtcoin') # attach viewer (optional)
with env:
    # geometries
    infobox0 = KinBody.Link.GeometryInfo()
    infobox0._type = GeometryType.Box
    infobox0._t[0,3] = 0
    infobox0._vGeomData = [0.1,0.2,0.3]
    infobox0._vDiffuseColor = [1,0,0]
    infobox1 = KinBody.Link.GeometryInfo()
    infobox1._type = GeometryType.Box
    infobox1._t[0,3] = 0.1
    infobox1._vGeomData = [0.3,0.05,0.05]
    infobox1._vDiffuseColor = [0,1,0]
    infobox2 = KinBody.Link.GeometryInfo()
    infobox2._type = GeometryType.Box
    infobox2._t[0,3] = 0
    infobox2._vGeomData = [0.1,0.2,0.3]
    infobox2._vDiffuseColor = [0,0,1]
    # links
    link0 = KinBody.LinkInfo()
    link0._vgeometryinfos = [infobox0, infobox1]
    link0._name = 'link0'
    link0._mapFloatParameters = {'param0':[1,2.3]}
    link0._mapIntParameters = {'param0':[4,5.6]}
    link1 = KinBody.LinkInfo()
    link1._vgeometryinfos = [infobox2]
    link1._name = 'link1'
    link1._mapFloatParameters = {'param0':[1,2.3]}
    link1._mapIntParameters = {'param0':[4,5.6]}
    link1._t[0,3] = 0.5
    # joints
    joint0 = KinBody.JointInfo()
    joint0._name = 'j0'
    joint0._linkname0 = 'link0'
    joint0._linkname1 = 'link1'
    joint0._type = KinBody.JointType.Hinge
    joint0._vlowerlimit = [-0.5]
    joint0._vupperlimit = [1.0]
    joint0._vaxes = [[0,0,1]]
    # instantiate
    body = RaveCreateKinBody(env,'')
    success=body.Init([link0,link1],[joint0])
    body.SetName('temp')
    env.Add(body)

Solve Dual-Arm Inverse Kinematics

"""Shows how to solve IK for two robot arms  simultaneously
"""
from openravepy import *
from numpy import *
from itertools import izip
import time
env = Environment() # create the environment
env.SetViewer('qtcoin') # start the viewer
env.Load('data/dualarmmanipulation.env.xml')

# get the first robot
robot=env.GetRobots()[0]

# create the dual-arm ik solver
dualsolver = misc.MultiManipIKSolver([robot.GetManipulator('leftarm'), robot.GetManipulator('rightarm')])

body=env.GetKinBody('Object1')
for itry in range(5):
    with env:
        Tbody=body.GetTransform()
        ab = body.ComputeAABB().extents()
        halfwidth= ab[1] #this is y
        #.04 is just half the thickness of the EEF
        TRightGrasp= dot(Tbody,array([[0, 0, -1, 0],[1, 0, 0, (halfwidth+.04)],[0, -1, 0, 0 ],[0, 0, 0, 1]]))
        # to determine the grasp for the eef given the transform of the object
        TLeftGrasp= dot(Tbody,array([[0, 0, -1, 0],[-1, 0, 0, -(halfwidth+.04)],[0, 1, 0, 0],[0, 0, 0, 1]]))
        solutions = dualsolver.findMultiIKSolution(Tgrasps=[TLeftGrasp,TRightGrasp],filteroptions=IkFilterOptions.CheckEnvCollisions)
        if solutions is not None:
            for manip,solution in izip(dualsolver.manips, solutions):
                robot.SetDOFValues(solution,manip.GetArmIndices())
            
    time.sleep(0.2)

Questions/Feedback

Having problems with OpenRAVE?