def test_print(robot, node_points, elements): #elements = [elements[0]] elements = [elements[-1]] [element_body] = create_elements(node_points, elements) wait_for_user() phi = 0 #grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=phi, yaw=theta)) # for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)] #grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=theta, yaw=phi)) # for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)] grasp_rotations = [sample_direction() for _ in range(25)] link = link_from_name(robot, TOOL_NAME) movable_joints = get_movable_joints(robot) sample_fn = get_sample_fn(robot, movable_joints) for grasp_rotation in grasp_rotations: n1, n2 = elements[0] length = np.linalg.norm(node_points[n2] - node_points[n1]) set_joint_positions(robot, movable_joints, sample_fn()) for t in np.linspace(-length / 2, length / 2, 10): #element_translation = Pose(point=Point(z=-t)) #grasp_pose = multiply(grasp_rotation, element_translation) #reverse = Pose(euler=Euler()) reverse = Pose(euler=Euler(roll=np.pi)) grasp_pose = get_grasp_pose(t, grasp_rotation, 0) grasp_pose = multiply(grasp_pose, reverse) element_pose = get_pose(element_body) link_pose = multiply(element_pose, invert(grasp_pose)) conf = inverse_kinematics(robot, link, link_pose) wait_for_user()
def command_collision(end_effector, command, bodies): # TODO: each new addition makes collision checking more expensive #offset = 4 #for robot_conf in trajectory[offset:-offset]: collisions = [False for _ in range(len(bodies))] # Orientation remains the same for the extrusion trajectory idx_from_body = dict(zip(bodies, range(len(bodies)))) # TODO: separate into another method. Sort paths by tool poses first for trajectory in command.trajectories: for tool_pose in randomize(trajectory.get_link_path()): # TODO: bisect end_effector.set_pose(tool_pose) #for body, _ in get_bodies_in_region(tool_aabb): # TODO for i, body in enumerate(bodies): if body not in idx_from_body: # Robot continue idx = idx_from_body[body] if not collisions[idx]: collisions[idx] |= pairwise_collision( end_effector.body, body) for trajectory in command.trajectories: for robot_conf in randomize(trajectory.path): set_joint_positions(trajectory.robot, trajectory.joints, robot_conf) for i, body in enumerate(bodies): if not collisions[i]: collisions[i] |= pairwise_collision(trajectory.robot, body) #for element, unsafe in zip(elements, collisions): # command.safe_per_element[element] = unsafe return collisions
def fn(conf1, conf2, fluents=[]): if teleport is True: path = [conf1, conf2] traj = MotionTrajectory(robot, movable_joints, path) return traj, # TODO: double check that collisions with movable obstacles are considered obstacles = list(obstacle_from_name.values()) attachments = parse_fluents(robot, brick_from_index, fluents, obstacles) set_joint_positions(robot, movable_joints, conf1) path = plan_joint_motion( robot, movable_joints, conf2, obstacles=obstacles, attachments=attachments, self_collisions=SELF_COLLISIONS, disabled_collisions=disabled_collisions, #weights=weights, resolutions=resolutions, restarts=5, iterations=50, smooth=100) if path is None: return None traj = MotionTrajectory(robot, movable_joints, path) return traj,
def plan_workspace_motion(robot, arm, tool_path, collision_buffer=COLLISION_BUFFER, **kwargs): _, resolutions = get_weights_resolutions(robot, arm) tool_link = link_from_name(robot, TOOL_FRAMES[arm]) arm_joints = get_arm_joints(robot, arm) arm_waypoints = plan_cartesian_motion(robot, arm_joints[0], tool_link, tool_path, pos_tolerance=5e-3) if arm_waypoints is None: return None arm_waypoints = [[conf[j] for j in movable_from_joints(robot, arm_joints)] for conf in arm_waypoints] set_joint_positions(robot, arm_joints, arm_waypoints[0]) return plan_waypoints_joint_motion( robot, arm_joints, arm_waypoints[1:], resolutions=resolutions, max_distance=collision_buffer, custom_limits=get_pr2_safety_limits(robot), **kwargs)
def main(): connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") with HideOutput(): with LockRenderer(): robot = load_model(FRANKA_URDF, fixed_base=True) dump_body(robot) print('Start?') wait_for_user() tool_link = link_from_name(robot, 'panda_hand') joints = get_movable_joints(robot) print('Joints', [get_joint_name(robot, joint) for joint in joints]) sample_fn = get_sample_fn(robot, joints) for i in range(10): print('Iteration:', i) conf = sample_fn() set_joint_positions(robot, joints, conf) wait_for_user() test_retraction(robot, PANDA_INFO, tool_link, max_distance=0.01, max_time=0.05) disconnect()
def experimental_inverse_kinematics(robot, link, pose, null_space=False, max_iterations=200, tolerance=1e-3): (point, quat) = pose # https://github.com/bulletphysics/bullet3/blob/389d7aaa798e5564028ce75091a3eac6a5f76ea8/examples/SharedMemory/PhysicsClientC_API.cpp # https://github.com/bulletphysics/bullet3/blob/c1ba04a5809f7831fa2dee684d6747951a5da602/examples/pybullet/examples/inverse_kinematics_husky_kuka.py joints = get_joints( robot) # Need to have all joints (although only movable returned) movable_joints = get_movable_joints(robot) current_conf = get_joint_positions(robot, joints) # TODO: sample other values for the arm joints as the reference conf min_limits = [get_joint_limits(robot, joint)[0] for joint in joints] max_limits = [get_joint_limits(robot, joint)[1] for joint in joints] #min_limits = current_conf #max_limits = current_conf #max_velocities = [get_max_velocity(robot, joint) for joint in joints] # Range of Jacobian max_velocities = [10000] * len(joints) # TODO: cannot have zero velocities # TODO: larger definitely better for velocities #damping = tuple(0.1*np.ones(len(joints))) #t0 = time.time() #kinematic_conf = get_joint_positions(robot, movable_joints) for iterations in range(max_iterations): # 0.000863273143768 / iteration # TODO: return none if no progress if null_space: kinematic_conf = p.calculateInverseKinematics( robot, link, point, quat, lowerLimits=min_limits, upperLimits=max_limits, jointRanges=max_velocities, restPoses=current_conf, #jointDamping=damping, ) else: kinematic_conf = p.calculateInverseKinematics( robot, link, point, quat) if (kinematic_conf is None) or any(map(math.isnan, kinematic_conf)): return None set_joint_positions(robot, movable_joints, kinematic_conf) link_point, link_quat = get_link_pose(robot, link) if np.allclose(link_point, point, atol=tolerance) and np.allclose( link_quat, quat, atol=tolerance): #print iterations break else: return None if violates_limits(robot, movable_joints, kinematic_conf): return None #total_time = (time.time() - t0) #print total_time #print (time.time() - t0)/max_iterations return kinematic_conf
def test_close_gripper(robot, arm): gripper_joints = get_gripper_joints(robot, arm) extend_fn = get_extend_fn(robot, gripper_joints) for positions in extend_fn(get_open_positions(robot, arm), get_closed_positions(robot, arm)): set_joint_positions(robot, gripper_joints, positions) print(positions) wait_if_gui('Continue?')
def is_safe(self, elements, element_bodies): # TODO: check the end-effector first known_elements = set(self.safe_per_element) & set(elements) if not all(self.safe_per_element[e] for e in known_elements): return False unknown_elements = randomize(set(elements) - known_elements) if not unknown_elements: return True for trajectory in randomize( self.trajectories ): # TODO: could cache each individual collision intersecting = trajectory.get_intersecting() for i in randomize(range(len(trajectory))): set_joint_positions(trajectory.robot, trajectory.joints, trajectory.path[i]) for element in unknown_elements: body = element_bodies[element] #if not pairwise_collision(trajectory.robot, body): # self.set_unsafe(element) # return False for robot_link, bodies in intersecting[i].items(): #print(robot_link, bodies, len(bodies)) if (element_bodies[element] in bodies) and pairwise_link_collision( trajectory.robot, robot_link, body, link2=BASE_LINK): self.set_unsafe(element) return False self.update_safe(elements) return True
def main(): # The URDF loader seems robust to package:// and slightly wrong relative paths? connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") #with HideOutput(): with LockRenderer(): robot = load_model(MOVO_URDF, fixed_base=True) dump_body(robot) print('Start?') wait_for_user() #joint_names = HEAD_JOINTS #joints = joints_from_names(robot, joint_names) joints = get_movable_joints(robot) print('Joints', [get_joint_name(robot, joint) for joint in joints]) sample_fn = get_sample_fn(robot, joints) for i in range(10): print('Iteration:', i) conf = sample_fn() set_joint_positions(robot, joints, conf) wait_for_user() disconnect()
def solve_inverse_kinematics(self, world_from_tool, nearby_tolerance=INF, **kwargs): if self.ik_solver is not None: return self.solve_trac_ik(world_from_tool, **kwargs) #if nearby_tolerance != INF: # return self.solve_pybullet_ik(world_from_tool, nearby_tolerance=nearby_tolerance) current_conf = get_joint_positions(self.robot, self.arm_joints) start_time = time.time() if nearby_tolerance == INF: generator = ikfast_inverse_kinematics(self.robot, PANDA_INFO, self.tool_link, world_from_tool, max_attempts=10, use_halton=True) else: generator = closest_inverse_kinematics( self.robot, PANDA_INFO, self.tool_link, world_from_tool, max_time=0.01, max_distance=nearby_tolerance, use_halton=True) conf = next(generator, None) #conf = sample_tool_ik(self.robot, world_from_tool, max_attempts=100) if conf is None: return conf max_distance = get_distance(current_conf, conf, norm=INF) #print('Time: {:.3f} | distance: {:.5f} | max: {:.5f}'.format( # elapsed_time(start_time), max_distance, nearby_tolerance)) set_joint_positions(self.robot, self.arm_joints, conf) return get_configuration(self.robot)
def set_initial_conf(self): set_joint_positions(self.robot, self.base_joints, [2.0, 0, np.pi]) #for rule in self.robot_yaml['cspace_to_urdf_rules']: # gripper: max is open # joint = joint_from_name(self.robot, rule['name']) # set_joint_position(self.robot, joint, rule['value']) set_joint_positions(self.robot, self.arm_joints, self.default_conf) # active_task_spaces
def test_retraction(robot, info, tool_link, distance=0.1, **kwargs): ik_joints = get_ik_joints(robot, info, tool_link) start_pose = get_link_pose(robot, tool_link) end_pose = multiply(start_pose, Pose(Point(z=-distance))) handles = [ add_line(point_from_pose(start_pose), point_from_pose(end_pose), color=BLUE) ] #handles.extend(draw_pose(start_pose)) #handles.extend(draw_pose(end_pose)) path = [] pose_path = list( interpolate_poses(start_pose, end_pose, pos_step_size=0.01)) for i, pose in enumerate(pose_path): print('Waypoint: {}/{}'.format(i + 1, len(pose_path))) handles.extend(draw_pose(pose)) conf = next( either_inverse_kinematics(robot, info, tool_link, pose, **kwargs), None) if conf is None: print('Failure!') path = None wait_for_user() break set_joint_positions(robot, ik_joints, conf) path.append(conf) wait_for_user() # for conf in islice(ikfast_inverse_kinematics(robot, info, tool_link, pose, max_attempts=INF, max_distance=0.5), 1): # set_joint_positions(robot, joints[:len(conf)], conf) # wait_for_user() remove_handles(handles) return path
def test_drake_base_motion(pr2, base_start, base_goal, obstacles=[]): # TODO: combine this with test_arm_motion """ Drake's PR2 URDF has explicit base joints """ disabled_collisions = get_disabled_collisions(pr2) base_joints = [joint_from_name(pr2, name) for name in PR2_GROUPS['base']] set_joint_positions(pr2, base_joints, base_start) base_joints = base_joints[:2] base_goal = base_goal[:len(base_joints)] wait_for_user('Plan Base?') base_path = plan_joint_motion(pr2, base_joints, base_goal, obstacles=obstacles, disabled_collisions=disabled_collisions) if base_path is None: print('Unable to find a base path') return print(len(base_path)) for bq in base_path: set_joint_positions(pr2, base_joints, bq) if SLEEP is None: wait_for_user('Continue?') else: time.sleep(SLEEP)
def extract_full_path(robot, path_joints, path, all_joints): with BodySaver(robot): new_path = [] for conf in path: set_joint_positions(robot, path_joints, conf) new_path.append(get_joint_positions( robot, all_joints)) # TODO: do without assigning return new_path
def at(self, time_from_start): assert self.spline is not None if (time_from_start < self.start_time) or (self.end_time < time_from_start): return None conf = self.spline(time_from_start) set_joint_positions(self.robot, self.joints, conf) return conf
def main(group=SE3): connect(use_gui=True) set_camera_pose(camera_point=SIZE*np.array([1., -1., 1.])) # TODO: can also create all links and fix some joints # TODO: SE(3) motion planner (like my SE(3) one) where some dimensions are fixed obstacle = create_box(w=SIZE, l=SIZE, h=SIZE, color=RED) #robot = create_cylinder(radius=0.025, height=0.1, color=BLUE) obstacles = [obstacle] collision_id, visual_id = create_shape(get_cylinder_geometry(radius=0.025, height=0.1), color=BLUE) robot = create_flying_body(group, collision_id, visual_id) body_link = get_links(robot)[-1] joints = get_movable_joints(robot) joint_from_group = dict(zip(group, joints)) print(joint_from_group) #print(get_aabb(robot, body_link)) dump_body(robot, fixed=False) custom_limits = {joint_from_group[j]: l for j, l in CUSTOM_LIMITS.items()} # sample_fn = get_sample_fn(robot, joints, custom_limits=custom_limits) # for i in range(10): # conf = sample_fn() # set_joint_positions(robot, joints, conf) # wait_for_user('Iteration: {}'.format(i)) # return initial_point = SIZE*np.array([-1., -1., 0]) #initial_point = -1.*np.ones(3) final_point = -initial_point initial_euler = np.zeros(3) add_line(initial_point, final_point, color=GREEN) initial_conf = np.concatenate([initial_point, initial_euler]) final_conf = np.concatenate([final_point, initial_euler]) set_joint_positions(robot, joints, initial_conf) #print(initial_point, get_link_pose(robot, body_link)) #set_pose(robot, Pose(point=-1.*np.ones(3))) path = plan_joint_motion(robot, joints, final_conf, obstacles=obstacles, self_collisions=False, custom_limits=custom_limits) if path is None: disconnect() return for i, conf in enumerate(path): set_joint_positions(robot, joints, conf) point, quat = get_link_pose(robot, body_link) #euler = euler_from_quat(quat) euler = intrinsic_euler_from_quat(quat) print(conf) print(point, euler) wait_for_user('Step: {}/{}'.format(i, len(path))) wait_for_user('Finish?') disconnect()
def main(): # The URDF loader seems robust to package:// and slightly wrong relative paths? connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") with LockRenderer(): with HideOutput(): robot = load_model(MOVO_URDF, fixed_base=True) for link in get_links(robot): set_color(robot, color=apply_alpha(0.25 * np.ones(3), 1), link=link) base_joints = joints_from_names(robot, BASE_JOINTS) draw_base_limits((get_min_limits( robot, base_joints), get_max_limits(robot, base_joints)), z=1e-2) dump_body(robot) wait_for_user('Start?') #for arm in ARMS: # test_close_gripper(robot, arm) arm = 'right' tool_link = link_from_name(robot, TOOL_LINK.format(arm)) #joint_names = HEAD_JOINTS #joints = joints_from_names(robot, joint_names) joints = base_joints + get_arm_joints(robot, arm) #joints = get_movable_joints(robot) print('Joints:', get_joint_names(robot, joints)) ik_joints = get_ik_joints(robot, MOVO_INFOS[arm], tool_link) #fixed_joints = [] fixed_joints = ik_joints[:1] #fixed_joints = ik_joints sample_fn = get_sample_fn(robot, joints) handles = [] for i in range(10): print('Iteration:', i) conf = sample_fn() print(conf) set_joint_positions(robot, joints, conf) tool_pose = get_link_pose(robot, tool_link) remove_handles(handles) handles = draw_pose(tool_pose) wait_for_user() #conf = next(ikfast_inverse_kinematics(robot, MOVO_INFOS[arm], tool_link, tool_pose, # fixed_joints=fixed_joints, max_time=0.1), None) #if conf is not None: # set_joint_positions(robot, ik_joints, conf) #wait_for_user() test_retraction(robot, MOVO_INFOS[arm], tool_link, fixed_joints=fixed_joints, max_time=0.1) disconnect()
def compute_door_paths(world, joint_name, door_conf1, door_conf2, obstacles=set(), teleport=False): door_paths = [] if door_conf1 == door_conf2: return door_paths door_joint = joint_from_name(world.kitchen, joint_name) door_joints = [door_joint] # TODO: could unify with grasp path door_extend_fn = get_extend_fn(world.kitchen, door_joints, resolutions=[DOOR_RESOLUTION]) door_path = [door_conf1.values] + list( door_extend_fn(door_conf1.values, door_conf2.values)) if teleport: door_path = [door_conf1.values, door_conf2.values] # TODO: open until collision for the drawers sign = world.get_door_sign(door_joint) pull = (sign * door_path[0][0] < sign * door_path[-1][0]) # door_obstacles = get_descendant_obstacles(world.kitchen, door_joint) for handle_grasp in get_handle_grasps(world, door_joint, pull=pull): link, grasp, pregrasp = handle_grasp handle_path = [] for door_conf in door_path: set_joint_positions(world.kitchen, door_joints, door_conf) # if any(pairwise_collision(door_obst, obst) # for door_obst, obst in product(door_obstacles, obstacles)): # return handle_path.append(get_link_pose(world.kitchen, link)) # Collide due to adjacency # TODO: check pregrasp path as well # TODO: check gripper self-collisions with the robot set_configuration(world.gripper, world.open_gq.values) tool_path = [ multiply(handle_pose, invert(grasp)) for handle_pose in handle_path ] for i, tool_pose in enumerate(tool_path): set_joint_positions(world.kitchen, door_joints, door_path[i]) set_tool_pose(world, tool_pose) # handles = draw_pose(handle_path[i], length=0.25) # handles.extend(draw_aabb(get_aabb(world.kitchen, link=link))) # wait_for_user() # for handle in handles: # remove_debug(handle) if any( pairwise_collision(world.gripper, obst) for obst in obstacles): break else: door_paths.append( DoorPath(door_path, handle_path, handle_grasp, tool_path)) return door_paths
def iterate(self, world, attachments): joints = get_gripper_joints(world.robot, self.arm) start_conf = get_joint_positions(world.robot, joints) end_conf = [self.position] * len(joints) extend_fn = get_extend_fn(world.robot, joints) path = [start_conf] + list(extend_fn(start_conf, end_conf)) for positions in path: set_joint_positions(world.robot, joints, positions) yield positions
def step_curve(robot, joints, path, step_size=None): wait_if_gui(message='Begin?') for i, conf in enumerate(path): set_joint_positions(robot, joints, conf) if step_size is None: wait_if_gui(message='{}/{} Continue?'.format(i, len(path))) else: wait_for_duration(duration=step_size) wait_if_gui(message='Finish?')
def main(): connect(use_gui=True) with HideOutput(): pr2 = load_model( "models/drake/pr2_description/urdf/pr2_simplified.urdf") set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['left_arm']), REST_LEFT_ARM) set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['right_arm']), rightarm_from_leftarm(REST_LEFT_ARM)) set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['torso']), [0.2]) dump_body(pr2) block = load_model(BLOCK_URDF, fixed_base=False) set_point(block, [2, 0.5, 1]) target_point = point_from_pose(get_pose(block)) # head_link = link_from_name(pr2, HEAD_LINK) head_joints = joints_from_names(pr2, PR2_GROUPS['head']) head_link = head_joints[-1] #max_detect_distance = 2.5 max_register_distance = 1.0 distance_range = (max_register_distance / 2, max_register_distance) base_generator = visible_base_generator(pr2, target_point, distance_range) base_joints = joints_from_names(pr2, PR2_GROUPS['base']) for i in range(5): base_conf = next(base_generator) set_joint_positions(pr2, base_joints, base_conf) p.addUserDebugLine(point_from_pose(get_link_pose(pr2, head_link)), target_point, lineColorRGB=(1, 0, 0)) # addUserDebugText p.addUserDebugLine(point_from_pose( get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))), target_point, lineColorRGB=(0, 0, 1)) # addUserDebugText # head_conf = sub_inverse_kinematics(pr2, head_joints[0], HEAD_LINK, ) head_conf = inverse_visibility(pr2, target_point) set_joint_positions(pr2, head_joints, head_conf) print(get_detections(pr2)) # TODO: does this detect the robot sometimes? detect_mesh, z = get_detection_cone(pr2, block) detect_cone = create_mesh(detect_mesh, color=(0, 1, 0, 0.5)) set_pose(detect_cone, get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))) view_cone = get_viewcone(depth=2.5, color=(1, 0, 0, 0.25)) set_pose(view_cone, get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))) wait_for_user() remove_body(detect_cone) remove_body(view_cone) disconnect()
def test_confs(robot, num_samples=10): joints = get_movable_joints(robot) print('Joints', [get_joint_name(robot, joint) for joint in joints]) sample_fn = get_sample_fn(robot, joints) for i in range(num_samples): print('Iteration:', i) conf = sample_fn() set_joint_positions(robot, joints, conf) wait_for_user()
def get_link_path(self, link_name=TOOL_LINK): link = link_from_name(self.robot, link_name) if link not in self.path_from_link: with BodySaver(self.robot): self.path_from_link[link] = [] for conf in self.path: set_joint_positions(self.robot, self.joints, conf) self.path_from_link[link].append( get_link_pose(self.robot, link)) return self.path_from_link[link]
def is_pull_safe(world, door_joint, door_plan): obstacles = get_descendant_obstacles(world.kitchen, door_joint) door_path, handle_path, handle_plan, tool_path = door_plan for door_conf in [door_path[0], door_path[-1]]: # TODO: check the whole door trajectory set_joint_positions(world.kitchen, [door_joint], door_conf) # TODO: just check collisions with the base of the robot if any(pairwise_collision(world.robot, b) for b in obstacles): if PRINT_FAILURES: print('Door start/end failure') return False return True
def test(arm1, conf1, arm2, conf2): # TODO: don't let the arms get too close if not collisions or (arm1 == arm2): return False arm1_joints = get_arm_joints(robot, arm1) set_joint_positions(robot, arm1_joints, conf1) arm2_joints = get_arm_joints(robot, arm2) set_joint_positions(robot, arm2_joints, conf2) return link_pairs_collision(robot, get_moving_links(robot, arm1_joints), robot, get_moving_links(robot, arm2_joints))
def test_arm_motion(pr2, left_joints, arm_goal): disabled_collisions = get_disabled_collisions(pr2) user_input('Plan Arm?') arm_path = plan_joint_motion(pr2, left_joints, arm_goal, disabled_collisions=disabled_collisions) if arm_path is None: print('Unable to find an arm path') return print(len(arm_path)) for q in arm_path: set_joint_positions(pr2, left_joints, q) #raw_input('Continue?') time.sleep(0.01)
def get_aabbs(self): #traj.aabb = aabb_union(map(get_turtle_traj_aabb, traj.iterate())) # TODO: union if self.aabbs is not None: return self.aabbs self.aabbs = [] links = get_all_links(self.robot) with BodySaver(self.robot): for conf in self.path: set_joint_positions(self.robot, self.joints, conf) self.aabbs.append( {link: get_aabb(self.robot, link) for link in links}) return self.aabbs
def test_arm_motion(pr2, left_joints, arm_goal): disabled_collisions = get_disabled_collisions(pr2) wait_if_gui('Plan Arm?') with LockRenderer(lock=False): arm_path = plan_joint_motion(pr2, left_joints, arm_goal, disabled_collisions=disabled_collisions) if arm_path is None: print('Unable to find an arm path') return print(len(arm_path)) for q in arm_path: set_joint_positions(pr2, left_joints, q) #wait_if_gui('Continue?') wait_for_duration(0.01)
def test_door(door): door_joints = get_movable_joints(door) sample_fn = get_sample_fn(door, door_joints) set_joint_positions(door, door_joints, sample_fn()) while True: positions = sample_fn() set_joint_positions(door, door_joints, positions) wait_if_gui() lower, upper = get_joint_intervals(door, door_joints) control_joints(door, door_joints, positions=lower) velocity_control_joints(door, door_joints, velocities=[PI / 4]) # Able to exceed limits
def iterate_path(robot, joints, path, step_size=None): # 1e-2 | None if path is None: return path = adjust_path(robot, joints, path) with LockRenderer(): handles = draw_path(path) wait_if_gui(message='Begin?') for i, conf in enumerate(path): set_joint_positions(robot, joints, conf) if step_size is None: wait_if_gui(message='{}/{} Continue?'.format(i, len(path))) else: wait_for_duration(duration=step_size) wait_if_gui(message='Finish?')