def __init__(self, env_file=None, view=False):
if env_file is None:
env_file = rospack.get_path('pr2_utils') + '/robots/my-pr2-beta-sim.robot.xml'
self.joint_state_msg = None
self.joint_state_sub = rospy.Subscriber('/joint_states', sm.JointState, self._joint_state_callback)
self.env = rave.Environment()
self.env.StopSimulation()
self.env.Load(env_file)
self.added_kinbody_names = list()
self.handles = list()
self.view = view
if view:
self.env.SetViewer('qtcoin')
self.robot = self.env.GetRobots()[0]
self.larm = self.robot.GetManipulator('leftarm')
self.rarm = self.robot.GetManipulator('rightarm')
for arm in [self.larm, self.rarm]:
self.robot.SetActiveManipulator(arm)
ikmodel = rave.databases.inversekinematics.InverseKinematicsModel(self.robot,iktype=rave.IkParameterizationType.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
def __init__(self, object_io_interface, scene_interface=None, verbose=False, num_hops=2, vis=False):
self._verbose = verbose
self._sampler_viewer = vis
self._orEnv = orpy.Environment() # create openrave environment
self._orEnv.SetDebugLevel(orpy.DebugLevel.Fatal)
self._orEnv.GetCollisionChecker().SetCollisionOptions(orpy.CollisionOptions.Contacts)
if vis:
self._orEnv.SetViewer('qtcoin') # attach viewer (optional)
self._or_handles = []
else:
self._or_handles = None
self._scene_or_env = None
self._hand_loaded = False
self._scene_interface = scene_interface
self._obj_loaded = False
self._max_iters = 40
self._reachability_weight = 1.0
self._mu = 2.0
self._min_stability = 0.0
self._b_force_new_hfts = False
self._object_kd_tree = None
self._object_points = None
# self._hops = num_hops
# TODO remove this aga
self._hops = 2
self._robot = None
self._obj = None
self._obj_com = None
self._data_labeled = None
self._hand_manifold = None
self._num_contacts = None
self._contact_combinations = []
self._num_levels = 0
self._branching_factors = []
self._object_io_interface = object_io_interface
def main():
"""Loads an environment and generates random positions until the robot can
reach an oject from a collision-free pose.
"""
env = openravepy.Environment()
env.Load('data/pr2test1.env.xml')
env.SetViewer('qtcoin')
robot=env.GetRobots()[0]
manip = robot.SetActiveManipulator('rightarm')
pose = generate_random_pos(robot)
robot.SetTransform(pose)
obj = env.GetKinBody('mug1')
#env.GetKinBody('obstacle').SetVisible(False)
try:
pose, sol, torso= get_collision_free_grasping_pose(robot,
obj,
100)
robot.SetTransform(pose)
robot.SetDOFValues(sol, manip.GetArmIndices())
openravepy.raveLogInfo("Done!!")
except ValueError, e:
openravepy.raveLogError("Error while trying to find a pose: %s" % e)
return
def __init__(self, view=False):
env_file = os.path.join(rll_quadrotor_folder,
'simulation/models/trajopt_quadrotor.xml') # TODO
self.env = rave.Environment()
physics = rave.RaveCreatePhysicsEngine(self.env,'bullet')
self.env.SetPhysicsEngine(physics)
self.env.StopSimulation()
self.env.Load(env_file)
self.kinbodies = set([b for b in self.env.GetBodies() if not b.IsRobot()])
self.handles = list()
self.view = view
self.viewer = None
if view:
self.env.SetViewer('qtcoin')
self.viewer = self.env.GetViewer()
time.sleep(0.1)
assert(len(self.env.GetRobots()) == 1)
self.robot = self.env.GetRobots()[0]
self.cc = trajoptpy.GetCollisionChecker(self.env)
self.aabb_cache = dict()
self.is_watertight_cache = dict()
def pr2_flashlidar():
env = rave.Environment()
#env.Load('envs/testpr2sensors.env.xml')
env.Load('envs/pr2-table.env.xml')
env.SetViewer('qtcoin')
time.sleep(1)
start_time = time.time()
sensors = [
s for s in env.GetSensors() if s.GetName().find("flashlidar") != -1
]
lidar = sensors[0]
lidar.Configure(Sensor.ConfigureCommand.PowerOn)
#lidar.Configure(Sensor.ConfigureCommand.RenderDataOn)
while True:
start_time = time.time()
olddata = lidar.GetSensorData(Sensor.Type.Laser)
while True:
data = lidar.GetSensorData(Sensor.Type.Laser)
if data.stamp != olddata.stamp:
break
time.sleep(0.1)
print('Elapsed: {0}'.format(time.time() - start_time))
break
lidar.Configure(Sensor.ConfigureCommand.PowerOff)
#lidar.Configure(Sensor.ConfigureCommand.RenderDataOff)
IPython.embed()
def main():
global e
global m
global robot
global knownstates
e = r.Environment()
m = r.RaveCreateModule(e, 'orcdchomp')
e.SetViewer('qtcoin')
#e.Load( 'lab1.env.xml' );
e.Load('herb2_padded_nosensors.robot.xml')
robot = e.GetRobot('Herb2')
knownstates = readKnownStates("data/knownstates.data")
printKnownStates(knownstates)
return
#robot.SetActiveManipulator( 0 )
filename = "randomstates2.data"
#filename = "randomManipulatorStates_0.data"
userGetRandomStates(filename)
return
def eih():
env = rave.Environment()
env.Load('data/testwamcamera.env.xml')
env.SetViewer('qtcoin')
ienablesensor = 0
while True:
sensors = env.GetSensors()
for i, sensor in enumerate(sensors):
if i == ienablesensor:
sensor.Configure(Sensor.ConfigureCommand.PowerOn)
sensor.Configure(Sensor.ConfigureCommand.RenderDataOn)
else:
sensor.Configure(Sensor.ConfigureCommand.PowerOff)
sensor.Configure(Sensor.ConfigureCommand.RenderDataOff)
print 'showing sensor %s, try moving obstacles' % (
sensors[ienablesensor].GetName())
if sensors[ienablesensor].Supports(Sensor.Type.Laser):
# if laser, wait for the sensor data to be updated and then print it
olddata = sensors[ienablesensor].GetSensorData(Sensor.Type.Laser)
while True:
data = sensors[ienablesensor].GetSensorData(Sensor.Type.Laser)
if data.stamp != olddata.stamp:
break
time.sleep(0.1)
print 'sensor data: ', data.ranges
time.sleep(5)
ienablesensor = (ienablesensor + 1) % len(sensors)
IPython.embed()
def __init__(self, databaseutils=None,
robotName="care-o-bot3.zae",
robotManipulator="arm",
env=None,
robot=None,
pregrasp_offset=0.2):
self.robotName = roslib.packages.get_pkg_dir("srs_grasping")+"/robots/"+robotName;
self.manipulator = robotManipulator;
self.env = env;
self.robot = robot;
self.pregrasp_offset = pregrasp_offset;
self.databaseutils = databaseutils
if self.databaseutils is None:
self.databaseutils = grasping_functions.databaseutils();
self.graspingutils = self.databaseutils.get_graspingutils();
try:
if self.env is None:
self.env = openravepy.Environment();
if self.robot is None:
self.robot = self.env.ReadRobotXMLFile(self.robotName);
self.robot.SetActiveManipulator(self.manipulator);
self.env.AddRobot(self.robot);
except:
rospy.logerr("The robot file %s does not exists.", self.robotName);
return GraspingErrorCodes.CORRUPTED_ROBOT_MESH_FILE;
def __init__(self):
# set up openrave
self.env = rave.Environment()
self.env.StopSimulation()
self.env.Load(
"robots/pr2-beta-static.zae") # todo: use up-to-date urdf
self.robot = self.env.GetRobots()[0]
self.joint_listener = TopicListener("/joint_states", sm.JointState)
# rave to ros conversions
joint_msg = self.get_last_joint_message()
ros_names = joint_msg.name
inds_ros2rave = np.array(
[self.robot.GetJointIndex(name) for name in ros_names])
self.good_ros_inds = np.flatnonzero(
inds_ros2rave != -1) # ros joints inds with matching rave joint
self.rave_inds = inds_ros2rave[
self.
good_ros_inds] # openrave indices corresponding to those joints
self.update_rave()
self.larm = Arm(self, "l")
self.rarm = Arm(self, "r")
self.lgrip = Gripper(self, "l")
self.rgrip = Gripper(self, "r")
self.head = Head(self)
self.torso = Torso(self)
self.base = Base(self)
rospy.on_shutdown(self.stop_all)
def test_camera():
env = rave.Environment()
env.Load('../envs/pr2-empty.env.xml')
env.SetViewer('qtcoin')
#env.GetViewer().SendCommand('SetFiguresInCamera 1') # also shows the figures in the image
time.sleep(1)
robot = env.GetRobots()[0]
sensor = robot.GetAttachedSensor('head_cam').GetSensor()
cam = Camera(robot, sensor)
is_hits, hits = cam.get_hits(subsampled=True)
zbuffer = cam.get_zbuffer()
height, width, _ = hits.shape
global handles
for i in xrange(height):
for j in xrange(width):
#print zpoints[i,j,:]
if is_hits[i, j]:
if cam.is_in_fov(hits[i, j, :], zbuffer):
handles += [utils.plot_point(env, hits[i, j, :], size=.01)]
IPython.embed()
def __init__(self):
self.env = openravepy.Environment()
self.env.SetViewer('qtcoin')
self.env.GetViewer().SetName('Tutorial Viewer')
self.env.Load('models/%s.env.xml' % PACKAGE_NAME)
# time.sleep(3) # wait for viewer to initialize. May be helpful to uncomment
self.robot = self.env.GetRobots()[0]
def load(source, content_type='application/zip'):
# determine content type
suffix = ''
if content_type == 'application/xml':
suffix = '.xml'
if content_type == 'application/zip':
suffix = '.zae'
# write source into a file, then load it in openrave
env = openravepy.Environment()
with tempfile.NamedTemporaryFile(suffix=suffix) as f:
f.write(source)
f.flush()
if not env.Load(f.name):
raise ValueError('Failed to parse the file.')
# extract information from the robot
data = {}
with env:
robot = env.GetRobots()[0]
data = {
'name': robot.GetName(),
'description': robot.GetDescription(),
'dof': robot.GetDOF(),
'sensors': [sensor.GetName() for sensor in robot.GetSensors()],
}
return data
def setUp(self):
import openravepy
if not self.is_setup:
openravepy.RaveInitialize(True)
openravepy.misc.InitOpenRAVELogging()
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Fatal)
self.is_setup = True
self.env = openravepy.Environment()
with self.env:
self.env.Load('data/wamtest2.env.xml')
self.robot = self.env.GetRobot('BarrettWAM')
self.manipulator = self.robot.GetManipulator('arm')
self.env.Remove(self.env.GetKinBody('floor'))
self.robot.SetActiveManipulator(self.manipulator)
self.robot.SetActiveDOFs(self.active_dof_indices)
self.planner = self.planner_factory()
for base_cls in self.__class__.__bases__:
if base_cls != BasePlannerTest and hasattr(base_cls, 'setUp'):
base_cls.setUp(self)
def env():
"""Simple openrave environment."""
rave_env = orpy.Environment()
rave_env.Load('data/lab1.env.xml')
rave_env.GetRobots()[0].SetActiveDOFs(range(7))
yield rave_env
rave_env.Destroy()
def test_cd():
global handles
env = rave.Environment()
env.Load('../envs/pr2-test.env.xml')
env.SetViewer('qtcoin')
mug = env.GetKinBody('mug')
mug_pos = tfx.pose(mug.GetTransform()).position.array
mugcd = rave.databases.convexdecomposition.ConvexDecompositionModel(mug)
if not mugcd.load():
mugcd.autogenerate()
mugcd_trimesh = mugcd.GenerateTrimeshFromHulls(mugcd.linkgeometry[0][0][1])
new_mug = rave.RaveCreateKinBody(env, '')
new_mug.SetName('new_mug')
new_mug.InitFromTrimesh(mugcd_trimesh)
new_mug.SetTransform(mug.GetTransform())
#env.Add(new_mug, True)
env.Remove(mug)
I, V = mugcd_trimesh.indices, mugcd_trimesh.vertices
for indices in I:
v0 = mug_pos + V[indices[0], :]
v1 = mug_pos + V[indices[1], :]
v2 = mug_pos + V[indices[2], :]
handles += utils.plot_segment(env, v0, v1)
handles += utils.plot_segment(env, v1, v2)
handles += utils.plot_segment(env, v2, v0)
IPython.embed()
def pr2_sensors():
env = rave.Environment()
#env.Load('robots/pr2-beta-sim.robot.xml')
env.Load('envs/testpr2sensors.env.xml')
r = env.GetRobots()[0]
env.SetViewer('qtcoin')
ienablesensor = 0
while True:
start_time = time.time()
sensors = env.GetSensors()
for i, sensor in enumerate(sensors):
if i == ienablesensor:
sensor.Configure(Sensor.ConfigureCommand.PowerOn)
sensor.Configure(Sensor.ConfigureCommand.RenderDataOn)
else:
sensor.Configure(Sensor.ConfigureCommand.PowerOff)
sensor.Configure(Sensor.ConfigureCommand.RenderDataOff)
print 'showing sensor %s, try moving obstacles' % (
sensors[ienablesensor].GetName())
if sensors[ienablesensor].Supports(Sensor.Type.Laser):
# if laser, wait for the sensor data to be updated and then print it
olddata = sensors[ienablesensor].GetSensorData(Sensor.Type.Laser)
while True:
data = sensors[ienablesensor].GetSensorData(Sensor.Type.Laser)
if data.stamp != olddata.stamp:
break
time.sleep(0.1)
print 'sensor data: ', data.ranges
#time.sleep(5)
ienablesensor = (ienablesensor + 1) % len(sensors)
print('Elapsed: {0}'.format(time.time() - start_time))
IPython.embed()
def setUp(self):
self.env = openravepy.Environment()
self.env.Load('wamtest1.env.xml')
self.robot = self.env.GetRobot('BarrettWAM')
self.manipulator = self.robot.GetManipulator('arm')
self.robot.Enable(True)
# Set all 7 DOF of the WAM arm to active
with self.env:
self.robot.SetActiveDOFs(self.manipulator.GetArmIndices())
self.robot.SetActiveManipulator(self.manipulator)
self.active_dof_indices = self.robot.GetActiveDOFIndices()
# Get the resolution (in radians) for the 7 joints
# [0.0043, 0.0087, 0.0087, 0.0174, 0.0193, 0.0282, 0.0282]
self.dof_resolutions = \
self.robot.GetDOFResolutions()[self.active_dof_indices]
#add the box object we will grab
self.to_grab_body = openravepy.RaveCreateKinBody(self.env, '')
self.to_grab_body.SetName('box')
self.to_grab_body.InitFromBoxes(numpy.array([[0., 0., 0., 0.1, 0.1, 0.1]]), False)
self.env.Add(self.to_grab_body)
self.to_grab_body.Enable(True)
T = numpy.eye(4)
T[2,3] = 20. #move far to not be in collision
self.to_grab_body.SetTransform(T)
def __init__(self):
self.env = rave.Environment()
self.env.SetViewer('qtcoin')
rospy.loginfo('Before loading model')
ravenFile = os.path.join(roslib.packages.get_pkg_subdir('RavenDebridement','models'),'myRaven.xml')
#ravenFile = '/home/gkahn/ros_workspace/RavenDebridement/models/myRaven.xml'
self.env.Load(ravenFile)
rospy.loginfo('After loading model')
self.robot = self.env.GetRobots()[0]
# setup variables for kinect constraint generation
self.num_cd_components = 50
self.geom_type = 'cd'
self.kinect_topic = '/camera/depth/points'
self.kinect_depth_frame = '/camera_depth_optical_frame'
self.robot_frame = '/world'
self.T_kinect_to_robot = tfx.lookupTransform(self.robot_frame, self.kinect_depth_frame, wait=20)
self.cd_body_names = []
self.cloud_id = 0
rospy.Subscriber(self.kinect_topic, PointCloud2, self.kinect_callback)
def setUpClass(cls):
np.set_printoptions(precision=6, suppress=True)
env = orpy.Environment()
makita = env.ReadKinBodyXMLFile('objects/makita.kinbody.xml')
env.AddKinBody(makita)
cls.env = env
print('') # dummy line
def __init__(self, showViewer=True):
'''Initializes openrave environment, etc.'''
# Parameters
self.projectDir = "/home/mgualti/mgualti/PickAndPlace/simulation"
# Create openrave environment
self.env = openravepy.Environment()
if showViewer: self.env.SetViewer('qtcoin')
self.showViewer = showViewer
self.env.Load(self.projectDir + "/openrave/environment_1.xml")
self.robot = self.env.GetRobots()[0]
self.robot.SetDOFValues(array([0.034]))
# don't want to be affected by gravity, since it is floating
for link in self.robot.GetLinks():
link.SetStatic(True)
#self.physicsEngine = openravepy.RaveCreatePhysicsEngine(self.env, "ode")
#self.env.SetPhysicsEngine(self.physicsEngine)
#self.env.GetPhysicsEngine().SetGravity([0,0,-9.8])
#self.env.GetPhysicsEngine().SetGravity([0,0,0])
self.env.StopSimulation()
tableObj = self.env.GetKinBody("table")
self.tablePosition = tableObj.GetTransform()[0:3, 3]
self.tableExtents = tableObj.ComputeAABB().extents()
def __init__(self, args):
# Initialize values
self.busy = False
self.topic_name = args.topic
self.file_name = args.file_name
self.octomap_resolution = str(args.resolution)
self.octomap_range = str(args.range)
self.octomap_frame = args.frame
# env_name = '' # TODO:
# robot_name = '' # TODO:
# obstacle = '' # TODO:
self.durations = []
# Initialize OpenRAVE environment
self.env = orpy.Environment()
self.env.SetViewer('qtcoin')
orpy.RaveSetDebugLevel(orpy.DebugLevel.Error)
# self.env.Load(env_name)
# Initialize ROS subscribers and publisher
self.sub_octomap = rospy.Subscriber('/occupied_cells_vis_array',
MarkerArray,
callback=self.process_octomap,
queue_size=1)
# Initialize octomap plugin
orpy.RaveLoadPlugin("or_octomap") # Load octomap plugin
self.sensor_server = orpy.RaveCreateSensorSystem(self.env, "or_octomap")
self.sensor_server.SendCommand("Enable") # Enable octomap thread
self.sensor_server.SendCommand("ResetTopic " +
self.topic_name) # Reset the registered pcl topic
self.sensor_server.SendCommand("ResetResolution " +
self.octomap_resolution) # Reset the octomap resolution
self.sensor_server.SendCommand("ResetRange " +
self.octomap_range) # Reset the octomap max range
self.sensor_server.SendCommand("ResetFrameID " +
self.octomap_frame) # Reset the octomap frame
self.time_start = time.time()
def openrave_init(self):
self.env = openravepy.Environment()
self.env.SetViewer('qtcoin')
self.env.GetViewer().SetName('HW1 Viewer')
self.env.Load(curr_path + '/models/%s.env.xml' % PACKAGE_NAME)
# time.sleep(3) # wait for viewer to initialize. May be helpful to
# uncomment
self.robot_left = self.env.GetRobots()[0]
self.robot_right = self.env.GetRobots()[1]
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]
self.right_manip = self.robot_right.GetManipulator('rightarm')
# self.right_manip = self.robot_right.GetManipulator('right_wam')
# self.robot.SetActiveDOFs(right_manip.GetArmIndices())
# self.robot.SetActiveDOFValues(right_relaxed)
self.left_manip = self.robot_left.GetManipulator('leftarm')
# self.robot.SetActiveDOFs(left_manip.GetArmIndices())
# self.robot.SetActiveDOFValues(left_relaxed)
self.robot_right.SetActiveManipulator('rightarm')
self.robot_left.SetActiveManipulator('leftarm')
def main():
global m
global e
e = r.Environment()
m = r.RaveCreateModule(e, 'orchomp')
e.SetViewer('qtcoin')
e.Load("intelkitchen_robotized_herb2_nosensors.env.xml")
robot = e.GetRobot("Herb2")
# set the manipulator to leftarm
#ikmodel = databases.inversekinematics.InverseKinematicsModel(
# robot,iktype=IkParameterization.Type.Transform6D)
#if not ikmodel.load():
# ikmodel.autogenerate()
Tz = r.matrixFromAxisAngle([0, 0, numpy.pi / 2])
Tz[0, 3] = 0.4
Tz[1, 3] = 1.6
Tz[2, 3] = -0.01
print Tz
with e:
for body in e.GetBodies():
body.SetTransform(numpy.dot(Tz, body.GetTransform()))
readFileCommands()
def main():
global get_motion_plan, env, robot, markerpub
if rospy.get_name() == "/unnamed":
rospy.init_node("move_group_battery")
env = rave.Environment()
env.Load("robot/pr2-beta-static.zae")
loadsuccess = env.Load(envfile)
assert loadsuccess
markerpub = rospy.Publisher('battery_targets', Marker)
get_motion_plan = rospy.ServiceProxy('plan_kinematic_path', GetMotionPlan)
print "waiting for plan_kinematic_path"
get_motion_plan.wait_for_service()
print "ok"
robot = env.GetRobots()[0]
update_rave_from_ros(robot, ROS_DEFAULT_JOINT_VALS, ROS_JOINT_NAMES)
xs, ys, zs = np.mgrid[.35:.65:.05, 0:.5:.05, .8:.9:.1]
results = []
for (x, y, z) in zip(xs.flat, ys.flat, zs.flat):
result = test_plan_to_pose([x, y, z], [0, 0, 0, 1],
"left",
robot,
planner_id=args.planner_id)
# publish_marker(x, y, z)
if result is not None: results.append(result)
process_results(results)
def initialize(model_filename='jaco', envXML=None):
'''
Load and configure the JACO robot. If envXML is not None, loads environment.
Returns robot and environment.
'''
env = openravepy.Environment()
if envXML is not None:
env.LoadURI(envXML)
env.SetViewer('qtcoin')
# Assumes the robot files are located in the data folder of the
# kinova_description package in the catkin workspace.
urdf_uri = 'package://iact_control/src/data/'+model_filename+'.urdf'
srdf_uri = 'package://iact_control/src/data/'+model_filename+'.srdf'
or_urdf = openravepy.RaveCreateModule(env, 'urdf')
robot_name = or_urdf.SendCommand('load {:s} {:s}'.format(urdf_uri, srdf_uri))
robot = env.GetRobot(robot_name)
bind_subclass(robot, ArchieRobot)
robot.SetActiveDOFs(np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9]))
robot.SetDOFValues(robot_starting_dofs)
viewer = env.GetViewer()
viewer.SetSize(500,500)
cam_params = np.array([[-0.99885711, -0.01248719, -0.0461361 , -0.18887213],
[ 0.02495645, 0.68697757, -0.72624996, 2.04733515],
[ 0.04076329, -0.72657133, -0.68588079, 1.67818344],
[ 0. , 0. , 0. , 1. ]])
viewer.SetCamera(cam_params)
#viewer.SetBkgndColor([0.8,0.8,0.8])
return env, robot
def wam7_test(self):
env = openravepy.Environment()
try:
robot = env.ReadRobotXMLFile('robots/barrettwam.robot.xml')
env.Add(robot)
robot.SetActiveDOFs(robot.GetManipulator('arm').GetArmIndices())
robot.SetActiveDOFValues(q_start)
planner = openravepy.RaveCreatePlanner(env, 'LEMUR')
self.assertIsNotNone(planner)
params = openravepy.Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetGoalConfig(q_goal)
params.SetExtraParameters(
'<roadmap_type>Halton</roadmap_type>' +
'<roadmap_param>num=1000</roadmap_param>' +
'<roadmap_param>radius=2.0</roadmap_param>')
planner.InitPlan(robot, params)
traj = openravepy.RaveCreateTrajectory(env, '')
result = planner.PlanPath(traj)
self.assertEqual(result, openravepy.PlannerStatus.HasSolution)
self.assertEqual(6, traj.GetNumWaypoints())
for iwp in range(6):
wp = traj.GetWaypoint(iwp)
self.assertEqual(7, len(wp))
for a, b in zip(wp, traj_expected[iwp]):
self.assertAlmostEqual(a, b, delta=1.0e-5)
finally:
env.Destroy()
def main(env=None, test=False):
"Main function."
parser = argparse.ArgumentParser(description="A simple example in which trajectories, which are planned using"
"OpenRAVE is retimed using toppra. The trajectories are found using"
"birrt, the default planner. Goals are generated randomly and "
"uniformly.")
parser.add_argument('-e', '--env', help='OpenRAVE Environment file', default="data/lab1.env.xml")
parser.add_argument('-v', '--verbose', help='Show DEBUG log and plot trajectories', action="store_true")
parser.add_argument('-N', '--Ngrid', help='Number of discretization step', default=100, type=int)
args = vars(parser.parse_args())
if args['verbose']:
toppra.setup_logging('DEBUG')
else:
toppra.setup_logging('INFO')
if env is None:
env = orpy.Environment()
env.SetDebugLevel(0)
env.Load(args['env'])
env.SetViewer('qtosg')
robot = env.GetRobots()[0]
robot.SetDOFAccelerationLimits(np.ones(11) * 3)
manipulator = robot.GetManipulators()[0]
robot.SetActiveManipulator(manipulator)
robot.SetActiveDOFs(manipulator.GetArmIndices())
controller = robot.GetController()
basemanip = orpy.interfaces.BaseManipulation(robot)
dof = robot.GetActiveDOF()
pc_torque = toppra.create_rave_torque_path_constraint(
robot, discretization_scheme=toppra.constraint.DiscretizationType.Interpolation)
it = 0
while True or (test and it > 5):
lower, upper = robot.GetActiveDOFLimits()
qrand = np.random.rand(dof) * (upper - lower) + lower
with robot:
robot.SetActiveDOFValues(qrand)
incollision = env.CheckCollision(robot) or robot.CheckSelfCollision()
if incollision:
continue
traj_original = basemanip.MoveActiveJoints(qrand, execute=False, outputtrajobj=True)
if traj_original is None:
continue
traj_retimed, trajra = toppra.retime_active_joints_kinematics(
traj_original, robot, output_interpolator=True, N=args['Ngrid'],
additional_constraints=[pc_torque])
print(("Original duration: {:.3f}. Retimed duration: {:3f}.".format(
traj_original.GetDuration(), traj_retimed.GetDuration())))
if args['verbose']:
plot_trajectories(traj_original, traj_retimed, robot)
time.sleep(1)
controller.SetPath(traj_retimed)
robot.WaitForController(0)
it += 1
def setUp(self):
self.env = openravepy.Environment()
self.fuze = prpy.rave.add_object(self.env, 'fuze_bottle',
'objects/fuze_bottle.kinbody.xml')
self.fuze_pose = numpy.eye(4)
self.fuze_pose[0:3, 3] = [0.8, -0.3, 0.4]
self.fuze.SetTransform(self.fuze_pose)
self.fuze.Enable(True)
def __init__(self):
# set up openrave
self.env = rave.Environment()
self.env.Load("robots/pr2-beta-static.zae")
self.robot = self.env.GetRobots()[0]
self.larm = Arm(self, "l")
self.rarm = Arm(self, "r")
def setUp(self):
self.env = openravepy.Environment()
urdf_module = openravepy.RaveCreateModule(self.env, 'urdf')
with self.env:
arg = 'load {:s}'.format(urdf_path)
bh_name = urdf_module.SendCommand(arg)
self.bh_body = self.env.GetKinBody(bh_name)
