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 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_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 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 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 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 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 create_inverse_reachability2(robot, body, table, arm, grasp_type, max_attempts=500, num_samples=500): tool_link = get_gripper_link(robot, arm) problem = MockProblem(robot, fixed=[table], grasp_types=[grasp_type]) placement_gen_fn = get_stable_gen(problem) grasp_gen_fn = get_grasp_gen(problem, collisions=True) ik_ir_fn = get_ik_ir_gen(problem, max_attempts=max_attempts, learned=False, teleport=True) placement_gen = placement_gen_fn(body, table) grasps = list(grasp_gen_fn(body)) print('Grasps:', len(grasps)) # TODO: sample the torso height # TODO: consider IK with respect to the torso frame start_time = time.time() gripper_from_base_list = [] while len(gripper_from_base_list) < num_samples: [(p,)] = next(placement_gen) (g,) = random.choice(grasps) output = next(ik_ir_fn(arm, body, p, g), None) if output is None: print('Failed to find a solution after {} attempts'.format(max_attempts)) else: (_, ac) = output [at,] = ac.commands at.path[-1].assign() gripper_from_base = multiply(invert(get_link_pose(robot, tool_link)), get_base_pose(robot)) gripper_from_base_list.append(gripper_from_base) print('{} / {} [{:.3f}]'.format( len(gripper_from_base_list), num_samples, elapsed_time(start_time))) wait_if_gui() return save_inverse_reachability(robot, arm, grasp_type, tool_link, gripper_from_base_list)
def create_inverse_reachability(robot, body, table, arm, grasp_type, max_attempts=500, num_samples=500): tool_link = get_gripper_link(robot, arm) robot_saver = BodySaver(robot) gripper_from_base_list = [] grasps = GET_GRASPS[grasp_type](body) start_time = time.time() while len(gripper_from_base_list) < num_samples: box_pose = sample_placement(body, table) set_pose(body, box_pose) grasp_pose = random.choice(grasps) gripper_pose = multiply(box_pose, invert(grasp_pose)) for attempt in range(max_attempts): robot_saver.restore() base_conf = next(uniform_pose_generator(robot, gripper_pose)) #, reachable_range=(0., 1.))) #set_base_values(robot, base_conf) set_group_conf(robot, 'base', base_conf) if pairwise_collision(robot, table): continue grasp_conf = pr2_inverse_kinematics(robot, arm, gripper_pose) #, nearby_conf=USE_CURRENT) #conf = inverse_kinematics(robot, link, gripper_pose) if (grasp_conf is None) or pairwise_collision(robot, table): continue gripper_from_base = multiply(invert(get_link_pose(robot, tool_link)), get_base_pose(robot)) #wait_if_gui() gripper_from_base_list.append(gripper_from_base) print('{} / {} | {} attempts | [{:.3f}]'.format( len(gripper_from_base_list), num_samples, attempt, elapsed_time(start_time))) wait_if_gui() break else: print('Failed to find a kinematic solution after {} attempts'.format(max_attempts)) return save_inverse_reachability(robot, arm, grasp_type, tool_link, gripper_from_base_list)
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 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(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 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 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 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 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 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 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 _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 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))) for pose in interpolate_poses(start_pose, end_pose, pos_step_size=0.01): conf = next( closest_inverse_kinematics(robot, info, tool_link, pose, **kwargs), None) if conf is None: print('Failure!') wait_for_user() break set_joint_positions(robot, ik_joints, conf) wait_for_user()
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 solve_trac_ik(self, world_from_tool, nearby_tolerance=INF): assert self.ik_solver is not None init_lower, init_upper = self.ik_solver.get_joint_limits() base_link = link_from_name(self.robot, self.ik_solver.base_link) world_from_base = get_link_pose(self.robot, base_link) tip_link = link_from_name(self.robot, self.ik_solver.tip_link) tool_from_tip = multiply( invert(get_link_pose(self.robot, self.tool_link)), get_link_pose(self.robot, tip_link)) world_from_tip = multiply(world_from_tool, tool_from_tip) base_from_tip = multiply(invert(world_from_base), world_from_tip) joints = joints_from_names( self.robot, self.ik_solver.joint_names) # self.ik_solver.link_names seed_state = get_joint_positions(self.robot, joints) # seed_state = [0.0] * self.ik_solver.number_of_joints lower, upper = init_lower, init_upper if nearby_tolerance < INF: tolerance = nearby_tolerance * np.ones(len(joints)) lower = np.maximum(lower, seed_state - tolerance) upper = np.minimum(upper, seed_state + tolerance) self.ik_solver.set_joint_limits(lower, upper) (x, y, z), (rx, ry, rz, rw) = base_from_tip # TODO: can also adjust tolerances conf = self.ik_solver.get_ik(seed_state, x, y, z, rx, ry, rz, rw) self.ik_solver.set_joint_limits(init_lower, init_upper) if conf is None: return conf # if nearby_tolerance < INF: # print(lower.round(3)) # print(upper.round(3)) # print(conf) # print(get_difference(seed_state, conf).round(3)) set_joint_positions(self.robot, joints, conf) return get_configuration(self.robot)
def test(object_name, pose, base_conf): if object_name in ALL_SURFACES: surface_name = object_name if surface_name not in vertices_from_surface: vertices_from_surface[surface_name] = grow_polygon( map(point_from_pose, load_inverse_placements(world, surface_name)), radius=GROW_INVERSE_BASE) if not vertices_from_surface[surface_name]: return False base_conf.assign() pose.assign() surface = surface_from_name(surface_name) world_from_surface = get_link_pose( world.kitchen, link_from_name(world.kitchen, surface.link)) world_from_base = get_link_pose(world.robot, world.base_link) surface_from_base = multiply(invert(world_from_surface), world_from_base) #result = is_point_in_polygon(point_from_pose(surface_from_base), vertices_from_surface[surface_name]) #if not result: # draw_pose(surface_from_base) # points = [Point(x, y, 0) for x, y, in vertices_from_surface[surface_name]] # add_segments(points, closed=True) # wait_for_user() return is_point_in_polygon(point_from_pose(surface_from_base), vertices_from_surface[surface_name]) else: if not base_from_objects: return False base_conf.assign() pose.assign() world_from_base = get_link_pose(world.robot, world.base_link) world_from_object = pose.get_world_from_body() base_from_object = multiply(invert(world_from_base), world_from_object) return is_point_in_polygon(point_from_pose(base_from_object), base_from_objects)
def test_ik(robot, node_order, node_points): link = link_from_name(robot, TOOL_NAME) movable_joints = get_movable_joints(robot) sample_fn = get_sample_fn(robot, movable_joints) for n in node_order: point = node_points[n] set_joint_positions(robot, movable_joints, sample_fn()) conf = inverse_kinematics(robot, link, (point, None)) if conf is not None: set_joint_positions(robot, movable_joints, conf) continue link_point, link_quat = get_link_pose(robot, link) #print(point, link_point) #user_input('Continue?') wait_for_user()
def gen(knob_name): obstacles = world.static_obstacles knob_link = link_from_name(world.kitchen, knob_name) pose = get_link_pose(world.kitchen, knob_link) #pose = RelPose(world.kitchen, knob_link, init=True) presses = cycle(get_grasp_presses(world, knob_name)) grasp = next(presses) gripper_pose = multiply(pose, invert( grasp.grasp_pose)) # w_f_g = w_f_o * (g_f_o)^-1 if learned: base_generator = cycle(load_pull_base_poses(world, knob_name)) else: base_generator = uniform_pose_generator(world.robot, gripper_pose) safe_base_generator = inverse_reachability(world, base_generator, obstacles=obstacles, **kwargs) while True: for i in range(max_attempts): try: base_conf = next(safe_base_generator) except StopIteration: return if base_conf is None: yield None continue grasp = next(presses) randomize = (random.random() < P_RANDOMIZE_IK) ik_outputs = next( plan_press(world, knob_name, pose, grasp, base_conf, obstacles, randomize=randomize, **kwargs), None) if ik_outputs is not None: print('Press succeeded after {} attempts'.format(i)) yield (base_conf, ) + ik_outputs break else: if PRINT_FAILURES: print( 'Press failure after {} attempts'.format(max_attempts)) #if not pose.init: # break yield None
def get_ik_generator(robot, tool_pose, track_limits=False, prev_free_list=[]): from .ikfast_abb_irb6600_track import get_ik world_from_base = get_link_pose(robot, link_from_name(robot, BASE_FRAME)) base_from_tool = multiply(invert(world_from_base), tool_pose) base_from_ik = multiply(base_from_tool, get_tool_from_ik(robot)) sampled_limits = get_ik_limits(robot, joint_from_name(robot, *TRACK_JOINT), track_limits) while True: if not prev_free_list: sampled_values = [random.uniform(*sampled_limits)] else: sampled_values = prev_free_list ik_joints = get_track_arm_joints(robot) 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 test(joint_name, base_conf): if not DOOR_PROXIMITY: return True if joint_name not in vertices_from_joint: base_confs = list(load_pull_base_poses(world, joint_name)) vertices_from_joint[joint_name] = grow_polygon( base_confs, radius=GROW_INVERSE_BASE) if not vertices_from_joint[joint_name]: return False # TODO: can't open hitman_drawer_top_joint any more # Likely due to conservative carter geometry base_conf.assign() base_point = point_from_pose( get_link_pose(world.robot, world.base_link)) return is_point_in_polygon(base_point[:2], vertices_from_joint[joint_name])
def is_robot_visible(world, links): for link in links: link_point = point_from_pose(get_link_pose(world.robot, link)) visible = False for camera_body, camera_matrix, camera_depth in world.cameras.values(): camera_pose = get_pose(camera_body) #camera_point = point_from_pose(camera_pose) #add_line(link_point, camera_point) if is_visible_point(camera_matrix, camera_depth, link_point, camera_pose): visible = True break if not visible: return False #wait_for_user() return True
def test_grasps(robot, block): for arm in ARMS: gripper_joints = get_gripper_joints(robot, arm) tool_link = link_from_name(robot, TOOL_LINK.format(arm)) tool_pose = get_link_pose(robot, tool_link) #handles = draw_pose(tool_pose) #grasps = get_top_grasps(block, under=True, tool_pose=unit_pose()) grasps = get_side_grasps(block, under=True, tool_pose=unit_pose()) for i, grasp_pose in enumerate(grasps): block_pose = multiply(tool_pose, grasp_pose) set_pose(block, block_pose) close_until_collision(robot, gripper_joints, bodies=[block], open_conf=get_open_positions(robot, arm), closed_conf=get_closed_positions(robot, arm)) handles = draw_pose(block_pose) wait_if_gui('Grasp {}'.format(i)) remove_handles(handles)