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 test_caching(robot, obstacles): with timer(message='{:f}'): #update_scene() # 5.19752502441e-05 step_simulation() # 0.000210046768188 with timer(message='{:f}'): #print(get_aabb(robot, link=None, only_collision=True)) print(contact_collision()) # 2.50339508057e-05 for _ in range(3): with timer(message='{:f}'): #print(get_aabb(robot, link=None, only_collision=True)) # Recomputes each time print(contact_collision()) # 1.69277191162e-05 print() obstacle = obstacles[-1] #for link in get_all_links(robot): # set_collision_pair_mask(robot, obstacle, link1=link, enable=False) # Doesn't seem to affect pairwise_collision with timer('{:f}'): print(pairwise_collision(robot, obstacle)) # 0.000031 links = get_all_links(robot) links = [link for link in get_all_links(robot) if can_collide(robot, link)] #links = randomize(links) with timer('{:f}'): print( any( pairwise_collision(robot, obstacle, link1=link) for link in links # 0.000179 )) #if aabb_overlap(get_aabb(robot, link), get_aabb(obstacles[-1])))) #if can_collide(robot, link))) with timer('{:f}'): print(pairwise_collision(robot, obstacle))
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 element_collision_fn(q): if collision_fn(q): return True #for body in get_bodies_in_region(get_aabb(robot)): # Perform per link? # if (element_from_body.get(body, None) in printed_elements) and pairwise_collision(robot, body): # return True for hull, bodies in hulls.items(): if pairwise_collision(robot, hull) and any(pairwise_collision(robot, body) for body in bodies): return True return False
def lower_spoon(world, bowl_name, spoon_name, min_spoon_z, max_spoon_z): assert min_spoon_z <= max_spoon_z bowl_body = world.get_body(bowl_name) bowl_urdf_from_center = get_urdf_from_base( bowl_body) # get_urdf_from_center spoon_body = world.get_body(spoon_name) spoon_quat = lookup_orientation(spoon_name, STIR_QUATS) spoon_urdf_from_center = get_urdf_from_base( spoon_body, reference_quat=spoon_quat) # get_urdf_from_center # Keeping the orientation consistent for generalization purposes # TODO: what happens when the base isn't flat (e.g. bowl) bowl_pose = get_pose(bowl_body) stir_z = None for z in np.arange(max_spoon_z, min_spoon_z, step=-0.005): bowl_from_stirrer = multiply(bowl_urdf_from_center, Pose(Point(z=z)), invert(spoon_urdf_from_center)) set_pose(spoon_body, multiply(bowl_pose, bowl_from_stirrer)) #wait_for_user() if pairwise_collision(bowl_body, spoon_body): # Want to be careful not to make contact with the base break stir_z = z return stir_z
def sample_placements(body_surfaces, obstacles=None, savers=[], min_distances={}): if obstacles is None: obstacles = OrderedSet(get_bodies()) - set(body_surfaces) obstacles = list(obstacles) savers = list(savers) + [BodySaver(obstacle) for obstacle in obstacles] if not isinstance(min_distances, dict): min_distances = {body: min_distances for body in body_surfaces} # TODO: max attempts here for body, surface in body_surfaces.items(): # TODO: shuffle min_distance = min_distances.get(body, 0.) while True: pose = sample_placement(body, surface) if pose is None: for saver in savers: saver.restore() return False for saver in savers: obstacle = saver.body if obstacle in [body, surface]: continue saver.restore() if pairwise_collision(body, obstacle, max_distance=min_distance): break else: obstacles.append(body) break for saver in savers: saver.restore() return True
def sample_placement(world, entity_name, surface_name, min_distance=0.05, robust_radius=0.025, **kwargs): entity_body = world.get_body(entity_name) placement_gen = get_stable_gen(world, pos_scale=0, rot_scale=0, robust_radius=robust_radius, **kwargs) for pose, in placement_gen(entity_name, surface_name): x, y, z = point_from_pose(pose.get_world_from_body()) if x < MIN_PLACEMENT_X: continue pose.assign() if not any( pairwise_collision( entity_body, obst_body, max_distance=min_distance) for obst_body in world.body_from_name.values() if entity_body != obst_body): return pose raise RuntimeError( 'Unable to find a pose for object {} on surface {}'.format( entity_name, surface_name))
def compute_cfree(body, poses, obstacles=[]): cfree_poses = set() for pose in poses: pose.assign() if not any(pairwise_collision(body, obst) for obst in obstacles): cfree_poses.add(pose) return cfree_poses
def test(o1, rp1, o2, rp2, s): if not collisions or (o1 == o2): return True if isinstance(rp1, SurfaceDist) or isinstance(rp2, SurfaceDist): return True # TODO: perform this probabilistically rp1.assign() rp2.assign() return not pairwise_collision(world.get_body(o1), world.get_body(o2))
def fn(body, pose, grasp): obstacles = [body] + fixed gripper_pose = end_effector_from_body(pose.pose, grasp.grasp_pose) approach_pose = approach_from_grasp(grasp.approach_pose, gripper_pose) draw_pose(gripper_pose, length=0.04) draw_pose(approach_pose, length=0.04) for _ in range(num_attempts): if USE_IKFAST: q_approach = sample_tool_ik(robot, approach_pose) if q_approach is not None: set_joint_positions(robot, movable_joints, q_approach) else: set_joint_positions(robot, movable_joints, sample_fn()) # Random seed q_approach = inverse_kinematics(robot, grasp.link, approach_pose) if (q_approach is None) or any(pairwise_collision(robot, b) for b in obstacles): continue conf = BodyConf(robot, joints=movable_joints) if USE_IKFAST: q_grasp = sample_tool_ik(robot, gripper_pose, closest_only=True) if q_grasp is not None: set_joint_positions(robot, movable_joints, q_grasp) else: q_grasp = inverse_kinematics(robot, grasp.link, gripper_pose) if (q_grasp is None) or any(pairwise_collision(robot, b) for b in obstacles): continue if teleport: path = [q_approach, q_grasp] else: conf.assign() #direction, _ = grasp.approach_pose #path = workspace_trajectory(robot, grasp.link, point_from_pose(approach_pose), -direction, # quat_from_pose(approach_pose)) path = plan_direct_joint_motion(robot, conf.joints, q_grasp, obstacles=obstacles, \ self_collisions=self_collisions) if path is None: if DEBUG_FAILURE: user_input('Approach motion failed') continue command = Command([BodyPath(robot, path, joints=movable_joints), Attach(body, robot, grasp.link), BodyPath(robot, path[::-1], joints=movable_joints, attachments=[grasp])]) return (conf, command) # TODO: holding collisions return None
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 test(o1, wp1): if isinstance(wp1, SurfaceDist): return True if not collisions or (wp1.support not in DRAWERS): return True body = world.get_body(o1) wp1.assign() obstacles = world.static_obstacles if any(pairwise_collision(body, obst) for obst in obstacles): return False return True
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(bq, o2, wp2): if not collisions: return True if isinstance(wp2, SurfaceDist): return True # TODO: perform this probabilistically bq.assign() world.carry_conf.assign() wp2.assign() obstacles = get_link_obstacles(world, o2) return not any( pairwise_collision(world.robot, obst) for obst in obstacles)
def test_base_conf(world, bq, obstacles, min_distance=0.0): robot_links = [world.franka_link, world.gripper_link ] if world.is_real() else [] bq.assign() for conf in world.special_confs: # Could even sample a special visible conf for this base_conf conf.assign() if not is_robot_visible(world, robot_links) or any( pairwise_collision(world.robot, b, max_distance=min_distance) for b in obstacles): return False return True
def test_supported(world, body, surface_name, collisions=True): # TODO: is_center_on_aabb or is_placed_on_aabb surface_aabb = compute_surface_aabb(world, surface_name) # TODO: epsilon thresholds? if not is_placed_on_aabb(body, surface_aabb): # , above_epsilon=z_offset+1e-3): return False obstacles = world.static_obstacles | get_surface_obstacles(world, surface_name) if not collisions: obstacles = set() #print([get_link_name(obst[0], list(obst[1])[0]) for obst in obstacles # if pairwise_collision(body, obst)]) return not any(pairwise_collision(body, obst) for obst in obstacles)
def test(o1, wp1, o2, wp2): if isinstance(wp1, SurfaceDist) or isinstance(wp2, SurfaceDist): return True if not collisions or (o1 == o2) or (o2 == wp1.support): # DRAWERS return True body = world.get_body(o1) wp1.assign() wp2.assign() if any( pairwise_collision(body, obst) for obst in get_surface_obstacles(world, o2)): return False return True
def create_inverse_reachability(robot, body, table, arm, grasp_type, num_samples=500): link = get_gripper_link(robot, arm) movable_joints = get_movable_joints(robot) default_conf = get_joint_positions(robot, movable_joints) gripper_from_base_list = [] grasps = GET_GRASPS[grasp_type](body) 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)) set_joint_positions(robot, movable_joints, default_conf) base_conf = next(uniform_pose_generator(robot, gripper_pose)) set_base_values(robot, base_conf) if pairwise_collision(robot, table): continue conf = inverse_kinematics(robot, link, gripper_pose) if (conf is None) or pairwise_collision(robot, table): continue gripper_from_base = multiply(invert(get_link_pose(robot, link)), get_pose(robot)) gripper_from_base_list.append(gripper_from_base) print('{} / {}'.format(len(gripper_from_base_list), num_samples)) filename = IR_FILENAME.format(grasp_type, arm) path = get_database_file(filename) data = { 'filename': filename, 'robot': get_body_name(robot), 'grasp_type': grasp_type, 'arg': arm, 'carry_conf': get_carry_conf(arm, grasp_type), 'gripper_link': link, 'gripper_from_base': gripper_from_base_list, } write_pickle(path, data) return path
def sample(self, discrete=True): # TODO: timeout if unable to find while True: poses = {} for name, pose_dist in randomize(self.pose_dists.items()): body = self.world.get_body(name) pose = pose_dist.sample_discrete( ) if discrete else pose_dist.sample() pose.assign() if any( pairwise_collision(body, self.world.get_body(other)) for other in poses): break poses[name] = pose else: return poses
def test(detect, obj_name, pose): if (detect.name == obj_name) or (detect.surface_name == obj_name) or isinstance( pose, SurfaceDist): return True move_occluding(world) detect.pose.assign() pose.assign() body = world.get_body(detect.name) obstacles = get_link_obstacles(world, obj_name) if any(pairwise_collision(body, obst) for obst in obstacles): return False visible = not obstacles & detect.compute_occluding() #if not visible: # handles = detect.draw() # wait_for_user() # remove_handles(handles) return visible
def is_approach_safe(world, obj_name, pose, grasp, obstacles): assert pose.support is not None obj_body = world.get_body(obj_name) pose.assign() # May set the drawer confs as well set_joint_positions(world.gripper, get_movable_joints(world.gripper), world.open_gq.values) #set_renderer(enable=True) for _ in iterate_approach_path(world, pose, grasp, body=obj_body): #for link in get_all_links(world.gripper): # set_color(world.gripper, apply_alpha(np.zeros(3)), link) #wait_for_user() if any( pairwise_collision(world.gripper, obst ) # or pairwise_collision(obj_body, obst) for obst in obstacles): print('Unsafe approach!') #wait_for_user() return False return True
def is_safe(self, obstacles): checked_bodies = set(self.safe_from_body) & set(obstacles) if any(not self.safe_from_body[e] for e in checked_bodies): return False unchecked_bodies = randomize(set(obstacles) - checked_bodies) if not unchecked_bodies: return True for tool_pose in randomize(self.tool_path): self.extrusion.end_effector.set_pose(tool_pose) tool_aabb = get_aabb( self.extrusion.end_effector.body) # TODO: cache nearby_bodies nearby_bodies = { b for b, _ in get_bodies_in_region(tool_aabb) if b in unchecked_bodies } for body in nearby_bodies: if pairwise_collision(self.extrusion.end_effector.body, body): self.safe_from_body[body] = False return False return True
def test(o1, wp1, g1, o2, wp2): # o1 will always be a movable object if isinstance(wp2, SurfaceDist): return True # TODO: perform this probabilistically if not collisions or (o1 == o2) or (o2 == wp1.support): return True # TODO: could define these on sets of samples to prune all at once body = world.get_body(o1) wp2.assign() obstacles = get_link_obstacles(world, o2) # - {body} if not obstacles: return True for _ in iterate_approach_path(world, wp1, g1, body=body): if any( pairwise_collision(part, obst) for part in [world.gripper, body] for obst in obstacles): # TODO: some collisions the bottom drawer and the top drawer handle #print(o1, wp1.support, o2, wp2.support) #wait_for_user() return False return True
def test(at, o, wp): if not collisions: return True # TODO: check door collisions # TODO: still need to check static links at least once if isinstance(wp, SurfaceDist): return True # TODO: perform this probabilistically wp.assign() state = at.context.copy() state.assign() all_bodies = { body for command in at.commands for body in command.bodies } for command in at.commands: obstacles = get_link_obstacles(world, o) - all_bodies # TODO: why did I previously remove o at p? #obstacles = get_link_obstacles(world, o) - command.bodies # - p.bodies # Doesn't include o at p if not obstacles: continue if isinstance(command, DoorTrajectory): [door_joint] = command.door_joints surface_name = get_link_name(world.kitchen, child_link_from_joint(door_joint)) if wp.support == surface_name: return True for _ in command.iterate(state): state.derive() #for attachment in state.attachments.values(): # if any(pairwise_collision(attachment.child, obst) for obst in obstacles): # return False # TODO: just check collisions with moving links if any( pairwise_collision(world.robot, obst) for obst in obstacles): #print(at, o, p) #wait_for_user() return False return True
def plan_workspace(world, tool_path, obstacles, randomize=True, teleport=False): # Assuming that pairs of fixed things aren't in collision at this point moving_links = get_moving_links(world.robot, world.arm_joints) robot_obstacle = (world.robot, frozenset(moving_links)) distance_fn = get_distance_fn(world.robot, world.arm_joints) if randomize: sample_fn = get_sample_fn(world.robot, world.arm_joints) set_joint_positions(world.robot, world.arm_joints, sample_fn()) else: world.carry_conf.assign() arm_path = [] for i, tool_pose in enumerate(tool_path): #set_joint_positions(world.kitchen, [door_joint], door_path[i]) tolerance = INF if i == 0 else NEARBY_PULL full_arm_conf = world.solve_inverse_kinematics( tool_pose, nearby_tolerance=tolerance) if full_arm_conf is None: # TODO: this fails when teleport=True if PRINT_FAILURES: print('Workspace kinematic failure') return None if any(pairwise_collision(robot_obstacle, b) for b in obstacles): if PRINT_FAILURES: print('Workspace collision failure') return None arm_conf = get_joint_positions(world.robot, world.arm_joints) if arm_path and not teleport: distance = distance_fn(arm_path[-1], arm_conf) if MAX_CONF_DISTANCE < distance: if PRINT_FAILURES: print( 'Workspace proximity failure (distance={:.5f})'.format( distance)) return None arm_path.append(arm_conf) # wait_for_user() return arm_path
def validate_trajectories(element_bodies, fixed_obstacles, trajectories): if trajectories is None: return False # TODO: combine all validation procedures remove_all_debug() for body in element_bodies.values(): set_color(body, np.zeros(4)) print('Trajectories:', len(trajectories)) obstacles = list(fixed_obstacles) for i, trajectory in enumerate(trajectories): for _ in trajectory.iterate(): #wait_for_user() if any(pairwise_collision(trajectory.robot, body) for body in obstacles): if has_gui(): print('Collision on trajectory {}'.format(i)) wait_for_user() return False if isinstance(trajectory, PrintTrajectory): body = element_bodies[trajectory.element] set_color(body, apply_alpha(RED)) obstacles.append(body) return True
def tool_path_collision(end_effector, element_pose, translation_path, direction, angle, reverse, obstacles): # TODO: allow sampling in the full sphere by checking collision with an element while sliding for tool_pose in randomize( compute_tool_path(element_pose, translation_path, direction, angle, reverse)): end_effector.set_pose(tool_pose) #bodies = obstacles tool_aabb = get_aabb(end_effector.body) # TODO: could just translate #handles = draw_aabb(tool_aabb) bodies = { b for b, _ in get_bodies_in_region(tool_aabb) if b in obstacles } #print(bodies) #for body, link in bodies: # handles.extend(draw_aabb(get_aabb(body, link))) #wait_for_user() #remove_handles(handles) if any(pairwise_collision(end_effector.body, obst) for obst in bodies): # TODO: sort by angle with smallest violation return True return False
def test_bounding(q): set_joint_positions(robot, joints, q) collision = (bounding is not None) and pairwise_collision(robot, bounding, max_distance=buffer) # body_collision return q, collision
def main(floor_width=2.0): # Creates a pybullet world and a visualizer for it connect(use_gui=True) identity_pose = (unit_point(), unit_quat()) origin_handles = draw_pose( identity_pose, length=1.0 ) # Draws the origin coordinate system (x:RED, y:GREEN, z:BLUE) # Bodies are described by an integer index floor = create_box(w=floor_width, l=floor_width, h=0.001, color=TAN) # Creates a tan box object for the floor set_point(floor, [0, 0, -0.001 / 2.]) # Sets the [x,y,z] translation of the floor obstacle = create_box(w=0.5, l=0.5, h=0.1, color=RED) # Creates a red box obstacle set_point( obstacle, [0.5, 0.5, 0.1 / 2.]) # Sets the [x,y,z] position of the obstacle print('Position:', get_point(obstacle)) set_euler(obstacle, [0, 0, np.pi / 4 ]) # Sets the [roll,pitch,yaw] orientation of the obstacle print('Orientation:', get_euler(obstacle)) with LockRenderer( ): # Temporarily prevents the renderer from updating for improved loading efficiency with HideOutput(): # Temporarily suppresses pybullet output robot = load_model(ROOMBA_URDF) # Loads a robot from a *.urdf file robot_z = stable_z( robot, floor ) # Returns the z offset required for robot to be placed on floor set_point(robot, [0, 0, robot_z]) # Sets the z position of the robot dump_body(robot) # Prints joint and link information about robot set_all_static() # Joints are also described by an integer index # The turtlebot has explicit joints representing x, y, theta x_joint = joint_from_name(robot, 'x') # Looks up the robot joint named 'x' y_joint = joint_from_name(robot, 'y') # Looks up the robot joint named 'y' theta_joint = joint_from_name( robot, 'theta') # Looks up the robot joint named 'theta' joints = [x_joint, y_joint, theta_joint] base_link = link_from_name( robot, 'base_link') # Looks up the robot link named 'base_link' world_from_obstacle = get_pose( obstacle ) # Returns the pose of the origin of obstacle wrt the world frame obstacle_aabb = get_subtree_aabb(obstacle) draw_aabb(obstacle_aabb) random.seed(0) # Sets the random number generator state handles = [] for i in range(10): for handle in handles: remove_debug(handle) print('\nIteration: {}'.format(i)) x = random.uniform(-floor_width / 2., floor_width / 2.) set_joint_position(robot, x_joint, x) # Sets the current value of the x joint y = random.uniform(-floor_width / 2., floor_width / 2.) set_joint_position(robot, y_joint, y) # Sets the current value of the y joint yaw = random.uniform(-np.pi, np.pi) set_joint_position(robot, theta_joint, yaw) # Sets the current value of the theta joint values = get_joint_positions( robot, joints) # Obtains the current values for the specified joints print('Joint values: [x={:.3f}, y={:.3f}, yaw={:.3f}]'.format(*values)) world_from_robot = get_link_pose( robot, base_link) # Returns the pose of base_link wrt the world frame position, quaternion = world_from_robot # Decomposing pose into position and orientation (quaternion) x, y, z = position # Decomposing position into x, y, z print('Base link position: [x={:.3f}, y={:.3f}, z={:.3f}]'.format( x, y, z)) euler = euler_from_quat( quaternion) # Converting from quaternion to euler angles roll, pitch, yaw = euler # Decomposing orientation into roll, pitch, yaw print('Base link orientation: [roll={:.3f}, pitch={:.3f}, yaw={:.3f}]'. format(roll, pitch, yaw)) handles.extend( draw_pose(world_from_robot, length=0.5) ) # # Draws the base coordinate system (x:RED, y:GREEN, z:BLUE) obstacle_from_robot = multiply( invert(world_from_obstacle), world_from_robot) # Relative transformation from robot to obstacle robot_aabb = get_subtree_aabb( robot, base_link) # Computes the robot's axis-aligned bounding box (AABB) lower, upper = robot_aabb # Decomposing the AABB into the lower and upper extrema center = (lower + upper) / 2. # Computing the center of the AABB extent = upper - lower # Computing the dimensions of the AABB handles.extend(draw_aabb(robot_aabb)) collision = pairwise_collision( robot, obstacle ) # Checks whether robot is currently colliding with obstacle print('Collision: {}'.format(collision)) wait_for_duration(1.0) # Like sleep() but also updates the viewer wait_for_user() # Like raw_input() but also updates the viewer # Destroys the pybullet world disconnect()
def body_pair_collision(body1, body2, collision_buffer=COLLISION_BUFFER): if body1 == body2: return False return pairwise_collision(body1, body2, max_distance=collision_buffer)