Short snippets of code to help get the feel for OpenRAVE functionality. Code can be directly executed inside the python interpreter.
"""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))
"""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()))
"""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
"""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))
"""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
"""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
"""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
"""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))
"""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)
"""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)
"""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)
"""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)
"""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)
"""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
"""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)
"""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)
"""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
"""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))
"""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)
"""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)
"""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)
"""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)