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)
Using Link Statistics¶
"""Speed-up planning and increase precision by using link statistics
"""
from openravepy import *
import 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
robot.SetActiveManipulator('leftarm_torso')
goal = [ 0.24865706, 0.09362862, 0, 2.21558089, -1.00901245, -1.18879056, -0.74486442, 0]
# normal planning
manipprob = interfaces.BaseManipulation(robot) # create the interface for basic manipulation programs
starttime = time.time()
manipprob.MoveManipulator(goal=goal,execute=False)
raveLogInfo('non-linkstatistics planning time: %fs'%(time.time()-starttime))
# using link statistics
lmodel=databases.linkstatistics.LinkStatisticsModel(robot)
if not lmodel.load():
lmodel.autogenerate()
lmodel.setRobotResolutions(0.01) # set resolution given smallest object is 0.01m
lmodel.setRobotWeights() # set the weights for planning
starttime = time.time()
traj=manipprob.MoveManipulator(goal=goal,execute=False,outputtrajobj=True)
raveLogInfo('linkstatistics planning time: %fs'%(time.time()-starttime))
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?
- Search for information in the archives of the openrave-users mailing list, or post a question.
- If you notice errors , please open a ticket and let us know!