def setUp(self):
env=Environment()
#env.SetViewer('qtcoin')
env.Load('test_collisionmap.env.xml')
robot=env.GetRobots()[0]
self.pose=Pose(robot)
self.env=env
self.env.SetDebugLevel(1)
self.robot=robot
class MetaPlannerTests(object):
def setUp(self):
from openravepy import Environment, RaveCreateTrajectory
self.join_timeout = 5.0
self.env = Environment()
self.env.Load('data/lab1.env.xml')
self.robot = self.env.GetRobot('BarrettWAM')
# Create a valid trajectory used to test planner successes.
cspec = self.robot.GetActiveConfigurationSpecification()
self.traj = RaveCreateTrajectory(self.env, 'GenericTrajectory')
self.traj.Init(cspec)
self.traj.Insert(0, numpy.zeros(cspec.GetDOF()))
self.traj.Insert(1, numpy.ones(cspec.GetDOF()))
def _create_environment(self, env_path):
"""
Create an Environment instance and load an XML environment to it.
Will create an openravepy.Environment instance and will try to
load an XML environment specification from file.
Args:
env_path: An XML file to load an environment.
Returns:
The openravepy.Environment instance.
Raises:
ValueError: If unable to load environment from file.
"""
env = Environment()
if env_path is not None:
if not env.Load(env_path):
raise ValueError("Unable to load environment "
"file: '{}'".format(env_path))
return env
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
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
objA = box_body(env,width,length,height,\
class DistanceFieldManagerTest(TestCase):
def setUp(self):
from prpy.planning.chomp import DistanceFieldManager
from openravepy import Environment
self.env = Environment()
self.env.Load('data/wamtest2.env.xml')
self.robot = self.env.GetRobot('BarrettWAM')
self.body = self.env.GetKinBody('mug-table')
self.bodies = set(self.env.GetBodies())
self.module = CHOMPModuleMock(self.env)
self.manager = DistanceFieldManager(self.module)
def test_GetGeometricState_ChangeEnabledStatusChangesState(self):
with self.env:
link = self.robot.GetLinks()[0]
link.Enable(True)
state_before = self.manager.get_geometric_state(self.robot)
link.Enable(False)
state_after = self.manager.get_geometric_state(self.robot)
self.assertNotEquals(state_after, state_before)
def test_GetGeometricState_ChangeInDOFValuesChangesState(self):
with self.env:
lower_limits, upper_limits = self.robot.GetDOFLimits()
self.robot.SetDOFValues(lower_limits)
state_before = self.manager.get_geometric_state(self.robot)
self.robot.SetDOFValues(upper_limits)
state_after = self.manager.get_geometric_state(self.robot)
self.assertNotEquals(state_after, state_before)
def test_GetGeometricState_ChangeInKinematicsGeometryHashChangesState(self):
self.robot.GetKinematicsGeometryHash = lambda: 'mock_before'
state_before = self.manager.get_geometric_state(self.robot)
self.robot.GetKinematicsGeometryHash = lambda: 'mock_after'
state_after = self.manager.get_geometric_state(self.robot)
self.assertNotEquals(state_after, state_before)
def test_GetCachePath_DifferentStatesProduceDifferentPaths(self):
self.robot.GetKinematicsGeometryHash = lambda: 'mock_before'
state_before = self.manager.get_geometric_state(self.robot)
path_before = self.manager.get_cache_path(state_before)
self.robot.GetKinematicsGeometryHash = lambda: 'mock_after'
state_after = self.manager.get_geometric_state(self.robot)
path_after = self.manager.get_cache_path(state_after)
self.assertNotEquals(path_after, path_before)
def test_Sync_InitiallyCreatesAllDistanceFields(self):
self.manager.sync(self.robot)
computed_bodies = [ args['kinbody'] for args in self.module.computedistancefield_args ]
self.assertItemsEqual(self.bodies, computed_bodies)
self.assertEqual(len(self.module.removefield_args), 0)
def test_Sync_SyncTwiceDoesNothing(self):
self.manager.sync(self.robot)
del self.module.computedistancefield_args[:]
del self.module.removefield_args[:]
self.manager.sync(self.robot)
self.assertEqual(len(self.module.computedistancefield_args), 0)
self.assertEqual(len(self.module.removefield_args), 0)
def test_Sync_IgnoresActiveRobotLinks(self):
self.is_processed = False
def computedistancefield(kinbody=None, cube_extent=None,
aabb_padding=None, cache_filename=None,
releasegil=False, **kw_args):
if kinbody.GetName() == self.robot.GetName():
self.assertTrue(self.robot.GetLink('segway').IsEnabled())
self.assertTrue(self.robot.GetLink('wam0').IsEnabled())
self.assertTrue(self.robot.GetLink('wam1').IsEnabled())
self.assertTrue(self.robot.GetLink('wam2').IsEnabled())
self.assertTrue(self.robot.GetLink('wam3').IsEnabled())
self.assertFalse(self.robot.GetLink('wam4').IsEnabled())
self.assertFalse(self.robot.GetLink('wam5').IsEnabled())
self.assertFalse(self.robot.GetLink('wam6').IsEnabled())
self.assertFalse(self.robot.GetLink('wam7').IsEnabled())
self.assertFalse(self.robot.GetLink('handbase').IsEnabled())
self.assertFalse(self.robot.GetLink('Finger0-0').IsEnabled())
self.assertFalse(self.robot.GetLink('Finger0-1').IsEnabled())
self.assertFalse(self.robot.GetLink('Finger0-2').IsEnabled())
self.assertFalse(self.robot.GetLink('Finger1-0').IsEnabled())
self.assertFalse(self.robot.GetLink('Finger1-1').IsEnabled())
self.assertFalse(self.robot.GetLink('Finger1-2').IsEnabled())
self.assertFalse(self.robot.GetLink('Finger2-1').IsEnabled())
self.assertFalse(self.robot.GetLink('Finger2-2').IsEnabled())
self.is_processed = True
dof_index = self.robot.GetJoint('Elbow').GetDOFIndex()
self.robot.SetActiveDOFs([ dof_index ])
self.robot.Enable(True)
self.module.computedistancefield = computedistancefield
self.manager.sync(self.robot)
self.assertTrue(self.is_processed)
def test_Sync_RecomputesDistanceFieldIfStateChanges(self):
self.manager.sync(self.robot)
del self.module.computedistancefield_args[:]
del self.module.removefield_args[:]
# Change the geometry to invalidate the key.
link = self.robot.GetLink('segway')
link.SetGroupGeometries('test', [])
link.SetGeometriesFromGroup('test')
self.manager.sync(self.robot)
self.assertEqual(len(self.module.computedistancefield_args), 1)
self.assertEqual(self.module.computedistancefield_args[0]['kinbody'], self.robot)
self.assertEqual(len(self.module.removefield_args), 1)
self.assertEqual(self.module.removefield_args[0]['kinbody'], self.robot)
self.assertLess(self.module.removefield_args[0]['__sequence__'],
self.module.computedistancefield_args[0]['__sequence__'])
class Turbine:
""" This class provides a configuration file loading for the whole turbine environment,
plus manipulation of the active elements of the turbine (robot/rail)
and their interacting (collision)
Args:
config: (TurbineConfig) configuration file object.
"""
env = None
config = None
# std names for printing propouses
std_name = {
_PRIMARY_RAIL: "primary rail",
_SECONDARY_RAIL: "secondary rail",
_BLADE: "blade",
_RUNNER_AREA: "runner area",
_IRIS: "iris",
_ROTOR: "rotor"
}
def __init__(self, config):
self.config = config
# Create OpenRave environment
RaveInitialize(True, 0)
self.env = Environment()
self.env.Load(config.environment.load)
self.robot = self.env.GetRobots()[0]
self.manipulator = self.robot.GetActiveManipulator()
ikmodel = databases.inversekinematics.InverseKinematicsModel(
robot=self.robot, iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
# Principal turbine elements linked to attributes
self.bodies = self.env.GetBodies()
try:
self.rotor = next(body for body in self.bodies
if body.GetName() == _ROTOR)
except StopIteration:
raise XMLStructError(_ROTOR)
try:
self.iris = next(body for body in self.bodies
if body.GetName() == _IRIS)
except StopIteration:
raise XMLStructError(_IRIS)
try:
self.runner_area = next(body for body in self.bodies
if body.GetName() == _RUNNER_AREA)
except StopIteration:
raise XMLStructError(_RUNNER_AREA)
try:
self.primary = next(body for body in self.bodies
if body.GetName() == _PRIMARY_RAIL)
except StopIteration:
raise XMLStructError(_PRIMARY_RAIL)
try:
self.secondary = next(body for body in self.bodies
if body.GetName() == _SECONDARY_RAIL)
except StopIteration:
raise XMLStructError(_SECONDARY_RAIL)
blade_number = 1
try:
self.blades = [
next(body for body in self.bodies
if body.GetName() == _BLADE + str(blade_number))
]
except StopIteration:
raise XMLStructError(_BLADE + str(blade_number))
blade_found = True
while blade_found:
blade_number += 1
try:
self.blades.append(
next(body for body in self.bodies
if body.GetName() == _BLADE + str(blade_number)))
except StopIteration:
blade_found = False
def place_rail(self, rail_place):
""" Place both rails in the environment
Args:
rail_place: (RailPlace) rail object.
"""
# P - primary rail,
# S - secondary rail,
# alpha - angle from the perpendicular to the primary rail
# Using camera standard axis and transformLookat for rails
# Primary Rail
primary_extent = self.primary.GetLinks()[0].GetGeometries(
)[0].GetBoxExtents()
primary_offset = array([
0, -primary_extent[1], -primary_extent[2]
]) + array([0, 0, self.config.environment.primary_safe_margin])
primary_offset_transform = eye(4)
primary_offset_transform[0:3, 3] = primary_offset
# Secondary Rail
secondary_extent = self.secondary.GetLinks()[0].GetGeometries(
)[0].GetBoxExtents()
# Resizing
secondary_extent[2] = abs(
rail_place.s) / 2.0 + self.config.environment.secondary_safe_margin
self.env.Remove(self.secondary)
self.secondary.InitFromBoxes(
array([concatenate([zeros(3), secondary_extent])]), True)
self.env.AddKinBody(self.secondary)
#
secondary_offset = array(
[0, -secondary_extent[1], secondary_extent[2]]) + array([
0, self.config.environment.robot_level_difference,
-self.config.environment.secondary_safe_margin
])
secondary_offset_transform = eye(4)
secondary_offset_transform[0:3, 3] = secondary_offset
# Rails Traonsform and Placement
primary_transform = transformLookat(
array([0, 0, self.config.environment.z_floor_level]),
array([rail_place.p, 0, self.config.environment.z_floor_level]),
[0, 0, 1])
self.primary.SetTransform(
dot(primary_transform, primary_offset_transform))
secondary_transform = transformLookat(
rail_place.getXYZ(self.config),
array([rail_place.p, 0, self.config.environment.z_floor_level]),
[0, 0, 1])
self.secondary.SetTransform(
dot(secondary_transform, secondary_offset_transform))
def check_rail_collision(self):
""" Check rail <-> blades collision """
with self.env:
collisions = [
self.env.CheckCollision(self.primary, blade)
or self.env.CheckCollision(self.secondary, blade)
for blade in self.blades
]
return any(collisions)
def place_robot(self, rail_place):
""" Place robot on the end of the secondary rail """
T = rail_place.getTransform(self.config)
self.robot.SetTransform(T)
def check_robotbase_collision(self):
""" Check robot's base <-> environment collision """
with self.robot:
for link in self.robot.GetLinks():
link.Enable(False)
self.robot.GetLink('Base').Enable(True)
self.robot.GetLink('Arm0').Enable(True)
with self.env:
collision_base = [(self.env.CheckCollision(self.robot, body)
and not (body == self.robot))
for body in self.bodies]
collisions = collision_base
return any(collisions)
def check_robot_collision(self):
""" Check robot <-> environment collision """
collisions = [(self.env.CheckCollision(self.robot, body)
and not (body == self.robot)) for body in self.bodies]
return any(collisions)