def initialize(robot_xml=None,
env_path=None,
viewer='rviz',
sim=True,
**kw_args):
prpy.logger.initialize_logging()
#Hide TrajOpt logging.
os.environ.setdefault('TRAJOPT_LOG_THRESH', 'WARN')
#Load plugins
prpy.dependency_manager.export()
RaveInitialize(True)
#Create the environment
env = Environment()
if env_path is not None:
if not env.Load(env_path):
raise ValueError(
'Unable to load environment from path {:s}'.format(env_path))
#Load the URDF file into OpenRave.
urdf_module = RaveCreateModule(env, 'urdf')
if urdf_module is None:
logger.error('Unable to load or_urdf module. Do you have or_urdf'
'built and installed in one of your Catkin workspaces?')
raise ValueError('Unable to load or_urdf plugin.')
urdf_uri = 'package://fetchpy/robot_description/fetch.gazebo.urdf'
srdf_uri = 'package://fetchpy/robot_description/fetch.srdf'
args = 'Load {:s} {:s}'.format(urdf_uri, srdf_uri)
fetch_name = urdf_module.SendCommand(args)
if fetch_name is None:
raise ValueError('Failed loading FETCH model using or_urdf.')
robot = env.GetRobot(fetch_name)
if robot is None:
raise ValueError(
'Unable to find robot with name "{:s}".'.format(fetch_name))
#Default to FCL.
collision_checker = RaveCreateCollisionChecker(env, 'fcl')
if collision_checker is not None:
env.SetCollisionChecker(collision_checker)
else:
collision_checker = env.GetCollisionChecker()
logger.warning(
'Failed creating "fcl", defaulting to the default OpenRAVE'
' collision checker. Did you install or_fcl?')
#Enable Baking if it is supported
try:
result = collision_checker.SendCommand('BakeGetType')
is_baking_suported = (result is not None)
except openrave_exception:
is_baking_suported = False
if is_baking_suported:
robot_checker_factory = BakedRobotCollisionCheckerFactory()
else:
robot_checker_factory = SimpleRobotCollisionCheckerFactory()
logger.warning(
'Collision checker does not support baking. Defaulting to'
' the slower SimpleRobotCollisionCheckerFactory.')
#Default arguments
keys = [
'arm_sim', 'arm_torso_sim', 'gripper_sim', 'head_sim', 'torso_sim',
'base_sim', 'talker_sim', 'perception_sim', 'whole_body_sim'
]
for key in keys:
if key not in kw_args:
kw_args[key] = sim
prpy.bind_subclass(robot,
FETCHRobot,
robot_checker_factory=robot_checker_factory,
**kw_args)
if sim:
dof_indices, dof_values = robot.configurations.get_configuration(
'straight')
robot.SetDOFValues(dof_values, dof_indices)
#Start by attempting to load or_rviz.
viewers = ['qtcoin', 'rviz']
if viewer is None:
viewer = 'rviz'
if viewer not in viewers:
raise Exception(
'Failed creating viewer of type "{0:s}".'.format(viewer))
if viewer == 'rviz':
env.SetViewer(viewer)
if env.GetViewer() is None:
logger.warning(
'Loading the RViz viewer failed. Do you have or_interactive'
' marker installed? Falling back on qtcoin.')
viewer == 'qtcoin'
if viewer == 'qtcoin':
env.SetViewer(viewer)
if viewer and env.GetViewer() is None:
env.SetViewer(viewer)
if env.GetViewer() is None:
raise Exception(
'Failed creating viewer of type "{0:s}".'.format(viewer))
# if attach_viewer == 'True':
# attach_viewer = 'qtcoin'
# env.SetViewer(attach_viewer)
# # Fall back on qtcoin if loading or_rviz failed
# if env.GetViewer() is None:
# logger.warning('Loading the RViz viewer failed. Do you have or_interactive'
# ' marker installed? Falling back on qtcoin.')
# attach_viewer = 'rviz'
# if attach_viewer and env.GetViewer() is None:
# env.SetViewer(attach_viewer)
# if env.GetViewer() is None:
# raise Exception('Failed creating viewer of type "{0:s}".'.format(attach_viewer))
#Remove the ROS logging handler again. It might have been added when we
# loaded or_rviz.
prpy.logger.remove_ros_logger()
return env, robot
class OpenRAVEViewer(Viewer):
_viewer = None
def __init__(self, env=None):
assert OpenRAVEViewer._viewer == None
if env == None:
self.env = Environment()
else:
self.env = env
self.env.SetViewer('qtcoin')
self.name_to_rave_body = {}
OpenRAVEViewer._viewer = self
def clear(self):
for b in self.name_to_rave_body.itervalues():
b.delete()
self.name_to_rave_body = {}
@staticmethod
def create_viewer():
# if reset and OpenRAVEViewer._viewer != None:
# ## close the old viewer to avoid a threading error
# OpenRAVEViewer._viewer = None
if OpenRAVEViewer._viewer == None:
return OpenRAVEViewer()
OpenRAVEViewer._viewer.clear()
return OpenRAVEViewer._viewer
def record_plan(self, plan, outf, res=(640, 480), cam=None):
"""
creates a video of a plan and stores it in outf
"""
obj_list = []
horizon = plan.horizon
v = self.env.GetViewer()
if osp.exists('.video_images'):
shutil.rmtree('.video_images')
os.makedirs('.video_images')
for p in plan.params.itervalues():
if not p.is_symbol():
obj_list.append(p)
for t in range(horizon):
self.draw(obj_list, t)
time.sleep(0.1)
if cam is None:
cam = v.GetCameraTransform()
im = v.GetCameraImage(res[0], res[1], cam, [640, 640, 320, 240])
scipy.misc.imsave(
'.video_images/frame_' + str('%05d' % t) + '.png', im)
outfname = "{}.mp4".format(outf)
if osp.exists(outfname):
os.remove(outfname)
arglist = [
'avconv', '-f', 'image2', '-r', '10', "-i",
".video_images/frame_%05d.png", "-f", "mp4", "-bf", "1", "-r",
"30", "{}.mp4".format(outf)
]
subprocess.call(arglist)
def initialize_from_workspace(self, workspace):
pass
def draw(self, objList, t, transparency=0.7):
"""
This function draws all the objects from the objList at timestep t
objList : list of parameters of type Object
t : timestep of the trajectory
"""
for obj in objList:
self._draw_rave_body(obj, obj.name, t, transparency)
def draw_traj(self, objList, t_range):
"""
This function draws the trajectory of objects from the objList
objList : list of parameters of type Object
t_range : range of timesteps to draw
"""
for t in t_range:
for obj in objList:
name = "{0}-{1}".format(obj.name, t)
self._draw_rave_body(obj, name, t)
def _draw_rave_body(self, obj, name, t, transparency=0.7):
rotation = np.array([[0], [0], [0]])
assert isinstance(obj, Object)
if name not in self.name_to_rave_body:
self.name_to_rave_body[name] = OpenRAVEBody(
self.env, name, obj.geom)
if isinstance(obj.geom, PR2):
self.name_to_rave_body[name].set_dof(obj.backHeight[:, t],
obj.lArmPose[:, t],
obj.lGripper[:, t],
obj.rArmPose[:, t],
obj.rGripper[:, t])
if isinstance(obj.geom, Can) or isinstance(
obj.geom, Box) or isinstance(obj.geom, Table):
rotation = obj.rotation[:, t]
assert not np.any(np.isnan(rotation))
assert not np.any(np.isnan(obj.pose[:, t]))
self.name_to_rave_body[name].set_pose(obj.pose[:, t], rotation)
self.name_to_rave_body[name].set_transparency(transparency)
def animate_plan(self, plan, delay=.1):
obj_list = []
horizon = plan.horizon
for p in plan.params.itervalues():
if not p.is_symbol():
obj_list.append(p)
for t in range(horizon):
self.draw(obj_list, t)
time.sleep(delay)
def draw_plan(self, plan):
horizon = plan.horizon
self.draw_plan_range(plan, (0, horizon - 1))
def draw_plan_range(self, plan, (start, end)):
obj_list = self._get_plan_obj_list(plan)
self.draw_traj(obj_list, range(start, end + 1))
from openravepy import RaveSetDebugLevel, DebugLevel, Environment, RaveDestroy
from openravepy.misc import SetViewerUserThread
from collections import deque
from random import sample as random_sample
#from tamp.learning_tamp import *
from random import random
from manipulation.primitives.utils import get_env
from manipulation.primitives.transforms import *
from openravepy import *
from manipulation.bodies.robot import open_gripper, get_manip_trans
from manipulation.bodies.robot import manip_from_pose_grasp
import copy
env=Environment()
ENV_FILENAME = '../..//env/just_empty.xml'
env.SetViewer('qtcoin')
env.Load(ENV_FILENAME)
def translate_point( target_transform,point):
if len(point) == 3:
point = np.concatenate([point, np.array([1])])
elif len(point) != 4:
print 'Invalid dimension'
return
transformed_point = trans_dot( target_transform, point) # equation 2.23 in Murray
return transformed_point[:-1]
# define object and place it (0,0,0)
width = 0.07
length = 0.03
height = 0.1