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)