def __init__(self, problem_idx):
np.random.seed(problem_idx)
random.seed(problem_idx)
self.env = Environment()
collisionChecker = openravepy.RaveCreateCollisionChecker(
self.env, 'fcl_')
self.env.SetCollisionChecker(collisionChecker)
self.problem_idx = problem_idx
self.initial_placements = []
self.placements = []
self.robot = None
self.objects = None
self.curr_state = None
self.curr_obj = None
self.init_saver = None
self.init_which_opreator = None
self.v = False
self.robot_region = None
self.obj_region = None
self.objs_to_move = None
self.problem_config = None
self.init_objs_to_move = None
self.optimal_score = None
self.name = None
self.is_solving_packing = False
self.is_solving_namo = False
self.is_solving_fetching = False
self.high_level_planner = None
self.namo_planner = None
self.fetch_planner = None
self.env.StopSimulation() # openrave crashes with physics engine on
self.motion_planner = None
def __init__(self, problem_idx):
self.env = Environment()
self.initial_placements = []
self.placements = []
self.objects_currently_not_in_goal = []
self.robot = None
self.objects = None
self.curr_state = None
self.curr_obj = None
self.init_saver = None
self.init_which_opreator = None
self.v = False
self.robot_region = None
self.obj_region = None
self.objs_to_move = None
self.problem_config = None
self.init_objs_to_move = None
self.optimal_score = None
self.name = None
self.is_solving_packing = False
self.is_solving_namo = False
self.is_solving_fetching = False
self.high_level_planner = None
self.namo_planner = None
self.fetch_planner = None
self.infeasible_reward = -2
self.regions = {}
self.env.StopSimulation()
self.problem_idx = problem_idx
self.prev_object_picked = None
def main(argv):
try:
opts, args = getopt.getopt(argv, 'h:v', ['help', 'viewer'])
except getopt.GetoptError as err:
print str(err)
print HELP_MESSAGE
return
viewer = False
for opt, arg in opts:
if opt in ('-h', '--help'):
print HELP_MESSAGE
return
elif opt in ('-v', '--viewer'):
viewer = True
env = Environment()
try:
execute = lambda: script(env)
if viewer: execute_viewer(env, execute)
else: execute()
finally:
if env.GetViewer() is not None:
env.GetViewer().quitmainloop()
RaveDestroy()
def test_pr2(self):
pr2_robot = robots.PR2()
env = Environment()
# env.SetViewer("qtcoin")
robot = env.ReadRobotXMLFile(pr2_robot.shape)
env.Add(robot)
dof_val = robot.GetActiveDOFValues()
init_dof = np.zeros((39,))
self.assertTrue(np.allclose(dof_val, init_dof, 1e-6))
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 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
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 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 main(argv):
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
env = Environment()
try:
execute = lambda: solve_tamp(env)
if args.viewer: execute_viewer(env, execute)
else: execute()
finally:
if env.GetViewer() is not None:
env.GetViewer().quitmainloop()
RaveDestroy()
print 'Done!'
def test_add_obstacle(self):
attrs = {
"geom": [],
"pose": [(3, 5)],
"_type": ["Obstacle"],
"name": ["obstacle"]
}
attr_types = {
"geom": Obstacle,
"pose": Vector2d,
"_type": str,
"name": str
}
obstacle = parameter.Object(attrs, attr_types)
env = Environment()
"""
to ensure the _add_obstacle is working, uncomment the line below to make
sure the obstacle is being created
"""
# env.SetViewer('qtcoin')
obstacle_body = OpenRAVEBody(env, obstacle.name, obstacle.geom)
obstacle_body.set_pose([2, 0])
arr = np.eye(4)
arr[0, 3] = 2
self.assertTrue(np.allclose(obstacle_body.env_body.GetTransform(),
arr))
def main(_):
parser = argparse.ArgumentParser()
parser.add_argument('-focus',
action='store_true',
help='use focused algorithm.')
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
env = Environment()
try:
execute = lambda: solve_tamp(env, args.focus)
if args.viewer: execute_viewer(env, execute)
else: execute()
finally:
if env.GetViewer() is not None:
env.GetViewer().quitmainloop()
RaveDestroy()
print 'Done!'
def test_rrt_planner(self):
# Adopting examples from openrave
domain, problem, params = load_environment(
'../domains/baxter_domain/baxter.domain',
'../domains/baxter_domain/baxter_probs/grasp_1234_1.prob')
env = Environment() # create openrave environment
objLst = [i[1] for i in params.items() if not i[1].is_symbol()]
view = OpenRAVEViewer.create_viewer(env)
view.draw(objLst, 0, 0.7)
can_body = view.name_to_rave_body["can0"]
baxter_body = view.name_to_rave_body["baxter"]
can = can_body.env_body
robot = baxter_body.env_body
dof = robot.GetActiveDOFValues()
inds = baxter_body._geom.dof_map['rArmPose']
r_init = params['robot_init_pose']
r_end = params['robot_end_pose']
dof[inds] = r_init.rArmPose.flatten()
robot.SetActiveDOFValues(dof)
robot.SetActiveDOFs(inds) # set joints the first 4 dofs
plan_params = Planner.PlannerParameters()
plan_params.SetRobotActiveJoints(robot)
plan_params.SetGoalConfig(
[0.7, -0.204, 0.862, 1.217, 2.731, 0.665,
2.598]) # set goal to all ones
# forces parabolic planning with 40 iterations
traj = RaveCreateTrajectory(env, '')
# Using openrave built in planner
trajectory = {}
plan_params.SetExtraParameters(
""" <_postprocessing planner="parabolicsmoother">
<_nmaxiterations>17</_nmaxiterations>
</_postprocessing>""")
trajectory["BiRRT"] = planing(env, robot, plan_params, traj,
'BiRRT') # 3.5s
# trajectory["BasicRRT"] = planing(env, robot, plan_params, traj, 'BasicRRT') # 0.05s can't run it by its own
# trajectory["ExplorationRRT"] = planing(env, robot, plan_params, traj, 'ExplorationRRT') # 0.03s
# plan_params.SetExtraParameters('<range>0.2</range>')
# Using OMPL planner
# trajectory["OMPL_RRTConnect"] = planing(env, robot, plan_params, traj, 'OMPL_RRTConnect') # 1.5s
# trajectory["OMPL_RRT"] = planing(env, robot, plan_params, traj, 'OMPL_RRT') # 10s
# trajectory["OMPL_RRTstar"] = planing(env, robot, plan_params, traj, 'OMPL_RRTstar') # 10s
# trajectory["OMPL_TRRT"] = planing(env, robot, plan_params, traj, 'OMPL_TRRT') # 10s
# trajectory["OMPL_pRRT"] = planing(env, robot, plan_params, traj, 'OMPL_pRRT') # Having issue, freeze
# trajectory["OMPL_LazyRRT"] = planing(env, robot, plan_params, traj, 'OMPL_LazyRRT') # 1.5s - 10s unsatble
# ompl_traj = trajectory["OMPL_RRTConnect"]
or_traj = trajectory["BiRRT"]
result = process_traj(or_traj, 20)
self.assertTrue(result.shape[1] == 20)
def test_basic(self):
pr2_robot = pr2.PR2()
env = Environment()
robot = env.ReadRobotXMLFile(pr2_robot.shape)
env.Add(robot)
dof_val = robot.GetActiveDOFValues()
init_dof = np.array([
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 2.77555756e-17,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 2.77555756e-17,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00
])
self.assertTrue(np.allclose(dof_val, init_dof, 1e-6))
def get_custom_ir(robot):
try:
if FORCE_CREATE_IR: raise IOError
ir_database = load_custom_ir(robot)
except IOError:
ir_database = create_custom_ir(robot, pap_ir_samples(Environment(
))) # TODO - temporarily set viewer to the new Environment?
#ir_database = create_custom_ir(robot, push_ir_database(Environment())) # TODO - options for doing push etc
save_custom_ir(
robot,
ir_database) # TODO - should hypothetically pass the new robot in
return ir_database
def test_obstructs(self):
#Obstructs, Robot, RobotPose, Can;
radius = 1
attrs = {
"geom": [radius],
"pose": [(0, 0)],
"_type": ["Robot"],
"name": ["robot"]
}
attr_types = {
"geom": circle.GreenCircle,
"pose": Vector2d,
"_type": str,
"name": str
}
robot = parameter.Object(attrs, attr_types)
attrs = {"value": [(0, 0)], "_type": ["RobotPose"], "name": ["r_pose"]}
attr_types = {"value": Vector2d, "_type": str, "name": str}
robotPose = parameter.Symbol(attrs, attr_types)
attrs = {
"geom": [radius],
"pose": [(0, 0)],
"_type": ["Can"],
"name": ["can1"]
}
attr_types = {
"geom": circle.BlueCircle,
"pose": Vector2d,
"_type": str,
"name": str
}
can = parameter.Object(attrs, attr_types)
env = Environment()
pred = namo_predicates.Obstructs(
"obstructs", [robot, robotPose, robotPose, can],
["Robot", "RobotPose", "RobotPose", "Can"], env)
pred.expr.expr.grad(np.array([1.9, 0, 0, 0]), True, 1e-2)
# val, jac = pred.distance_from_obj(np.array([1.9,0,0,0]))
# self.assertTrue(np.allclose(np.array(val), .11, atol=1e-2))
# jac2 = np.array([[-0.95968306, -0., 0.95968306, 0.]])
# self.assertTrue(np.allclose(jac, jac2, atol=1e-2))
robot.pose = np.zeros((2, 4))
can.pose = np.array(
[[2 * (radius + pred.dsafe), 0, 0, 2 * radius - pred.dsafe],
[0, 2 * (radius + pred.dsafe), 0, 0]])
self.assertFalse(pred.test(time=0))
self.assertFalse(pred.test(time=1))
self.assertTrue(pred.test(time=2))
self.assertTrue(pred.test(time=3))
def _build_plan(self, data):
print "Building plan."
env = Environment()
params = {}
for param in data.parameters:
new_param = self._build_param(param)
params[new_param.name] = new_param
actions = []
for action in data.actions:
actions.append(self._build_action(action, params, env))
return Plan(params, actions, data.horizon, env)
def main(or_viewer=False, hpn_viewer=False):
#exp = Experiment({'tableIkea1' : (hu.Pose(1.3, 0.0, 0.0, math.pi/2.0), TINY_VAR)},
# {'objA' : (hu.Pose(1.1, 0.0, ikZ, 0.0), TINY_VAR)},
# ['tableIkea1Top', 'tableIkea1Left'])
#PlanTest(exp)
#raw_input('Here') # NOTE - cannot run both viewers
assert not or_viewer or not hpn_viewer
print 'Window:', not debug('noW')
env = Environment() # NOTE - need this for load_problem
#execute = lambda: solve_belief(env, no_window=not hpn_viewer)
execute = lambda: solve_deterministic(env, no_window=not hpn_viewer)
#execute = lambda: openrave_test(env)
#execute = lambda: grasp_test(env, no_window=not hpn_viewer)
try:
if or_viewer: execute_viewer(env, execute)
else: execute()
finally:
if env.GetViewer() is not None:
env.GetViewer().quitmainloop()
RaveDestroy()
def main(argv):
HELP_MESSAGE = 'python generate_problem.py -p <problem>'
try:
opts, args = getopt.getopt(argv, 'hp:v',
['help', 'problem=', 'visualize'])
except getopt.GetoptError:
print HELP_MESSAGE
return
problem = None
visualize = False
for opt, arg in opts:
if opt in ('-h', '--help'):
print HELP_MESSAGE
return
elif opt in ('-p', '--problem'):
problem = arg
elif opt in ('-v', '--visualize'):
visualize = not visualize
if problem is None:
print HELP_MESSAGE
return
if not hasattr(generate, problem):
print problem, 'is not a valid problem'
return
env = Environment()
try:
execute = lambda: save(env, getattr(generate, problem))
if visualize: execute_viewer(env, execute)
else: execute()
finally:
if env.GetViewer() is not None:
env.GetViewer().quitmainloop()
RaveDestroy()
def main(argv):
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('--problem', help='problem name.', default='simple')
parser.add_argument('-fd', action='store_true', help='FastDownward (a dummy flag).')
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
parser.add_argument('-display', action='store_true', help='display solution.')
parser.add_argument('-focus', action='store_true', help='focused.')
parser.add_argument('-movie', action='store_true', help='record a movie.')
args = parser.parse_args()
if args.problem not in list_problems():
print args.problem, 'is not a valid problem'
return
env = Environment() # NOTE - need this for load_problem
problem = load_problem(args.problem)
try:
execute = lambda: solve_tamp(env, problem, args.display, incremental=not args.focus, focused=args.focus, movie=args.movie)
if args.viewer: execute_viewer(env, execute)
else: execute()
finally:
if env.GetViewer() is not None:
env.GetViewer().quitmainloop()
RaveDestroy()
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 _build_plan(self, group):
env = Environment()
params = {}
for param in group['params'].values():
new_param = self._build_param(param)
params[new_param.name] = new_param
actions = []
for action in group['actions'].values():
actions.append(self._build_action(action, params, env))
return Plan(params,
actions,
group['horizon'].value,
env,
determine_free=False)
def test_add_circle(self):
attrs = {
"geom": [1],
"pose": [(3, 5)],
"_type": ["Can"],
"name": ["can0"]
}
attr_types = {
"geom": circle.GreenCircle,
"pose": Vector2d,
"_type": str,
"name": str
}
green_can = parameter.Object(attrs, attr_types)
attrs = {
"geom": [1],
"pose": [(3, 5)],
"_type": ["Can"],
"name": ["can0"]
}
attr_types = {
"geom": circle.BlueCircle,
"pose": Vector2d,
"_type": str,
"name": str
}
blue_can = parameter.Object(attrs, attr_types)
env = Environment()
"""
to ensure the _add_circle and create_cylinder is working, uncomment the
line below to make sure cylinders are being created
"""
# env.SetViewer('qtcoin')
green_body = OpenRAVEBody(env, "can0", green_can.geom)
blue_body = OpenRAVEBody(env, "can1", blue_can.geom)
green_body.set_pose([2, 0])
arr = np.eye(4)
arr[0, 3] = 2
self.assertTrue(np.allclose(green_body.env_body.GetTransform(), arr))
blue_body.set_pose(np.array([0, -1]))
arr = np.eye(4)
arr[1, 3] = -1
self.assertTrue(np.allclose(blue_body.env_body.GetTransform(), arr))
def test_exception(self):
env = Environment()
attrs = {
"geom": [],
"pose": [(3, 5)],
"_type": ["Can"],
"name": ["can0"]
}
attr_types = {
"geom": DummyGeom,
"pose": Vector2d,
"_type": str,
"name": str
}
green_can = parameter.Object(attrs, attr_types)
with self.assertRaises(OpenRAVEException) as cm:
green_body = OpenRAVEBody(env, "can0", green_can.geom)
self.assertTrue("Geometry not supported" in cm.exception.message)
def test_collides(self):
env = Environment()
attrs = {
"geom": [1],
"pose": [(0, 0)],
"_type": ["Can"],
"name": ["can1"]
}
attr_types = {
"geom": circle.BlueCircle,
"pose": Vector2d,
"_type": str,
"name": str
}
can = parameter.Object(attrs, attr_types)
attrs = {
"geom": ["closet"],
"pose": [(0, 0)],
"_type": ["Obstacle"],
"name": ["wall"]
}
attr_types = {
"geom": wall.Wall,
"pose": Vector2d,
"_type": str,
"name": str
}
border = parameter.Object(attrs, attr_types)
pred = namo_predicates.Collides("collides", [can, border],
["Can", "Obstacle"], env)
self.assertTrue(pred.test(0))
border.pose = np.zeros((2, 4))
can.pose = np.array(
[[1 + pred.dsafe, 1 + 2 * pred.dsafe, 1, 1 - pred.dsafe],
[0, 0, 0, 0]])
self.assertTrue(pred.test(0))
self.assertFalse(pred.test(1))
self.assertTrue(pred.test(2))
self.assertTrue(pred.test(3))
"""
def test_can_world(self):
domain, problem, params = load_environment(
'../domains/baxter_domain/baxter.domain',
'../domains/baxter_domain/baxter_training_probs/grasp_training_4321_0.prob'
)
geom = params['can0'].geom
params['can0'].geom = GreenCan(geom.radius, geom.height)
env = Environment()
objLst = [i[1] for i in params.items() if not i[1].is_symbol()]
view = OpenRAVEViewer(env)
view.draw(objLst, 0, 0.7)
can_body = view.name_to_rave_body["can0"]
baxter_body = view.name_to_rave_body["baxter"]
can = can_body.env_body
robot = baxter_body.env_body
dof = robot.GetActiveDOFValues()
targ = params['target1']
# import ipdb; ipdb.set_trace()
can_body.set_pose(targ.value, targ.rotation)
def test_grad(self):
radius = 1
attrs = {
"geom": [radius],
"pose": [(0, 0)],
"_type": ["Robot"],
"name": ["robot"]
}
attr_types = {
"geom": circle.GreenCircle,
"pose": Vector2d,
"_type": str,
"name": str
}
robot = parameter.Object(attrs, attr_types)
attrs = {"value": [(0, 0)], "_type": ["RobotPose"], "name": ["r_pose"]}
attr_types = {"value": Vector2d, "_type": str, "name": str}
robotPose = parameter.Symbol(attrs, attr_types)
attrs = {
"geom": [radius],
"pose": [(0, 0)],
"_type": ["Can"],
"name": ["can1"]
}
attr_types = {
"geom": circle.BlueCircle,
"pose": Vector2d,
"_type": str,
"name": str
}
can = parameter.Object(attrs, attr_types)
env = Environment()
pred = namo_predicates.Obstructs(
"obstructs", [robot, robotPose, robotPose, can],
["Robot", "RobotPose", "RobotPose", "Can"], env)
pred.expr.expr.grad(np.array([1.9, 0, 0, 0]), True, 1e-2)
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))
def test_obstructs_holding(self):
# ObstructsHolding, Robot, RobotPose, Can, Can;
radius = 1
attrs = {
"geom": [radius],
"pose": [(0, 0)],
"_type": ["Robot"],
"name": ["pr2"]
}
attr_types = {
"geom": circle.GreenCircle,
"pose": Vector2d,
"_type": str,
"name": str
}
robot = parameter.Object(attrs, attr_types)
attrs = {"value": [(0, 0)], "_type": ["RobotPose"], "name": ["r_pose"]}
attr_types = {"value": Vector2d, "_type": str, "name": str}
robotPose = parameter.Symbol(attrs, attr_types)
attrs = {
"geom": [radius],
"pose": [(0, 0)],
"_type": ["Can"],
"name": ["can1"]
}
attr_types = {
"geom": circle.BlueCircle,
"pose": Vector2d,
"_type": str,
"name": str
}
can1 = parameter.Object(attrs, attr_types)
attrs = {
"geom": [radius],
"pose": [(0, 2)],
"_type": ["Can"],
"name": ["can2"]
}
attr_types = {
"geom": circle.BlueCircle,
"pose": Vector2d,
"_type": str,
"name": str
}
can2 = parameter.Object(attrs, attr_types)
env = Environment()
pred = namo_predicates.ObstructsHolding(
"ObstructsHolding", [robot, robotPose, robotPose, can1, can2],
["Robot", "RobotPose", "RobotPose", "Can", "Can"], env)
#First test should fail because all objects's positions are in (0,0)
self.assertTrue(pred.test(time=0))
# This predicate has two expressions
# pred.expr.expr.grad(np.array([1.9,0,0,0,0,0]), True, 1e-2)
robot.pose = np.zeros((2, 4))
can1.pose = np.array(
[[2 * (radius + pred.dsafe) + 0.1, 0, .1, 2 * radius - pred.dsafe],
[0, 2 * (radius + pred.dsafe) + 0.1, 0, 0]])
can2.pose = np.zeros((2, 4))
self.assertFalse(pred.test(time=0))
self.assertFalse(pred.test(time=1))
self.assertTrue(pred.test(time=2))
self.assertTrue(pred.test(time=3))
pred2 = namo_predicates.ObstructsHolding(
"obstruct holding2", [robot, robotPose, robotPose, can2, can2],
["Robot", "RobotPose", "RobotPose", "Can", "Can"], env)
# since the object holding, object obstructing and robot are at the same position, can2 obstructs robot
self.assertTrue(pred2.test(0))
can2.pose = np.array([[
2 * radius, 2 * radius + pred2.dsafe, 2 * radius - pred2.dsafe,
3 * radius
], [0, 0, 0, 0]])
# At timestep 0, touching is allowed, so not obstruct
self.assertFalse(pred2.test(time=0))
# At timestep 1, with a distance of dsafe is also allowed, also not obstruct
self.assertFalse(pred2.test(time=1))
# At timestep 2, there is intersect distance of dsafe, it obstructs
self.assertTrue(pred2.test(time=2))
# At timestep 4, objects are far away from robot, thus not obstructs
self.assertFalse(pred2.test(time=3))
# Test whether negation is consistent
self.assertTrue(pred2.test(time=0, negated=True))
self.assertTrue(pred2.test(time=1, negated=True))
self.assertFalse(pred2.test(time=2, negated=True))
self.assertTrue(pred2.test(time=3, negated=True))
def test_in_contact(self):
# InContact, Robot, RobotPose, Target
radius = 1
attrs = {
"geom": [radius],
"pose": [(0, 0)],
"_type": ["Robot"],
"name": ["pr2"]
}
attr_types = {
"geom": circle.GreenCircle,
"pose": Vector2d,
"_type": str,
"name": str
}
robot = parameter.Object(attrs, attr_types)
attrs = {"value": [(0, 0)], "_type": ["RobotPose"], "name": ["r_pose"]}
attr_types = {"value": Vector2d, "_type": str, "name": str}
robotPose = parameter.Symbol(attrs, attr_types)
attrs = {
"geom": [radius],
"value": [(0, 0)],
"_type": ["Target"],
"name": ["target"]
}
attr_types = {
"geom": circle.BlueCircle,
"value": Vector2d,
"_type": str,
"name": str
}
target = parameter.Symbol(attrs, attr_types)
env = Environment()
pred = namo_predicates.InContact("InContact",
[robot, robotPose, target],
["Robot", "RobotPose", "Target"],
env=env)
#First test should fail because all objects's positions are in (0,0)
self.assertFalse(pred.test(time=0))
# Test Gradient for it
pred.expr.expr.grad(np.array([1.9, 0, 0, 0]), True, 1e-2)
val, jac = pred.distance_from_obj(np.array([1.9, 0, 0, 0]))
# self.assertTrue(np.allclose(np.array(val), .11, atol=1e-2))
jac2 = np.array([[-0.95968306, -0., 0.95968306, 0.]])
# self.assertTrue(np.allclose(jac, jac2, atol=1e-2))
robotPose.value = np.zeros((2, 4))
target.value = np.array(
[[2 * radius, radius, 2 * radius, 2 * radius - pred.dsafe, 0],
[0, 0, 0, 0, 0]])
self.assertTrue(pred.test(time=0))
self.assertTrue(pred.test(time=1))
self.assertTrue(pred.test(time=2))
self.assertTrue(pred.test(time=3))
self.assertTrue(pred.test(time=4))
# since it symbol are assumed to be unchanged, test should always check distance with first traj vector
robotPose.value = np.array([[
-pred.dsafe, 3 * radius + pred.dsafe, 0, -pred.dsafe,
2 * radius + pred.dsafe
], [0, 0, 0, 0, 0]])
self.assertFalse(pred.test(time=0))
self.assertFalse(pred.test(time=1))
self.assertFalse(pred.test(time=2))
self.assertFalse(pred.test(time=3))
self.assertFalse(pred.test(time=4))
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