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 sample_free_motion(conf1, conf2): # Sample motion while not holding if DISABLE_MOTIONS: #traj = Traj([conf1.value, conf2.value]) traj = Traj([conf2.value]) traj.pose = None traj.grasp = None yield [(traj, )] return enable_all(all_bodies, False) set_manipulator_conf(manipulator, conf1.value) 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) if path is None: return traj = Traj(path) traj.pose = None traj.grasp = None yield [(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 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 load_env(env): viewer = env.GetViewer() if viewer is not None: viewer.SetName('Simulation') viewer.SetCamera(camera_look_at((0, -2.5, 5), look_point=(0, 0, 0)), focalDistance=5.0) viewer.SetSize(640, 480) viewer.Move(0, 0) #robot = env.ReadRobotXMLFile('robots/pr2-beta-static.zae') #robot = env.ReadRobotXMLFile('robots/pr2-beta-sim.robot.xml') or_robot = env.ReadRobotXMLFile('robots/pr2-beta-static.zae') robot = env.ReadRobotXMLFile('robotics/openrave/environments/robot.dae') env.Add(robot) for manipulator in or_robot.GetManipulators(): robot.AddManipulator(manipulator.GetInfo()) for link_name in IGNORE_LINKS: link = robot.GetLink(link_name) link.SetVisible(False) link.Enable(False) set_manipulator_conf(robot.GetManipulator('leftarm'), DEFAULT_LEFT_ARM) open_gripper(robot.GetManipulator('leftarm')) set_manipulator_conf(robot.GetManipulator('rightarm'), REST_RIGHT_ARM) # mirror_arm_config(robot, REST_LEFT_ARM)) #close_gripper(robot.GetManipulator('rightarm')) open_gripper(robot.GetManipulator('rightarm')) robot.SetDOFValues([.15], [robot.GetJointIndex('torso_lift_joint')]) robot.SetAffineTranslationLimits(*(MAX_DISTANCE * np.array([[-1, -1, 0], [1, 1, 0]]))) vel_multi = 0.25 robot.SetDOFVelocityLimits(vel_multi * robot.GetDOFVelocityLimits()) robot.SetAffineTranslationMaxVels(vel_multi * robot.GetAffineTranslationMaxVels()) robot.SetAffineRotationAxisMaxVels(vel_multi * robot.GetAffineRotationAxisMaxVels()) collision_checker = ODE if RaveCreateCollisionChecker(env, FCL) is None else FCL _, arm, base_manip, _ = initialize_openrave( env, 'leftarm', min_delta=.01, collision_checker=collision_checker) print 'Using collision checker', env.GetCollisionChecker() voxels = [] if OCTREE_RESOLUTION is not None: # TODO: add legs of tables extent = 3 * (OCTREE_RESOLUTION, ) centers = [] for z in np.arange(OCTREE_MIN_HEIGHT, OCTREE_MAX_HEIGHT, OCTREE_RESOLUTION): for t in np.arange(-MAX_DISTANCE, MAX_DISTANCE, OCTREE_RESOLUTION): if np.random.random() < 0.9: centers += [(t, MAX_DISTANCE, z), (t, -MAX_DISTANCE, z), (MAX_DISTANCE, t, z), (-MAX_DISTANCE, t, z)] voxels.append((extent, centers)) return robot, Occupancy(voxels, unit_pose())
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 sample_free_motion(conf1, conf2): if DISABLE_MOTIONS: yield [(make_traj([conf2.value]),)] return enable_all(all_bodies, False) set_manipulator_conf(manipulator, conf1.value) path = manipulator_motion_plan( base_manip, manipulator, conf2.value, max_iterations=10) if path is None: return yield [(make_traj(path),)]
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 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, grasp): # Sample pregrasp config and motion plan that performs a grasp body = bodies[obj] def reset_env(): enable_all(bodies, False) body.Enable(True) set_pose(body, pose.value) set_active_manipulator(robot, arm.GetName()) reset_env() for _ in xrange(max_failures): gripper_from_obj, gripper_from_pregrasp = grasp.value.sample() world_from_gripper = manip_from_pose_grasp(pose.value, gripper_from_obj) world_from_base = next(random_inverse_reachability(ir_model, world_from_gripper)) set_trans(robot, world_from_base) set_manipulator_conf(arm, carry_arm_conf) if check_collision(robot): continue q = Conf(robot.GetConfigurationValues()) grasp_arm_conf = solve_inverse_kinematics(arm, world_from_gripper) if grasp_arm_conf is None: continue set_manipulator_conf(arm, grasp_arm_conf) #robot.Grab(body) pregrasp_arm_conf = solve_inverse_kinematics(arm, world_from_gripper.dot(gripper_from_pregrasp)) if pregrasp_arm_conf is None: #robot.Release(body) continue set_manipulator_conf(arm, pregrasp_arm_conf) if DISABLE_MOTIONS: path = [grasp_arm_conf, pregrasp_arm_conf, carry_arm_conf] t = Prehensile(full_from_active(robot, path), obj, pose, gripper_from_obj) yield [(q, t)] reset_env() return 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: pass
def solve_tamp(env, use_focused): use_viewer = env.GetViewer() is not None problem = PROBLEM(env) # TODO: most of my examples have literally had straight-line motions plans robot, manipulator, base_manip, ir_model = initialize_openrave( env, ARM, min_delta=MIN_DELTA) set_manipulator_conf(ir_model.manip, CARRY_CONFIG) bodies = {obj: env.GetKinBody(obj) for obj in problem.movable_names} surfaces = {surface.name: surface for surface in problem.surfaces} open_gripper(manipulator) initial_q = Conf(robot.GetConfigurationValues()) initial_poses = { name: Pose(name, get_pose(body)) for name, body in bodies.iteritems() } # TODO: just keep track of the movable degrees of freedom # GetActiveDOFIndices, GetActiveJointIndices, GetActiveDOFValues saver = EnvironmentStateSaver(env) #cfree_pose = cfree_pose_fn(env, bodies) #cfree_traj = cfree_traj_fn(env, robot, manipulator, body1, body2, all_bodies) #base_generator = base_generator_fn(ir_model) #base_values = base_values_from_full_config(initial_q.value) #goal_values = full_config_from_base_values(base_values + np.array([1, 0, 0]), initial_q.value) #goal_conf = Conf(goal_values) #return #################### # TODO - should objects contain their geometry cond_streams = [ EasyListGenStream(inputs=[O, P, G], outputs=[Q, T], conditions=[IsPose(O, P), IsGrasp(O, G)], effects=[GraspMotion(O, P, G, Q, T)], generator=sample_grasp_traj_fn( base_manip, ir_model, bodies, CARRY_CONFIG)), EasyGenStream(inputs=[Q, Q2], outputs=[T], conditions=[], effects=[FreeMotion(Q, Q2, T)], generator=sample_free_base_motion_fn(base_manip, bodies), order=1, max_level=0), EasyTestStream( inputs=[O, P, O2, P2], conditions=[IsPose(O, P), IsPose(O2, P2)], effects=[CFreePose(P, P2)], test=lambda *args: True, #cfree_pose, eager=True), EasyTestStream(inputs=[T, P], conditions=[], effects=[CFreeTraj(T, P)], test=lambda *args: True), #test=cfree_traj), EasyListGenStream(inputs=[O], outputs=[G], conditions=[], effects=[IsGrasp(O, G)], generator=grasp_generator_fn(bodies, TOP_GRASPS, SIDE_GRASPS, MAX_GRASPS)), EasyListGenStream(inputs=[O, S], outputs=[P], conditions=[], effects=[IsPose(O, P), Stable(P, S)], generator=pose_generator_fn(bodies, surfaces)), #EasyGenStream(inputs=[O, P, G], outputs=[Q], conditions=[IsPose(O, P), IsGrasp(O, G)], # effects=[], generator=base_generator), ] #################### constants = [] initial_atoms = [ConfEq(initial_q), HandEmpty()] for name in initial_poses: initial_atoms += body_initial_atoms(name, initial_poses, bodies, surfaces) goal_formula = And( ConfEq(initial_q), *[ OnSurface(obj, surface) for obj, surface in problem.goal_surfaces.iteritems() ]) #goal_formula = ConfEq(goal_conf) #obj, _ = problem.goal_surfaces.items()[0] #goal_formula = And(Holding(obj)) #goal_formula = Holding(obj) # TODO: this cause a bug stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, cond_streams, constants) print stream_problem handles = draw_affine_limits(robot) if use_viewer: for surface in problem.surfaces: surface.draw(env) raw_input('Start?') max_time = INF search_fn = get_fast_downward('eager', max_time=10, verbose=False) if use_focused: solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False, max_time=max_time) else: solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=10, waves=False, debug=False, max_time=max_time) with env: plan, universe = solve() print SEPARATOR plan = convert_plan(plan) if plan is not None: print 'Success' for i, (action, args) in enumerate(plan): print i + 1, action, args else: print 'Failure' #################### if plan is not None: commands = process_plan(robot, bodies, plan) if use_viewer: print SEPARATOR saver.Restore() visualize_solution(commands, step=False) raw_input('Finish?')
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 _execute_traj(confs): for j, conf in enumerate(confs): set_manipulator_conf(manipulator, conf) sleep(0.05)