def sample_push_contact(world, feature, parameter, under=False): robot = world.robot arm = feature['arm_name'] body = world.get_body(feature['block_name']) push_yaw = feature['push_yaw'] center, (width, _, height) = approximate_as_prism(body, body_pose=Pose(euler=Euler(yaw=push_yaw))) max_backoff = width + 0.1 # TODO: add gripper bounding box tool_link = link_from_name(robot, TOOL_FRAMES[arm]) tool_pose = get_link_pose(robot, tool_link) gripper_link = link_from_name(robot, GRIPPER_LINKS[arm]) collision_links = get_link_subtree(robot, gripper_link) urdf_from_center = Pose(point=center) reverse_z = Pose(euler=Euler(pitch=math.pi)) rotate_theta = Pose(euler=Euler(yaw=push_yaw)) #translate_z = Pose(point=Point(z=-feature['block_height']/2. + parameter['gripper_z'])) # Relative to base translate_z = Pose(point=Point(z=parameter['gripper_z'])) # Relative to center tilt_gripper = Pose(euler=Euler(pitch=parameter['gripper_tilt'])) grasps = [] for i in range(1 + under): flip_gripper = Pose(euler=Euler(yaw=i * math.pi)) for x in np.arange(0, max_backoff, step=0.01): translate_x = Pose(point=Point(x=-x)) grasp_pose = multiply(flip_gripper, tilt_gripper, translate_x, translate_z, rotate_theta, reverse_z, invert(urdf_from_center)) set_pose(body, multiply(tool_pose, TOOL_POSE, grasp_pose)) if not link_pairs_collision(robot, collision_links, body, collision_buffer=0.): grasps.append(grasp_pose) break return grasps
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 sample_tool_ik(robot, arm, world_from_target, max_attempts=10, **kwargs): world_from_tool = get_link_pose( robot, link_from_name(robot, PR2_TOOL_FRAMES[arm])) world_from_ik = get_link_pose(robot, link_from_name(robot, IK_LINK[arm])) tool_from_ik = multiply(invert(world_from_tool), world_from_ik) ik_pose = multiply(world_from_target, tool_from_ik) generator = get_ik_generator(robot, arm, ik_pose, **kwargs) for _ in range(max_attempts): try: solutions = next(generator) if solutions: return random.choice(solutions) except StopIteration: break return None
def compute_surface_aabb(world, surface_name): if surface_name in ENV_SURFACES: # TODO: clean this up # TODO: the aabb for golf is off the table surface_body = world.environment_bodies[surface_name] return get_aabb(surface_body) surface_body = world.kitchen surface_name, shape_name, _ = surface_from_name(surface_name) surface_link = link_from_name(surface_body, surface_name) surface_pose = get_link_pose(surface_body, surface_link) if shape_name == SURFACE_TOP: surface_aabb = get_aabb(surface_body, surface_link) elif shape_name == SURFACE_BOTTOM: data = sorted(get_collision_data(surface_body, surface_link), key=lambda d: point_from_pose(get_data_pose(d))[2])[0] extent = np.array(get_data_extents(data)) aabb = AABB(-extent/2., +extent/2.) vertices = apply_affine(multiply(surface_pose, get_data_pose(data)), get_aabb_vertices(aabb)) surface_aabb = aabb_from_points(vertices) else: [data] = filter(lambda d: d.filename != '', get_collision_data(surface_body, surface_link)) meshes = read_obj(data.filename) #colors = spaced_colors(len(meshes)) #set_color(surface_body, link=surface_link, color=np.zeros(4)) mesh = meshes[shape_name] #for i, (name, mesh) in enumerate(meshes.items()): mesh = tform_mesh(multiply(surface_pose, get_data_pose(data)), mesh=mesh) surface_aabb = aabb_from_points(mesh.vertices) #add_text(surface_name, position=surface_aabb[1]) #draw_mesh(mesh, color=colors[i]) #wait_for_user() #draw_aabb(surface_aabb) #wait_for_user() return surface_aabb
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 test_aabb(robot): base_link = link_from_name(robot, BASE_LINK_NAME) region_aabb = AABB(lower=-np.ones(3), upper=+np.ones(3)) draw_aabb(region_aabb) # bodies = get_bodies_in_region(region_aabb) # print(len(bodies), bodies) # for body in get_bodies(): # set_pose(body, Pose()) #step_simulation() # Need to call before get_bodies_in_region #update_scene() for i in range(3): with timer(message='{:f}'): bodies = get_bodies_in_region( region_aabb) # This does cache some info print(i, len(bodies), bodies) # https://github.com/bulletphysics/bullet3/search?q=broadphase # https://github.com/bulletphysics/bullet3/search?p=1&q=getCachedOverlappingObjects&type=&utf8=%E2%9C%93 # https://andysomogyi.github.io/mechanica/bullet.html # http://www.cs.kent.edu/~ruttan/GameEngines/lectures/Bullet_User_Manual aabb = get_aabb(robot) # aabb = get_subtree_aabb(robot, base_link) print(aabb) draw_aabb(aabb) for link in [BASE_LINK, base_link]: print(link, get_collision_data(robot, link), pairwise_link_collision(robot, link, robot, link))
def fn(o, j, a): link = link_from_name(world.kitchen, o) # link not surface p = RelPose( world.kitchen, # link, confs=[a], init=a.init) return (p, )
def get_disabled_collisions(robot): return { tuple( link_from_name(robot, link) for link in pair if has_link(robot, link)) for pair in DISABLED_COLLISIONS }
def get_ik_generator(robot, tool_pose): from .ikfast_kuka_kr6_r900 import get_ik world_from_base = get_link_pose(robot, link_from_name(robot, BASE_FRAME)) #world_from_base == get_pose(robot) base_from_tool = multiply(invert(world_from_base), tool_pose) base_from_ik = multiply(base_from_tool, get_tool_from_ik(robot)) yield compute_inverse_kinematics(get_ik, base_from_ik)
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 test_grasps(robot, node_points, elements): element = elements[-1] [element_body] = create_elements(node_points, [element]) phi = 0 link = link_from_name(robot, TOOL_NAME) link_pose = get_link_pose(robot, link) draw_pose(link_pose) #, parent=robot, parent_link=link) wait_for_user() n1, n2 = element p1 = node_points[n1] p2 = node_points[n2] length = np.linalg.norm(p2 - p1) # Bottom of cylinder is p1, top is p2 print(element, p1, p2) for phi in np.linspace(0, np.pi, 10, endpoint=True): theta = np.pi / 4 for t in np.linspace(-length / 2, length / 2, 10): translation = Pose( point=Point(z=-t) ) # Want to be moving backwards because +z is out end effector direction = Pose(euler=Euler(roll=np.pi / 2 + theta, pitch=phi)) angle = Pose(euler=Euler(yaw=1.2)) grasp_pose = multiply(angle, direction, translation) element_pose = multiply(link_pose, grasp_pose) set_pose(element_body, element_pose) wait_for_user()
def gen(knob_name, base_conf): knob_link = link_from_name(world.kitchen, knob_name) pose = get_link_pose(world.kitchen, knob_link) presses = cycle(get_grasp_presses(world, knob_name)) max_failures = FIXED_FAILURES if world.task.movable_base else INF failures = 0 while failures <= max_failures: for i in range(max_attempts): grasp = next(presses) randomize = (random.random() < P_RANDOMIZE_IK) ik_outputs = next( plan_press(world, knob_name, pose, grasp, base_conf, world.static_obstacles, randomize=randomize, **kwargs), None) if ik_outputs is not None: print('Fixed press succeeded after {} attempts'.format(i)) yield ik_outputs break # return else: if PRINT_FAILURES: print('Fixed pull failure after {} attempts'.format( max_attempts)) yield None max_failures += 1
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 get_link_obstacles(world, link_name): if link_name in world.movable: return flatten_links(world.get_body(link_name)) elif has_link(world.kitchen, link_name): link = link_from_name(world.kitchen, link_name) return flatten_links(world.kitchen, get_link_subtree(world.kitchen, link)) # subtree? assert link_name in SURFACE_FROM_NAME return set()
def create_gripper(robot, arm): # gripper = load_pybullet(os.path.join(get_data_path(), 'pr2_gripper.urdf')) # gripper = load_pybullet(os.path.join(get_models_path(), 'pr2_description/pr2_l_gripper.urdf'), fixed_base=False) links = get_link_subtree(robot, link_from_name(robot, GRIPPER_LINKS[arm])) #print([get_link_name(robot, link) for link in links]) gripper = clone_body(robot, links=links, visual=True, collision=True) # TODO: joint limits return gripper
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 main(): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() connect(use_gui=True) with LockRenderer(): draw_pose(unit_pose(), length=1, width=1) floor = create_floor() set_point(floor, Point(z=-EPSILON)) table1 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2, top_color=TAN, cylinder=False) set_point(table1, Point(y=+0.5)) table2 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2, top_color=TAN, cylinder=False) set_point(table2, Point(y=-0.5)) tables = [table1, table2] #set_euler(table1, Euler(yaw=np.pi/2)) with HideOutput(): # data_path = add_data_path() # robot_path = os.path.join(data_path, WSG_GRIPPER) robot_path = get_model_path(WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF robot = load_pybullet(robot_path, fixed_base=True) #dump_body(robot) block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED) block_z = stable_z(block1, table1) set_point(block1, Point(y=-0.5, z=block_z)) block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN) set_point(block2, Point(x=-0.25, y=-0.5, z=block_z)) block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE) set_point(block3, Point(x=-0.15, y=+0.5, z=block_z)) blocks = [block1, block2, block3] set_camera_pose(camera_point=Point(x=-1, z=block_z+1), target_point=Point(z=block_z)) block_pose = get_pose(block1) open_gripper(robot) tool_link = link_from_name(robot, 'tool_link') base_from_tool = get_relative_pose(robot, tool_link) #draw_pose(unit_pose(), parent=robot, parent_link=tool_link) grasps = get_side_grasps(block1, tool_pose=Pose(euler=Euler(yaw=np.pi/2)), top_offset=0.02, grasp_length=0.03, under=False)[1:2] for grasp in grasps: gripper_pose = multiply(block_pose, invert(grasp)) set_pose(robot, multiply(gripper_pose, invert(base_from_tool))) wait_for_user() wait_for_user('Finish?') disconnect()
def get_tool_pose(robot): from .ikfast_kuka_kr6_r900 import get_fk ik_joints = get_movable_joints(robot) conf = get_joint_positions(robot, ik_joints) # TODO: this should be linked to ikfast's get numOfJoint function assert len(conf) == 6 base_from_tool = compute_forward_kinematics(get_fk, conf) world_from_base = get_link_pose(robot, link_from_name(robot, BASE_FRAME)) return multiply(world_from_base, base_from_tool)
def extract_point(loc): if isinstance(loc, str): name = loc.split('-')[0] conf = initial_confs[name] conf.assign() robot = index_from_name(robots, name) return point_from_pose( get_link_pose(robot, link_from_name(robot, TOOL_LINK))) else: return node_points[loc]
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 create_surface_attachment(world, obj_name, surface_name): body = world.get_body(obj_name) if surface_name in ENV_SURFACES: surface_body = world.environment_bodies[surface_name] surface_link = BASE_LINK else: surface = surface_from_name(surface_name) surface_body = world.kitchen surface_link = link_from_name(surface_body, surface.link) return create_attachment(surface_body, surface_link, body)
def get_tool_pose(robot, arm): from .ikfast_eth_rfl import get_fk ik_joints = get_torso_arm_joints(robot, arm) conf = get_joint_positions(robot, ik_joints) # TODO: this should be linked to ikfast's get numOfJoint junction base_from_ik = compute_forward_kinematics(get_fk, conf) base_from_tool = multiply(base_from_ik, invert(get_tool_from_ik(robot, arm))) world_from_base = get_link_pose(robot, link_from_name(robot, IK_BASE_FRAMES[arm])) return multiply(world_from_base, base_from_tool)
def compute_grasp_width(robot, arm, body, grasp_pose, **kwargs): gripper_joints = get_gripper_joints(robot, arm) tool_link = link_from_name(robot, TOOL_FRAMES[arm]) tool_pose = get_link_pose(robot, tool_link) body_pose = multiply(tool_pose, grasp_pose) set_pose(body, body_pose) return close_until_collision(robot, gripper_joints, bodies=[body], max_distance=0.0, **kwargs)
def _create_robot(self): with ClientSaver(self.client): with HideOutput(): pr2_path = os.path.join(get_models_path(), PR2_URDF) self.pr2 = load_pybullet(pr2_path, fixed_base=True) # Base link is the origin base_pose = get_link_pose(self.robot, link_from_name(self.robot, BASE_FRAME)) set_pose(self.robot, invert(base_pose)) return self.pr2
def get_grasp_presses(world, knob, pre_distance=APPROACH_DISTANCE): knob_link = link_from_name(world.kitchen, knob) pre_direction = pre_distance * get_unit_vector([0, 0, 1]) post_direction = unit_point() for i, grasp_pose in enumerate( get_top_presses(world.kitchen, link=knob_link, tool_pose=TOOL_POSE, top_offset=FINGER_EXTENT[0] / 2 + 5e-3)): pregrasp_pose = multiply(Pose(point=pre_direction), grasp_pose, Pose(point=post_direction)) grasp = Grasp(world, knob, TOP_GRASP, i, grasp_pose, pregrasp_pose) yield grasp
def create_gripper(robot, visual=False): gripper_link = link_from_name(robot, get_gripper_link(robot)) links = get_link_descendants(robot, gripper_link) # get_link_descendants | get_link_subtree with LockRenderer(): # Actually uses the parent of the first link gripper = clone_body(robot, links=links, visual=False, collision=True) # TODO: joint limits if not visual: for link in get_all_links(gripper): set_color(gripper, np.zeros(4), link) #dump_body(robot) #dump_body(gripper) #user_input() return gripper
def test_shelves(visualize): arms = [LEFT_ARM] # TODO: smaller (2) shelves # TODO: sample with respect to the link it will supported by # shelves.off | shelves_points.off | tableShelves.off # import os # data_directory = '/Users/caelan/Programs/LIS/git/lis-data/meshes/' # mesh = read_mesh_off(os.path.join(data_directory, 'tableShelves.off')) # draw_mesh(mesh) # wait_for_interrupt() start_link = 'shelf2' # shelf1 | shelf2 | shelf3 | shelf4 end_link = 'shelf1' shelf_name = 'two_shelves' #shelf_name = 'three_shelves' cup_name = create_name('bluecup', 1) world, table_body = create_world([shelf_name, cup_name], visualize=visualize) cup_x = 0.65 #shelf_x = 0.8 shelf_x = 0.75 initial_poses = { shelf_name: ([shelf_x, 0.3, 0.0], quat_from_euler(Euler(yaw=-np.pi/2))), #cup_name: ([cup_x, 0.0, 0.0], unit_quat()), } with ClientSaver(world.perception.client): for name, pose in initial_poses.items(): point, quat = pose point[2] += stable_z(world.perception.sim_bodies[name], table_body) + Z_EPSILON world.perception.set_pose(name, point, quat) shelf_body = world.perception.sim_bodies[shelf_name] #print([str(get_link_name(shelf_body, link)) for link in get_links(shelf_body)]) #print([str(get_link_name(shelf_body, link)) for link in get_links(world.perception.sim_bodies[cup_name])]) #shelf_link = None shelf_link = link_from_name(shelf_body, start_link) cup_z = stable_z(world.perception.sim_bodies[cup_name], shelf_body, surface_link=shelf_link) + Z_EPSILON world.perception.set_pose(cup_name, [cup_x, 0.1, cup_z], unit_quat()) update_world(world, table_body, camera_point=np.array([-0.5, -1, 1.5])) init = [ ('Stackable', cup_name, shelf_name, end_link), ] goal = [ ('On', cup_name, shelf_name, end_link), #('On', cup_name, TABLE_NAME, TOP), #('Holding', cup_name), ] task = Task(init=init, goal=goal, arms=arms) return world, task
def attach(self, arm, obj, **kwargs): self.holding[arm] = obj assert obj not in self.attachments body = self.world.get_body(obj) with ClientSaver(self.client): if arm == 'l': frame = "left_gripper" elif arm == 'r': frame = "right_gripper" else: raise ValueError("Arm should be l or r but was {}".format(arm)) robot_link = link_from_name(self.robot, PR2_TOOL_FRAMES[frame]) self.attachments[obj] = add_fixed_constraint( body, self.robot, robot_link, max_force=self.attachment_force)
def get_ik_generator(robot, arm, tool_pose, **kwargs): from .ikfast_eth_rfl import get_ik world_from_base = get_link_pose(robot, link_from_name(robot, IK_BASE_FRAMES[arm])) base_from_tool = multiply(invert(world_from_base), tool_pose) base_from_ik = multiply(base_from_tool, get_tool_from_ik(robot, arm)) sampled_limits = get_ik_limits(robot, get_torso_joint(robot, arm), **kwargs) while True: sampled_values = [random.uniform(*sampled_limits)] ik_joints = get_torso_arm_joints(robot, arm) confs = compute_inverse_kinematics(get_ik, base_from_ik, sampled_values) yield [q for q in confs if not violates_limits(robot, ik_joints, q)]
def plan_approach(end_effector, print_traj, collision_fn, approach_distance=APPROACH_DISTANCE): # TODO: collisions at the ends of elements if approach_distance == 0: return Command([print_traj]) robot = end_effector.robot joints = get_movable_joints(robot) extend_fn = get_extend_fn(robot, joints, resolutions=0.25 * JOINT_RESOLUTIONS) tool_link = link_from_name(robot, TOOL_LINK) approach_pose = Pose(Point(z=-approach_distance)) #element = print_traj.element #midpoint = get_point(element) #points = map(point_from_pose, [print_traj.tool_path[0], print_traj.tool_path[-1]]) #midpoint = np.average(list(points), axis=0) #draw_point(midpoint) #element_to_base = 0.05*get_unit_vector(get_point(robot) - midpoint) #retreat_pose = Pose(Point(element_to_base)) # TODO: perpendicular to element # TODO: solve_ik start_conf = print_traj.path[0] set_joint_positions(robot, joints, start_conf) initial_pose = multiply(print_traj.tool_path[0], approach_pose) #draw_pose(initial_pose) #wait_if_gui() initial_conf = inverse_kinematics(robot, tool_link, initial_pose) if initial_conf is None: return None initial_path = [initial_conf] + list(extend_fn(initial_conf, start_conf)) if any(map(collision_fn, initial_path)): return None initial_traj = MotionTrajectory(robot, joints, initial_path) end_conf = print_traj.path[-1] set_joint_positions(robot, joints, end_conf) final_pose = multiply(print_traj.tool_path[-1], approach_pose) final_conf = inverse_kinematics(robot, tool_link, final_pose) if final_conf is None: return None final_path = [end_conf] + list(extend_fn( end_conf, final_conf)) # TODO: version that includes the endpoints if any(map(collision_fn, final_path)): return None final_traj = MotionTrajectory(robot, joints, final_path) return Command([initial_traj, print_traj, final_traj])