def sample_edge_point(polygon, radius):
from misc.numerical import sample_categorical
edges = zip(polygon, polygon[-1:] + polygon[:-1])
edge_weights = {
i: max(length(v2 - v1) - 2 * radius, 0)
for i, (v1, v2) in enumerate(edges)
}
# TODO: fail if no options
while True:
index = sample_categorical(edge_weights)
v1, v2 = edges[index]
t = random.uniform(radius, length(v2 - v1) - 2 * radius)
yield t * normalize(v2 - v1) + v1
def get_closest_edge_point(polygon, point):
edges = zip(polygon, polygon[-1:] + polygon[:-1])
best = None
for v1, v2 in edges:
proj = (v2 - v1)[:2].dot((point - v1)[:2])
if proj <= 0:
closest = v1
elif length((v2 - v1)[:2]) <= proj:
closest = v2
else:
closest = proj * normalize((v2 - v1))
if (best is None) or (length((point - closest)[:2]) < length(
(point - best)[:2])):
best = closest
return best
def workspace_motion_plan(
base_manip,
manipulator,
vector,
steps=10): # TODO - use IK to check if even possibly valid
distance, direction = length(vector), normalize(vector)
step_length = distance / steps
with base_manip.robot:
base_manip.robot.SetActiveManipulator(manipulator)
base_manip.robot.SetActiveDOFs(manipulator.GetArmIndices())
with collision_saver(base_manip.robot.GetEnv(),
openravepy_int.CollisionOptions.ActiveDOFs):
try: # TODO - Bug in specifying minsteps. Need to specify at least 2 times the desired otherwise it stops early
traj = base_manip.MoveHandStraight(direction,
minsteps=10 * steps,
maxsteps=steps,
steplength=step_length,
ignorefirstcollision=None,
starteematrix=None,
greedysearch=True,
execute=False,
outputtraj=None,
maxdeviationangle=None,
planner=None,
outputtrajobj=True)
return list(sample_manipulator_trajectory(manipulator, traj))
except planning_error:
return None
def sample_edge_pose(surface, mesh):
world_from_surface = trans_from_pose(surface.pose)
radius = max(length(v[:2]) for v in mesh.vertices)
origin_from_base = trans_from_point(0, 0, np.min(mesh.vertices[:, 2]))
for point in sample_edge_point(surface.convex_hull, radius):
theta = random.uniform(0, 2 * np.pi)
surface_from_origin = trans_from_quat_point(quat_from_z_rot(theta),
point)
yield pose_from_trans(
world_from_surface.dot(surface_from_origin).dot(origin_from_base))
def update_belief(robot, world, belief, plan):
# TODO: before or after movement?
holding, surfaces, items = belief.holding, belief.surfaces, belief.items
surface_types = {s.type: s for s in world.surfaces}
item_types = {i.type: i for i in world.items}
head = robot.GetManipulator('head')
for action, args in plan:
if action.name == 'scan_room':
# TODO: observe objects under some conditions
surfaces = world.surfaces[:]
elif action.name == 'move_head':
#items = world.items[:]
# TODO: should this be a sense action?
pass
elif action.name == 'move_base':
items = [Object(ty, p, False) for ty, p, _ in items]
elif action.name == 'register':
pass
elif action.name == 'pick':
if holding is not None: print holding
assert holding is None
_, p, g, bg, _ = args # TODO: use g.value
item = items.pop(get_pose_index(items, p.value))
holding = Object(item.type, g, False)
elif action.name == 'place':
assert holding is not None
_, p, g, bg, _ = args
items.append(Object(holding.type, p.value, False))
holding = None
else:
raise ValueError(action.name)
# TODO: add noise to these?
#for name, pose in observe_env(robot, p_hit_vis=0.25).items():
visible = observe_env(robot, p_hit_vis=1)
print 'Visible:', visible.keys()
for name, pose in visible.items():
ty, _ = name.split('-')
if ty in item_types:
# TODO: probabilities for detect and register
for index, item in reversed(list(enumerate(items))):
if ty == item.type:
point = point_from_pose(item.pose) if is_oriented(item.pose) else item.pose
if (point is None) or np.allclose(point_from_pose(pose), point):
items.pop(index)
#index = get_point_index(items, pose)
distance = length(get_point(head)[:2] - point_from_pose(pose)[:2])
pose = pose if (distance <= MAX_REG_DISTANCE) else point_from_pose(pose)
items.append(Object(ty, pose, is_oriented(pose)))
elif ty in surface_types:
[matched] = [s for s in surfaces if (s.type == ty) and (s.pose is not None) and
np.allclose(s.pose, pose)]
matched.observations += 1
return Belief(holding, surfaces, items)
def workspace_motion_plan(base_manip, manipulator, vector, steps=10):
distance, direction = length(vector), normalize(vector)
step_length = distance / steps
with base_manip.robot:
base_manip.robot.SetActiveManipulator(manipulator)
base_manip.robot.SetActiveDOFs(manipulator.GetArmIndices())
with collision_saver(base_manip.robot.GetEnv(),
openravepy_int.CollisionOptions.ActiveDOFs):
try:
traj = base_manip.MoveHandStraight(direction,
minsteps=10 * steps,
maxsteps=steps,
steplength=step_length,
ignorefirstcollision=None,
starteematrix=None,
greedysearch=True,
execute=False,
outputtraj=None,
maxdeviationangle=None,
planner=None,
outputtrajobj=True)
return list(sample_manipulator_trajectory(manipulator, traj))
except planning_error:
return None
def get_mesh_radius(body):
[geometry] = get_geometries(body)
mesh = geometry.GetCollisionMesh()
return max(length(v[:2]) for v in mesh.vertices)
def sample_grasp_traj(obj, pose, pose2):
enable_all(bodies, False)
body = bodies[obj]
#body.Enable(True) # TODO: fix this
set_pose(body, pose.value)
# TODO: check if on the same surface
point = point_from_pose(pose.value)
point2 = point_from_pose(pose2.value)
distance = length(point2 - point)
if (distance <= 0.0) or (0.6 <= distance):
return
direction = normalize(point2 - point)
orientation = quat_from_angle_vector(angle_from_vector(direction),
[0, 0, 1])
#rotate_direction = trans_from_quat_point(orientation, unit_point())
steps = int(math.ceil(distance / PUSH_MAX_DISTANCE) + 1)
distances = np.linspace(0., distance, steps)
points = [point + d * direction for d in distances]
poses = [pose_from_quat_point(orientation, point) for point in points]
pose_objects = [pose] + map(Pose, poses[1:-1]) + [pose2]
print distance, steps, distances, len(poses)
radius, height = get_mesh_radius(body), get_mesh_height(body)
contact = cylinder_contact(radius, height)
pushes = []
# TODO: I could do all trajectories after all pushes planned
for i in xrange(len(poses) - 1):
p1, p2 = poses[i:i + 2]
# TODO: choose midpoint for base
ir_world_from_gripper = manip_from_pose_grasp(p1, contact)
set_manipulator_conf(arm, carry_arm_conf)
for world_from_base in islice(
random_inverse_reachability(ir_model,
ir_world_from_gripper),
max_failures):
set_trans(robot, world_from_base)
set_manipulator_conf(arm, carry_arm_conf)
if check_collision(robot):
continue
q = Conf(robot.GetConfigurationValues())
push_arm_confs = []
approach_paths = []
for p in (p1, p2):
# TODO: make sure I have the +x push conf
world_from_gripper = manip_from_pose_grasp(p, contact)
set_manipulator_conf(arm, carry_arm_conf)
grasp_arm_conf = solve_inverse_kinematics(
arm, world_from_gripper, collisions=False)
if grasp_arm_conf is None:
break
push_arm_confs.append(grasp_arm_conf)
set_manipulator_conf(arm, grasp_arm_conf)
pregrasp_arm_conf = solve_inverse_kinematics(
arm,
world_from_gripper.dot(gripper_from_pregrasp),
collisions=False)
if pregrasp_arm_conf is None:
break
#if DISABLE_MOTIONS:
if True:
approach_paths.append([
carry_arm_conf, pregrasp_arm_conf, grasp_arm_conf
])
continue
"""
set_manipulator_conf(arm, pregrasp_arm_conf)
grasp_path = plan_straight_path(robot, grasp_arm_conf, pregrasp_arm_conf)
# robot.Release(body)
if grasp_path is None:
continue
pregrasp_path = plan_path(base_manip, pregrasp_arm_conf, carry_arm_conf)
if pregrasp_path is None:
continue
t = Prehensile(full_from_active(robot, grasp_path + pregrasp_path[1:]), obj, pose, grasp)
yield [(q, t)]
reset_env()
return
"""
else:
# Start and end may have different orientations
pq1, pq2 = push_arm_confs
push_path = plan_straight_path(robot, pq1, pq2)
# TODO: make sure the straight path is actually straight
if push_path is None:
continue
po1, po2 = pose_objects[i:i + 2]
ap1, ap2 = approach_paths
m = Push(
full_from_active(robot, ap1),
full_from_active(robot, push_arm_confs),
#full_from_active(robot, push_path),
full_from_active(robot, ap2[::-1]),
obj,
po1,
po2,
contact)
pushes.append(PushMotion(obj, po1, po2, q, m))
break
else:
print 'Failure', len(pushes)
return
print 'Success', len(pushes)
yield pushes