def sample_grasp_traj(pose, grasp):
enable_all(all_bodies, False)
body1.Enable(True)
set_pose(body1, pose.value)
manip_trans = manip_from_pose_grasp(pose.value, grasp.value)
grasp_conf = solve_inverse_kinematics(manipulator, manip_trans)
if grasp_conf is None:
return
if DISABLE_MOTIONS:
yield [(Conf(grasp_conf), Traj([]))]
return
set_manipulator_conf(manipulator, grasp_conf)
robot.Grab(body1)
pregrasp_trans = manip_trans.dot(trans_from_point(*APPROACH_VECTOR))
pregrasp_conf = solve_inverse_kinematics(manipulator, pregrasp_trans)
if pregrasp_conf is None:
return
if has_mp():
path = mp_straight_line(robot, grasp_conf, pregrasp_conf)
else:
path = linear_motion_plan(robot, pregrasp_conf)
robot.Release(body1)
if path is None:
return
grasp_traj = Traj(path)
grasp_traj.pose = pose
grasp_traj.grasp = grasp
yield [(Conf(pregrasp_conf), grasp_traj)]
def sample_holding_motion(conf1, conf2,
grasp): # Sample motion while holding
if DISABLE_MOTIONS:
#traj = Traj([conf1.value, conf2.value])
traj = Traj([conf2.value])
traj.pose = None
traj.grasp = grasp
yield [(traj, )]
return
enable_all(all_bodies, False)
body1.Enable(True)
set_manipulator_conf(manipulator, conf1.value)
manip_trans = manipulator.GetTransform()
set_pose(body1, object_trans_from_manip_trans(manip_trans,
grasp.value))
robot.Grab(body1)
if has_mp():
path = mp_birrt(robot, conf1.value, conf2.value)
else:
#traj = cspace_traj_helper(base_manip, cspace, conf2.value, max_iterations=10)
path = manipulator_motion_plan(base_manip,
manipulator,
conf2.value,
max_iterations=10)
robot.Release(body1)
if path is None:
return
traj = Traj(path)
traj.pose = None
traj.grasp = grasp
yield [(traj, )]
def cylinder(env):
from manipulation.bodies.bodies import mesh_cylinder_body
surfaces = initialize_two_tables(env)
fixed_names = map(get_name, env.GetBodies())
surface_map = {s.name: s for s in surfaces}
#objA = mesh_cylinder_body(env, .03, .1, name='objA', color=BLUE)
#objA = mesh_cylinder_body(env, .05, .05, name='objA', color=BLUE)
#objA = mesh_cylinder_body(env, .265/2, .025, name='objA', color=BLUE) # Plate
objA = mesh_cylinder_body(env, .2 / 2, .025, name='objA', color=BLUE)
env.Add(objA)
while True:
pose = surface_map['table1'].sample_placement(objA)
if pose is not None:
set_pose(objA, pose)
break
goal_surfaces = {get_name(objA): 'table2'}
movable_names = filter(lambda name: name not in fixed_names,
map(get_name, env.GetBodies()))
return RealProblem(movable_names=movable_names,
surfaces=surfaces,
goal_surfaces=goal_surfaces)
def visualize_solution(env, initial_conf, initial_poses, robot, bodies, plan):
def _execute_traj(confs):
for j, conf in enumerate(confs):
set_full_config(robot, conf)
sleep(0.02)
set_full_config(robot, initial_conf.value)
for obj, pose in initial_poses.iteritems():
set_pose(bodies[obj], pose.value)
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?' % (i, len(plan)))
if action.name == 'move':
_, _, traj = args
_execute_traj(traj.value)
elif action.name == 'move_holding':
_, _, traj, _, _ = args
_execute_traj(traj.value)
elif action.name == 'pick':
obj, _, _, _, traj = args
_execute_traj(traj.value[::-1])
robot.Grab(bodies[obj])
_execute_traj(traj.value)
elif action.name == 'place':
obj, _, _, _, traj = args
_execute_traj(traj.value[::-1])
robot.Release(bodies[obj])
_execute_traj(traj.value)
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
def visualize_solution(env, problem, initial_conf, robot, manipulator, bodies,
plan):
def _execute_traj(confs):
for j, conf in enumerate(confs):
set_manipulator_conf(manipulator, conf)
sleep(0.05)
#raw_input('%s/%s) Step?'%(j, len(confs)))
# Resets the initial state
set_manipulator_conf(manipulator, initial_conf.value)
for obj, pose in problem.initial_poses.iteritems():
set_pose(bodies[obj], pose.value)
raw_input('Start?')
for i, (action, args) in enumerate(plan):
#raw_input('\n%s/%s) Next?'%(i, len(plan)))
if action.name == 'move':
_, _, traj = args
_execute_traj(traj.value)
elif action.name == 'move_holding':
_, _, traj, _, _ = args
_execute_traj(traj.value)
elif action.name == 'pick':
obj, _, _, _, traj = args
_execute_traj(traj.value[::-1])
robot.Grab(bodies[obj])
_execute_traj(traj.value)
elif action.name == 'place':
obj, _, _, _, traj = args
_execute_traj(traj.value[::-1])
robot.Release(bodies[obj])
_execute_traj(traj.value)
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
def simple(env):
env.Load(os.path.join(ENVIRONMENTS_DIR, '2tables.xml'))
fixed_names = map(get_name, env.GetBodies())
tables = filter(lambda body: 'table' in get_name(body), env.GetBodies())
surfaces = map(compute_surface, tables)
surface_map = {s.name: s for s in surfaces}
robot = env.GetRobots()[0]
set_manipulator_conf(robot.GetManipulator('leftarm'), REST_LEFT_ARM)
open_gripper(robot.GetManipulator('leftarm'))
set_manipulator_conf(robot.GetManipulator('rightarm'),
mirror_arm_config(robot, REST_LEFT_ARM))
close_gripper(robot.GetManipulator('rightarm'))
robot.SetDOFValues([.15], [robot.GetJointIndex('torso_lift_joint')])
set_base_conf(robot, (0, 0, 0))
robot.SetAffineTranslationLimits(*(2 * np.array([[-1, -1, 0], [1, 1, 0]])))
objA = box_body(env, 'objA', .07, .05, .2, color=BLUE)
env.Add(objA)
while True:
pose = surface_map['table1'].sample_placement(objA)
if pose is not None:
set_pose(objA, pose)
break
goal_surfaces = {
get_name(objA): 'table2',
}
movable_names = filter(
lambda name: name not in fixed_names, map(get_name, env.GetBodies()))
return RealProblem(movable_names=movable_names,
surfaces=surfaces, goal_surfaces=goal_surfaces)
def cfree_pose(
pose1, pose2
): # Collision free test between an object at pose1 and an object at pose2
body1.Enable(True)
set_pose(body1, pose1.value)
body2.Enable(True)
set_pose(body2, pose2.value)
return not env.CheckCollision(body1, body2)
def _cfree_traj_pose(traj, pose):
enable_all(all_bodies, False)
body2.Enable(True)
set_pose(body2, pose.value)
for conf in traj.value:
set_manipulator_conf(manipulator, conf)
if env.CheckCollision(robot, body2):
return False
return True
def cfree_pose(obj1, pose1, obj2, pose2):
body1 = bodies[obj1]
body1.Enable(True)
set_pose(body1, pose1.value)
body2 = bodies[obj2]
body2.Enable(True)
set_pose(body2, pose2.value)
return not env.CheckCollision(body1, body2)
def _cfree_traj_pose(
traj, pose
): # Collision free test between a robot executing traj and an object at pose
enable_all(all_bodies, False)
body2.Enable(True)
set_pose(body2, pose.value)
for conf in traj.value:
set_manipulator_conf(manipulator, conf)
if env.CheckCollision(robot, body2):
return False
return True
def _cfree_traj_grasp_pose(traj, grasp, pose):
enable_all(all_bodies, False)
body1.Enable(True)
body2.Enable(True)
set_pose(body2, pose.value)
for conf in traj.value:
set_manipulator_conf(manipulator, conf)
manip_trans = manipulator.GetTransform()
set_pose(body1, object_trans_from_manip_trans(
manip_trans, grasp.value))
if env.CheckCollision(body1, body2):
return False
return True
def _cfree_traj_grasp_pose(
traj, grasp, pose
): # Collision free test between an object held at grasp while executing traj and an object at pose
enable_all(all_bodies, False)
body1.Enable(True)
body2.Enable(True)
set_pose(body2, pose.value)
for conf in traj.value:
set_manipulator_conf(manipulator, conf)
manip_trans = manipulator.GetTransform()
set_pose(body1,
object_trans_from_manip_trans(manip_trans, grasp.value))
if env.CheckCollision(body1, body2):
return False
return True
def sample_holding_motion(conf1, conf2, obj, grasp):
if DISABLE_MOTIONS:
yield [(make_traj([conf2.value], obj=obj, grasp=grasp),)]
return
enable_all(bodies, False)
body = bodies[obj]
body.Enable(True)
set_manipulator_conf(manipulator, conf1.value)
manip_trans = manipulator.GetTransform()
set_pose(body, object_trans_from_manip_trans(manip_trans, grasp.value))
robot.Grab(body)
path = manipulator_motion_plan(
base_manip, manipulator, conf2.value, max_iterations=10)
robot.Release(body)
if path is None:
return
yield [(make_traj(path, obj=obj, grasp=grasp),)]
def sample_grasp_traj(obj, pose, grasp):
enable_all(bodies, False)
body = bodies[obj]
body.Enable(True)
set_pose(body, pose.value)
set_manipulator_conf(manipulator, carry_config)
manip_tform = manip_from_pose_grasp(pose.value, grasp.value)
for base_tform in islice(random_inverse_reachability(ir_model, manip_tform), max_failures):
set_trans(robot, base_tform)
if env.CheckCollision(robot) or robot.CheckSelfCollision():
continue
approach_robot_conf = robot.GetConfigurationValues()
grasp_manip_conf = solve_inverse_kinematics(
manipulator, manip_tform)
if grasp_manip_conf is None:
continue
set_manipulator_conf(manipulator, grasp_manip_conf)
grasp_robot_conf = robot.GetConfigurationValues()
if DISABLE_MOTIONS:
path = [approach_robot_conf,
grasp_robot_conf, approach_robot_conf]
yield [(Conf(approach_robot_conf), make_traj(path, obj, pose, grasp))]
return
robot.Grab(body)
pregrasp_tform = manip_tform.dot(
trans_from_point(*APPROACH_VECTOR))
pregrasp_conf = solve_inverse_kinematics(
manipulator, pregrasp_tform)
if pregrasp_conf is None:
continue
path = linear_motion_plan(robot, pregrasp_conf)
robot.Release(body)
if path is None:
continue
yield [(Conf(approach_robot_conf), make_traj(path, obj, pose, grasp))]
return
def simple(env):
surfaces = initialize_two_tables(env)
fixed_names = map(get_name, env.GetBodies())
surface_map = {s.name: s for s in surfaces}
objA = box_body(env, 'objA', .07, .05, .2, color=BLUE)
env.Add(objA)
while True:
pose = surface_map['table1'].sample_placement(objA)
if pose is not None:
set_pose(objA, pose)
break
goal_surfaces = {get_name(objA): 'table2'}
movable_names = filter(lambda name: name not in fixed_names,
map(get_name, env.GetBodies()))
return RealProblem(movable_names=movable_names,
surfaces=surfaces,
goal_surfaces=goal_surfaces)
def sample_grasp_traj(
pose, grasp
): # Sample pregrasp config and motion plan that performs a grasp
enable_all(all_bodies, False)
body1.Enable(True)
set_pose(body1, pose.value)
manip_trans = manip_from_pose_grasp(pose.value, grasp.value)
grasp_conf = solve_inverse_kinematics(
manipulator, manip_trans) # Grasp configuration
if grasp_conf is None:
return
if DISABLE_MOTIONS:
yield [(Conf(grasp_conf), Traj([]))]
return
set_manipulator_conf(manipulator, grasp_conf)
robot.Grab(body1)
pregrasp_trans = manip_trans.dot(trans_from_point(*APPROACH_VECTOR))
pregrasp_conf = solve_inverse_kinematics(
manipulator, pregrasp_trans) # Pre-grasp configuration
if pregrasp_conf is None:
return
# Trajectory from grasp configuration to pregrasp
if has_mp():
path = mp_straight_line(robot, grasp_conf, pregrasp_conf)
else:
path = linear_motion_plan(robot, pregrasp_conf)
#grasp_traj = vector_traj_helper(env, robot, approach_vector)
#grasp_traj = workspace_traj_helper(base_manip, approach_vector)
robot.Release(body1)
if path is None:
return
grasp_traj = Traj(path)
grasp_traj.pose = pose
grasp_traj.grasp = grasp
yield [(Conf(pregrasp_conf), grasp_traj)]
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
def dantam_distract(env, n_obj):
env.Load(os.path.join(ENVIRONMENTS_DIR, 'empty.xml'))
m, n = 3, 3
side_dim = .07
height_dim = .1
box_dims = (side_dim, side_dim, height_dim)
separation = (side_dim, side_dim)
coordinates = list(product(range(m), range(n)))
assert n_obj <= len(coordinates)
obj_coordinates = sample(coordinates, n_obj)
length = m * (box_dims[0] + separation[0])
width = n * (box_dims[1] + separation[1])
height = .7
table = box_body(env, 'table', length, width, height, color=TAN)
set_pose(table, pose_from_quat_point(unit_quat(), np.array([0, 0, 0])))
env.Add(table)
robot = env.GetRobots()[0]
set_manipulator_conf(robot.GetManipulator('leftarm'), TOP_HOLDING_LEFT_ARM)
open_gripper(robot.GetManipulator('leftarm'))
set_manipulator_conf(robot.GetManipulator('rightarm'),
mirror_arm_config(robot, REST_LEFT_ARM))
close_gripper(robot.GetManipulator('rightarm'))
robot.SetDOFValues([.15], [robot.GetJointIndex('torso_lift_joint')])
set_base_conf(robot, (-.75, .2, -math.pi / 2))
poses = []
z = height + SURFACE_Z_OFFSET
for r in range(m):
row = []
x = -length / 2 + (r + .5) * (box_dims[0] + separation[0])
for c in range(n):
y = -width / 2 + (c + .5) * (box_dims[1] + separation[1])
row.append(Pose(pose_from_quat_point(
unit_quat(), np.array([x, y, z]))))
poses.append(row)
initial_poses = {}
goal_poses = {}
for i, (r, c) in enumerate(obj_coordinates):
row_color = np.zeros(4)
row_color[2 - r] = 1.
if i == 0:
name = 'goal%d-%d' % (r, c)
color = BLUE
goal_poses[name] = poses[m / 2][n / 2]
else:
name = 'block%d-%d' % (r, c)
color = RED
initial_poses[name] = poses[r][c]
obj = box_body(env, name, *box_dims, color=color)
set_pose(obj, poses[r][c].value)
env.Add(obj)
known_poses = list(flatten(poses))
object_names = initial_poses.keys()
known_grasps = list(
map(Grasp, top_grasps(env.GetKinBody(object_names[0]))))
return ManipulationProblem(object_names=object_names, table_names=[table.GetName()],
goal_poses=goal_poses, initial_poses=initial_poses, known_poses=known_poses, known_grasps=known_grasps)
def reset_env(): enable_all(bodies, False) body.Enable(True) set_pose(body, pose.value) set_active_manipulator(robot, arm.GetName())
def stable_test(obj, pose, surface): bodies[obj].Enable(True) set_pose(bodies[obj], pose.value) return surfaces[surface].supports(bodies[obj])
def cfree_pose(pose1, pose2):
body1.Enable(True)
set_pose(body1, pose1.value)
body2.Enable(True)
set_pose(body2, pose2.value)
return not env.CheckCollision(body1, body2)
