def cylinder_collision(oracle, body_name1, body_name2):
# TODO
# - ODE cylinder-cylinder collision doesn't seem to work reliably. But PQP does work. Set the PQP just for cylinders
# - Incorporate cylinder1.GetTransform() for local point
# - Just use the aabb
# - Only considers case where cylinders are resting on their base
body1, body2 = oracle.get_body(body_name1), oracle.get_body(body_name2)
cylinder1, cylinder2 = oracle.get_geometries(
body_name1)[0], oracle.get_geometries(body_name2)[0]
if is_upright(get_trans(body1)) and is_upright(get_trans(body2)):
point1, point2 = get_point(body1), get_point(body2)
return abs(point1[2] - point2[2]) < (cylinder1.GetCylinderHeight() + cylinder2.GetCylinderHeight())/2. and \
length(point1[:2] - point2[:2]) < cylinder1.GetCylinderRadius() + cylinder2.GetCylinderRadius()
return oracle.env.CheckCollision(body1, body2)
def display_custom_ir(oracle, manip_trans):
load_ir_database(oracle)
handles = []
default_trans = get_trans(oracle.robot)
for transform in oracle.ir_database:
base_trans = ir_base_trans(oracle.robot, manip_trans, transform,
default_trans)
handles.append(draw_point(oracle.env, point_from_trans(base_trans)))
return handles
def grasp_env_cfree(mt, g):
enable_all(False) # TODO - base config?
body1.Enable(True)
for conf in mt.path():
set_manipulator_values(manipulator, conf) # NOTE - can also grab
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1):
return False
return True
def cfree_mtraj_grasp(mt, g):
enable_all(False)
body1.Enable(True)
for conf in mt.path():
set_manipulator_values(manipulator, conf) # NOTE - can also grab
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1):
print 'cfree_mtraj_grasp'
return False
return True
def grasp_pose_cfree(mt, g, p):
enable_all(False) # TODO - base config?
body1.Enable(True)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1, body2):
return False
return True
def get_values(self, universe, dependent_atoms=set(), **kwargs):
mq1, mq2 = map(get_value, self.inputs)
collision_atoms = filter(
lambda atom: atom.predicate in
[CFreeMTrajGrasp, CFreeMTrajPose], dependent_atoms)
collision_params = {atom: atom.args[0] for atom in collision_atoms}
grasp = None
for atom in collision_atoms:
if atom.predicate is CFreeMTrajGrasp:
assert grasp is None # Can't have two grasps
_, grasp = map(get_value, atom.args)
placed = []
for atom in collision_atoms:
if atom.predicate is CFreeMTrajPose:
_, pose = map(get_value, atom.args)
placed.append(pose)
#placed, grasp = [], None
#print grasp, placed
if placed or grasp:
assert len(placed) <= len(all_bodies)
enable_all(False)
for b, p in zip(all_bodies, placed):
b.Enable(True)
set_pose(b, p.value)
if grasp:
assert grasp is None or len(placed) <= len(all_bodies) - 1
set_pose(
body1,
object_trans_from_manip_trans(get_trans(manipulator),
grasp.grasp_trans))
robot.Grab(body1)
set_manipulator_values(manipulator, mq1.value)
mt = motion_plan(env, cspace, mq2.value, self_collisions=True)
if grasp: robot.Release(body1)
if mt:
self.enumerated = True # NOTE - if satisfies all constraints then won't need another. Not true. What if called with different grasps...
# TODO - could always hash this trajectory for the current set of constraints
bound_collision_atoms = [
atom.instantiate({collision_params[atom]: MTRAJ(mt)})
for atom in collision_atoms
]
#bound_collision_atoms = []
return [ManipMotion(mq1, mq2, mt)] + bound_collision_atoms
raise ValueError()
enable_all(False)
set_manipulator_values(manipulator, mq1.value)
mt = motion_plan(env, cspace, mq2.value, self_collisions=True)
if mt:
return [ManipMotion(mq1, mq2, mt)]
return []
def plan_base_traj(oracle, start_config, end_config, holding=None, obstacle_poses={}):
with oracle.state_saver():
oracle.set_all_object_poses(obstacle_poses)
oracle.set_robot_config(end_config)
base_trans = get_trans(oracle.robot)
oracle.set_robot_config(start_config)
open_gripper(oracle)
if holding is not None:
oracle.set_pose(holding.object_name, object_pose_from_robot_config(oracle, start_config, holding.grasp))
grab(oracle, holding.object_name)
traj = cspace_traj(oracle, CSpace.robot_base(oracle.robot), base_values_from_trans(base_trans))
if holding is not None:
release(oracle, holding.object_name)
return traj
def pap_ir_statistics(env, trials=100):
from manipulation.inverse_reachability.inverse_reachability import display_custom_ir, load_ir_database, \
ir_base_trans, forward_transform, manip_base_values, is_possible_fr_trans, is_possible_ir_trans
from manipulation.pick_and_place import PickAndPlace
oracle = pap_ir_oracle(env)
body_name = oracle.objects[0]
table_name = oracle.tables[0]
convert_point = lambda p: np.concatenate([p[:2], [1.]])
"""
load_ir_database(oracle)
print oracle.ir_aabb
print aabb_min(oracle.ir_aabb), aabb_max(oracle.ir_aabb)
handles = []
vertices = xy_points_from_aabb(oracle.ir_aabb)
print vertices
print np.min(oracle.ir_database, axis=0), np.max(oracle.ir_database, axis=0)
for v in vertices:
handles.append(draw_point(oracle.env, convert_point(v)))
for v1, v2 in zip(vertices, vertices[1:] + vertices[-1:]):
handles.append(draw_line(oracle.env, convert_point(v1), convert_point(v2)))
raw_input('Start?')
"""
successes = []
for pose in take(random_region_placements(oracle, body_name, [table_name]),
trials):
grasp = choice(get_grasps(oracle, body_name))
pap = PickAndPlace(None, pose, grasp)
oracle.set_pose(body_name, pap.pose)
#if pap.sample(oracle, body_name, max_failures=50, base_iterator_fn=openrave_base_iterator, check_base=False):
if pap.sample(oracle, body_name, max_failures=50, check_base=False):
oracle.set_robot_config(pap.grasp_config)
successes.append(int(pap.iterations))
handles = display_custom_ir(oracle, pap.manip_trans)
default_trans = get_trans(oracle.robot)
#vertices = xy_points_from_aabb(aabb_apply_trans(oracle.ir_aabb, pap.manip_trans))
vertices = [
point_from_trans(
ir_base_trans(oracle.robot, pap.manip_trans, v, default_trans))
for v in xy_points_from_aabb(oracle.ir_aabb)
]
for v1, v2 in zip(vertices, vertices[1:] + vertices[-1:]):
handles.append(
draw_line(oracle.env, convert_point(v1), convert_point(v2)))
point_from_inverse = lambda trans: point_from_trans(
np.dot(pap.base_trans, forward_transform(trans)))
for trans in oracle.ir_database:
handles.append(
draw_point(oracle.env,
convert_point(point_from_inverse(trans)),
color=(0, 1, 0, .5)))
#vertices = [point_from_inverse(v) for v in xy_points_from_aabb(oracle.ir_aabb)] # Doesn't have the angle in it...
vertices = [
point_from_trans(np.dot(pap.base_trans, trans_from_base_values(v)))
for v in xy_points_from_aabb(oracle.fr_aabb)
]
for v1, v2 in zip(vertices, vertices[1:] + vertices[-1:]):
handles.append(
draw_line(oracle.env,
convert_point(v1),
convert_point(v2),
color=(0, 1, 0, .5)))
assert is_possible_ir_trans(oracle, pap.manip_trans, pap.base_trans) and \
is_possible_fr_trans(oracle, pap.base_trans, pap.manip_trans)
raw_input('Continue?')
return float(
len(successes)) / trials, np.mean(successes), np.std(successes)
def get_base_generator(robot, ir_database, manip_iterator):
default_trans = get_trans(robot)
for manip_trans in cycle(manip_iterator):
yield ir_base_trans(robot, manip_trans, choice(ir_database),
default_trans), manip_trans
def manip_point(q):
if isinstance(q, Config): q = q.value
robot = get_env().GetRobots()[0]
with robot:
robot.SetActiveDOFValues(q)
return point_from_trans(get_trans(robot.GetActiveManipulator()))
