def AttachController(self, name, args, dof_indices, affine_dofs,
simulated):
"""
Create and attach a controller to a subset of this robot's DOFs. If
simulated is False, a controller is created using 'args' and is attached
to the multicontroller. In simulation mode an IdealController is
created instead. Regardless of the simulation mode, the multicontroller
must be finalized before use.
@param name user-readable name used to identify this controller
@param args real controller arguments
@param dof_indices controlled joint DOFs
@param affine_dofs controleld affine DOFs
@param simulated simulation mode
@returns created controller
"""
if simulated:
args = 'IdealController'
delegate_controller = openravepy.RaveCreateController(
self.GetEnv(), args)
if delegate_controller is None:
type_name = args.split()[0]
message = 'Creating controller {0:s} of type {1:s} failed.'.format(
name, type_name)
raise openravepy.openrave_exception(message)
self.multicontroller.AttachController(delegate_controller, dof_indices,
affine_dofs)
return delegate_controller
def __init__(self, env, manip):
self.name = 'herb'
self.robot = env.ReadRobotXMLFile(
'models/robots/herb2_padded.robot.xml')
env.Add(self.robot)
right_relaxed = [5.65, -1.76, -0.26, 1.96, -1.15, 0.87, -1.43]
left_relaxed = [0.64, -1.76, 0.26, 1.96, 1.16, 0.87, 1.43]
right_manip = self.robot.GetManipulator('right_wam')
self.robot.SetActiveDOFs(right_manip.GetArmIndices())
self.robot.SetActiveDOFValues(right_relaxed)
left_manip = self.robot.GetManipulator('left_wam')
self.robot.SetActiveDOFs(left_manip.GetArmIndices())
self.robot.SetActiveDOFValues(left_relaxed)
if manip == 'right':
self.manip = right_manip
self.robot.SetActiveManipulator('right_wam')
else:
self.manip = left_manip
self.robot.SetActiveManipulator('left_wam')
self.robot.SetActiveDOFs(self.manip.GetArmIndices())
self.robot.controller = openravepy.RaveCreateController(
self.robot.GetEnv(), 'IdealController')
def GetRealStartPoses(self, youbot_names, objects):
# Register with tfplugin
idealcontrollers = {}
if self.tfplugin is None:
self.tfplugin = TfPlugin(self.env, 'map')
for youbot_name in youbot_names:
idealcontrollers[youbot_name] = self.youbots[
youbot_name].GetController(
) # plug in the simulation controller
probotcontroller = orpy.RaveCreateController(
self.env, 'youbotcontroller') # plug in the real controller
self.youbots[youbot_name].SetController(probotcontroller)
self.tfplugin.RegisterRobotHand(or_body=self.youbots[youbot_name],
tf_id=youbot_name + '_arm')
for oname in objects:
self.tfplugin.RegisterBody(or_body=self.env.GetKinBody(oname),
tf_id=oname)
#raw_input('Hit enter if start poses are updated from vicon.')
# Unregister with tfplugin
time.sleep(3.0)
for youbot_name in youbot_names:
self.tfplugin.UnregisterBody(or_body=self.youbots[youbot_name])
# set back to idealcontroller
self.youbots[youbot_name].SetController(
idealcontrollers[youbot_name])
for oname in objects:
self.tfplugin.UnregisterBody(or_body=self.env.GetKinBody(oname))
def load_ghost(env, robotname, prefix="ref_", color=[.8, .8, .4]):
""" Create a ghost robot to overlay with an existing robot in the world to show an alternate state."""
ref_robot = env.ReadRobotURI(robotname)
ref_robot.SetName(prefix + ref_robot.GetName())
ref_robot.Enable(False)
env.Add(ref_robot)
ref_robot.SetController(_rave.RaveCreateController(env, 'mimiccontroller'))
set_robot_color(ref_robot, color, color, trans=.5)
return ref_robot
def load_scene(env, scenename=None, stop=False, physics=True):
""" Load a robot model into the given environment, configuring a
trajectorycontroller and a reference robot to show desired movements vs. actual
pose. The returned tuple contains:
robot: handle to the created robot
controller: either trajectorycontroller or idealcontroller depending on physics
name-to-joint-index converter
ref_robot: handle to visiualization "ghost" robot
recorder: video recorder python class for quick video dumps
"""
# Set the robot controller and start the simulation
recorder = viewerrecorder(env)
#Default to "sim-timed video" i.e. plays back much faster
recorder.videoparams[0:2] = [1024, 768]
recorder.realtime = False
with env:
if stop:
env.StopSimulation()
if type(scenename) is list:
for n in scenename:
loaded = env.Load(n)
elif type(scenename) is str:
loaded = env.Load(scenename)
time.sleep(1)
#Explicitly disable physics if option is selected
with env:
if not physics:
env.SetPhysicsEngine(
rave.RaveCreatePhysicsEngine(env, 'GenericPhysicsEngine'))
robot = env.GetRobots()[0]
pose = ones(robot.GetDOF()) * .6
robot.SetDOFValues(pose)
collisionChecker = rave.RaveCreateCollisionChecker(env, 'pqp')
if collisionChecker == None:
collisionChecker = rave.RaveCreateCollisionChecker(env, 'ode')
print 'Note: Using ODE collision checker since PQP is not available'
env.SetCollisionChecker(collisionChecker)
if env.GetPhysicsEngine().GetXMLId(
) != 'GenericPhysicsEngine' and physics:
controller = rave.RaveCreateController(env, 'servocontroller')
robot.SetController(controller)
controller.SendCommand('set gains 100 0 0')
controller.SetDesired(pose / pose * pi / 4 * 1.1)
time.sleep(.5)
ind = makeNameToIndexConverter(robot)
return (robot, controller, ind, recorder)
def __init__(self, env):
self.name = 'simple'
self.robot = env.ReadRobotXMLFile('models/robots/pr2-beta-sim.robot.xml')
env.Add(self.robot)
right_relaxed = [ 0., 0., 0., -2.3, 0., 0., 0.]
left_relaxed = [ 0., 0., 0., -2.3, 0., 0., 0.]
right_manip = self.robot.GetManipulator('rightarm')
self.robot.SetActiveDOFs(right_manip.GetArmIndices())
self.robot.SetActiveDOFValues(right_relaxed)
left_manip = self.robot.GetManipulator('leftarm')
self.robot.SetActiveDOFs(left_manip.GetArmIndices())
self.robot.SetActiveDOFValues(left_relaxed)
self.robot.controller = openravepy.RaveCreateController(self.robot.GetEnv(), 'IdealController')
def PlayTrajWithPhysics(robot,
traj,
autoinit=False,
waitdone=True,
resetafter=False):
#TODO eliminate adding controller here, potential issues switching back
#Lock the environment, halt simulation in preparation
env = robot.GetEnv()
with env:
if robot.GetController().GetXMLId() != 'trajectorycontroller':
ctrl = _rave.RaveCreateController(env, 'trajectorycontroller')
robot.SetController(ctrl)
else:
ctrl = robot.GetController()
ctrl.SetPath(traj)
#Resets initial pose, so now we need to set any DOF not in trajectory
if autoinit:
setInitialPose(robot)
ctrl.SetDesired(robot.GetDOFValues())
#Eventuall make this variable? might not matter if .0005 is good
_time.sleep(1)
ctrl.SendCommand('start')
if waitdone:
t0 = _time.time()
while not (ctrl.IsDone()):
print "Real time {}, sim time {}".format(_time.time() - t0,
ctrl.GetTime())
#Only approximate time here
_time.sleep(.1)
handle = plot_projected_com(robot)
if resetafter:
with env:
ctrl.Reset()
return ctrl
right_relaxed = [5.65, -1.76, -0.26, 1.96, -1.15, 0.87, -1.43]
left_relaxed = [0.64, -1.76, 0.26, 1.96, 1.16, 0.87, 1.43]
right_manip = robot.GetManipulator('right_wam')
robot.SetActiveDOFs(right_manip.GetArmIndices())
robot.SetActiveDOFValues(right_relaxed)
left_manip = robot.GetManipulator('left_wam')
robot.SetActiveDOFs(left_manip.GetArmIndices())
robot.SetActiveDOFValues(left_relaxed)
if args.manip == 'right':
robot.SetActiveManipulator('right_wam')
else:
robot.SetActiveManipulator('left_wam')
robot.controller = openravepy.RaveCreateController(robot.GetEnv(),
'IdealController')
robot.ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot, iktype=openravepy.IkParameterization.Type.Transform6D)
if not robot.ikmodel.load():
robot.ikmodel.autogenerate()
robot.irmodel = openravepy.databases.inversereachability.InverseReachabilityModel(
robot)
if not robot.irmodel.load():
print "irmodel didnt' load. "
robot.irmodel.autogenerate()
# add a table and move the robot into place
table = env.ReadKinBodyXMLFile('models/objects/table.kinbody.xml')
env.Add(table)
def __init__(self,
sim=False,
viewer=False,
env_xml=None,
youbot_names=[],
registered_objects=[],
use_tfplugin=True,
youbot_model='kuka-youbot.robot.xml'):
self.youbotpydir = os.popen('rospack find youbotpy').read()
print self.youbotpydir
#RaveSetDebugLevel(0) # suppresses printing of non fatal errors
env = orpy.Environment()
if viewer:
env.SetViewer('qtcoin')
if env_xml:
env.Load(env_xml)
youbots = {}
youbot_hands = {}
for youbot_name in youbot_names:
specific_filename = self.youbotpydir[:-1] + '/../models/robots/' + youbot_model[
0:-10] + '-' + youbot_name + youbot_model[-10:]
if os.path.isfile(specific_filename):
print('loading robot-specific filename: %d', specific_filename)
youbot = env.ReadRobotURI(specific_filename)
else:
youbot = env.ReadRobotURI(self.youbotpydir[:-1] +
'/../models/robots/' + youbot_model)
#youbot = env.ReadRobotURI(self.youbotpydir[:-1] + '/../models/robots/kuka-youbot-hires.robot.xml')
youbot.SetName(youbot_name)
env.Add(youbot, True)
youbots[youbot_name] = youbot
self.basemanips = {}
for name in youbot_names:
self.basemanips[name] = orpy.interfaces.BaseManipulation(
youbots[name], plannername='BiRRT')
tfplugin = None
if not sim:
if use_tfplugin:
tfplugin = TfPlugin(env, 'map')
for youbot_name in youbot_names:
probotcontroller = orpy.RaveCreateController(
env, 'youbotcontroller')
youbots[youbot_name].SetController(probotcontroller)
#tfplugin.RegisterBody(or_body=youbots[youbot_name],tf_id=youbot_name)
if use_tfplugin:
openrave_frame_in_tf_frame = np.eye(4)
openrave_frame_in_tf_frame[
2,
3] = -0.12 # difference betweeen vicon origin and openrave kinbody origin.
tfplugin.RegisterBody(
or_body=youbots[youbot_name],
tf_id=youbot_name,
openrave_frame_in_tf_frame=openrave_frame_in_tf_frame)
#tfplugin.RegisterBody(or_body=youbots[youbot_name],tf_id=youbot_name,openrave_frame_in_tf_frame=openrave_frame_in_tf_frame,planar_tracking=True)
#tfplugin.RegisterRobotHand(or_body=youbots[youbot_name],tf_id=youbot_name+'_arm')
# Also add youbot hands that are tracked separately.
detached_hands = True
if detached_hands:
youbot_hand = env.ReadKinBodyURI(
self.youbotpydir[:-1] +
'/../models/objects/youbothand.kinbody.xml')
youbot_hand.SetName(youbot_name + '_arm')
env.Add(youbot_hand, True)
youbot_hand.Enable(False)
openrave_frame_in_tf_frame = np.eye(4)
openrave_frame_in_tf_frame[
2,
3] = 0.013 # difference betweeen vicon origin and openrave kinbody origin.
tfplugin.RegisterBody(
or_body=youbot_hand,
tf_id=youbot_hand.GetName(),
openrave_frame_in_tf_frame=openrave_frame_in_tf_frame)
youbot_hands[youbot_name] = youbot_hand
if use_tfplugin:
if not (registered_objects is None):
for oname in registered_objects:
tfplugin.RegisterBody(or_body=env.GetKinBody(oname),
tf_id=oname)
if sim and viewer:
# nudge robots a little, b/c clicking on trimeshes overlapping perfectly causes openrave to crash.
for name in youbots:
randpose = np.eye(4)
randpose[:2, 3] = np.array([random.random(), random.random()])
youbots[name].SetTransform(randpose)
self.env = env
self.tfplugin = tfplugin
self.sim = sim
self.youbots = youbots
self.youbot_hands = youbot_hands
self.arm_joint_offsets = np.array([2.96, 1.70, -0.55, 0.80, 4.58])
import openhubo as oh
import time
from numpy import pi, sqrt
(env, options) = oh.setup('qtcoin', True)
env.SetDebugLevel(4)
#Note that the load function now directly parses the option structure
if options.physics is None:
options.physics = True
options.scenefile = None
[robot, ctrl, ind, ref, recorder] = oh.load_scene(env, options)
#Change the pose to lift the elbows and send
pose = oh.Pose(robot, ctrl)
pose['REP'] = -pi / 2
pose['LEP'] = -pi / 2
pose.send()
physics = env.GetPhysicsEngine()
physics.SetGravity([0, 0, 0])
ctrl2 = _rave.RaveCreateController(env, "idealcontroller")
robot.SetController(ctrl2)
env.StartSimulation(timestep=oh.TIMESTEP)
initialPose = robot.GetDOFValues()
time.sleep(5)
finalPose = robot.GetDOFValues()
err = sqrt(sum(pow(initialPose - finalPose, 2)))
def openrave_init(self):
self.env = openravepy.Environment()
self.env.SetViewer('qtcoin')
self.env.GetViewer().SetName('HPN Viewer')
#self.env.Load('models/%s.env.xml' %PACKAGE_NAME)
# time.sleep(3) # wait for viewer to initialize. May be helpful to uncomment
self.name = 'herb'
self.robot = self.env.ReadRobotXMLFile('models/robots/herb2_padded.robot.xml')
self.env.Add(self.robot)
right_relaxed = [ 5.65, -1.76, -0.26, 1.96, -1.15 , 0.87, -1.43 ]
left_relaxed = [ 0.64, -1.76, 0.26, 1.96, 1.16, 0.87, 1.43 ]
right_manip = self.robot.GetManipulator('right_wam')
self.robot.SetActiveDOFs(right_manip.GetArmIndices())
self.robot.SetActiveDOFValues(right_relaxed)
left_manip = self.robot.GetManipulator('left_wam')
self.robot.SetActiveDOFs(left_manip.GetArmIndices())
self.robot.SetActiveDOFValues(left_relaxed)
self.manip = right_manip
self.robot.SetActiveManipulator('right_wam')
self.end_effector = self.manip.GetEndEffector()
self.robot.SetActiveDOFs(self.manip.GetArmIndices())
self.robot.controller = openravepy.RaveCreateController(self.robot.GetEnv(), 'IdealController')
# add a table
self.table = self.robot.GetEnv().ReadKinBodyXMLFile('models/data/furniture/table.kinbody.xml')
self.robot.GetEnv().Add(self.table)
table_pose = np.array([[ 0, 0, -1, 0.6],
[-1, 0, 0, 0],
[ 0, 1, 0, 0],
[ 0, 0, 0, 1]])
self.table.SetTransform(table_pose)
# set the camera
camera_pose = np.array([[ 0.3259757 , 0.31990565, -0.88960678, 2.84039211],
[ 0.94516159, -0.0901412 , 0.31391738, -0.87847549],
[ 0.02023372, -0.9431516 , -0.33174637, 1.61502194],
[ 0. , 0. , 0. , 1. ]])
self.robot.GetEnv().GetViewer().SetCamera(camera_pose)
#add kinbodies
#glass
self.target_kinbody1 = self.env.ReadKinBodyURI('models/data/objects/plastic_glass.kinbody.xml')
self.robot.GetEnv().Add(self.target_kinbody1)
glass_pose = np.array([[ 0, 0, 0, 0.7],
[-1, 0, 1, -0.5],
[ 0, 1, 0, 0.7165],
[ 0, 0, 0, 1]])
self.target_kinbody1.SetTransform(glass_pose)
#bowl
self.target_kinbody2 = self.env.ReadKinBodyURI('models/data/objects/plastic_bowl.kinbody.xml')
self.robot.GetEnv().Add(self.target_kinbody2)
bowl_pose = np.array([[ 0, 0, 0, 0.5],
[-1, 0, 1, -0.4],
[ 0, 1, 0, 0.7165],
[ 0, 0, 0, 1]])
self.target_kinbody2.SetTransform(bowl_pose)
#plate
self.target_kinbody3 = self.env.ReadKinBodyURI('models/data/objects/plastic_plate.kinbody.xml')
self.robot.GetEnv().Add(self.target_kinbody3)
plate_pose = np.array([[ 0, 0, 0, 0.7],
[-1, 0, 1, -0.3],
[ 0, 1, 0, 0.7165],
[ 0, 0, 0, 1]])
self.target_kinbody3.SetTransform(plate_pose)
#add tray
self.target_tray = self.env.ReadKinBodyURI('models/data/objects/wicker_tray.kinbody.xml')
self.robot.GetEnv().Add(self.target_tray)
tray_pose = np.array([[ 0, 1, 0, 0.6],
[1, 0, 0, 0.5],
[ 0, 0, 1, 0.7165],
[ 0, 0, 0, 1]])
self.target_tray.SetTransform(tray_pose)
'{}/openrave-{}.{}'.format(libpath, ormaj, ormin))
path_append(os.environ, 'OPENRAVE_DATA',
'{}/ordata'.format(roslib.packages.get_pkg_dir('ada_description')))
# load an openrave environment
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
atexit.register(openravepy.RaveDestroy)
e = openravepy.Environment()
atexit.register(e.Destroy)
e.SetViewer('qtcoin')
r = e.ReadRobotXMLFile('robots/mico-modified.robot.xml')
e.Add(r)
c = openravepy.RaveCreateController(e, 'roscontroller openrave / 1')
r.SetController(c, range(8), 0)
#r.SetActiveDOFs(range(6))
time.sleep(1)
# make a sweet trajectory
raw_input('Press [Enter] to send trajectory ...')
t = openravepy.RaveCreateTrajectory(e, '')
t.Init(r.GetActiveConfigurationSpecification())
print(t.GetConfigurationSpecification())
t.Insert(0, r.GetActiveDOFValues())
if r.GetActiveDOFValues()[5] < 0.5:
t.Insert(1, [0.0, -1.57, 1.57, 0.0, 0.0, 1.0, 0.0, 0.0])
else:
#!/usr/bin/python
# -*- coding: utf-8 -*-
import openravepy
import os
env = openravepy.Environment()
env.SetViewer('qtcoin')
env.Load('tutorial/env1.xml')
#robot= env.GetRobots()[0]
robot= env.GetRobot('SimpleBot')
#openravepy.RaveLoadPlugin(os.getcwd()+'/libtestcontrollers.so')
openravepy.RaveLoadPlugin(os.getcwd()+'/test_ctrl/build/libtestcontrollers.so')
controller= openravepy.RaveCreateController(env,'TestController')
robot.SetController(controller,range(robot.GetDOF()),controltransform=1)
raw_input("Press Enter to start...")
import time
env.UpdatePublishedBodies()
time.sleep(0.1)
nav = openravepy.examples.simplenavigation.SimpleNavigationPlanning(robot)
nav.performNavigationPlanning()
raw_input("Press Enter to exit...")
env.Destroy()
def load_scene_from_options(env, options):
"""Load a scene for openhubo based on the options structure (see setup function for detailed options)."""
if hasattr(options, 'physics') and options.physics is False:
#Kludge since we won't be not using ODE for a while...
physics = False
elif hasattr(options, 'physics') and options.physics:
physics = True
elif options._physics == 'ode':
physics = True
else:
physics = False
vidrecorder = _recorder(env, filename=options.recordfile)
vidrecorder.videoparams[0:2] = [1024, 768]
vidrecorder.realtime = False
with env:
if options.stop or physics:
#KLUDGE: need to do this for stability reasons, not sure why, probably can be fixed another way
print "Stopping OpenRAVE simulation to load models and configure..."
env.StopSimulation()
if type(options.scenefile) is list:
for n in options.scenefile:
env.Load(n)
elif type(options.scenefile) is str:
env.Load(options.scenefile)
#This method ensures that the URI is preserved in the robot
if options.robotfile is not None:
robot = env.ReadRobotURI(options.robotfile)
env.Add(robot)
else:
robot = env.GetRobots()[0]
_rave.raveLogWarn("Assuming robot is {} (id=0)...".format(
robot.GetName()))
robot.SetDOFValues(zeros(robot.GetDOF()))
#Legacy command mode
if options.physicsfile == True:
options.physicsfile = 'physics.xml'
with env:
if physics and not check_physics(env):
_rave.raveLogInfo('Loading physics parameters from "{}"'.format(
options.physicsfile))
env.Load(options.physicsfile)
elif not physics:
env.SetPhysicsEngine(
_rave.RaveCreatePhysicsEngine(env, 'GenericPhysicsEngine'))
else:
#TODO: find a more efficient way to avoid double creation?
_rave.raveLogInfo(
"Physics engine already configured, overwriting...")
env.Load(options.physicsfile)
if check_physics(env):
_rave.raveLogInfo('Creating controller for physics simulation')
controller = _rave.RaveCreateController(env,
'trajectorycontroller')
robot.SetController(controller)
controller.SendCommand('set gains 150 0 .9 ')
controller.SendCommand('set radians ')
else:
#Just load ideal controller if physics engine is not present
_rave.raveLogInfo(
'Physics engine not loaded, using idealcontroller...')
controller = _rave.RaveCreateController(env, 'idealcontroller')
robot.SetController(controller)
if options.ghost and options.robotfile:
ghost_robot = load_ghost(env, options.robotfile, prefix="ghost_")
if check_physics(env):
controller.SendCommand("set visrobot " + ghost_robot.GetName())
else:
ghost_robot = None
collisionChecker = _rave.RaveCreateCollisionChecker(env, 'pqp')
if collisionChecker == None:
collisionChecker = _rave.RaveCreateCollisionChecker(env, 'ode')
_rave.raveLogWarn(
'Using ODE collision checker since PQP is not available...')
env.SetCollisionChecker(collisionChecker)
ind = make_name_to_index_converter(robot)
if options.atheight is not None:
align_robot(robot, options.atheight)
if ghost_robot:
align_robot(ghost_robot, options.atheight)
#FIXME: better way to do this?
global TIMESTEP
TIMESTEP = get_timestep(env)
return (robot, controller, ind, ghost_robot, vidrecorder)
