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 sample_tool_ik(robot, tool_pose, max_attempts=10, closest_only=False, get_all=False, prev_free_list=[], **kwargs): generator = get_ik_generator(robot, tool_pose, prev_free_list=prev_free_list, **kwargs) ik_joints = get_movable_joints(robot) for _ in range(max_attempts): try: solutions = next(generator) if closest_only and solutions: current_conf = get_joint_positions(robot, ik_joints) solutions = [ min(solutions, key=lambda conf: get_distance(current_conf, conf)) ] solutions = list( filter( lambda conf: not violates_limits(robot, ik_joints, conf), solutions)) return solutions if get_all else select_solution( robot, ik_joints, solutions, **kwargs) except StopIteration: break return None
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 translate_linearly(world, distance): # TODO: could just apply in the base frame x, y, theta = get_joint_positions(world.robot, world.base_joints) pos = np.array([x, y]) goal_pos = pos + distance * unit_from_theta(theta) goal_pose = np.append(goal_pos, [theta]) return goal_pose
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 sample_tool_ik(robot, tool_pose, closest_only=False, get_all=False, **kwargs): generator = get_ik_generator(robot, tool_pose) ik_joints = get_movable_joints(robot) solutions = next(generator) if closest_only and solutions: current_conf = get_joint_positions(robot, ik_joints) solutions = [min(solutions, key=lambda conf: get_distance(current_conf, conf))] solutions = list(filter(lambda conf: not violates_limits(robot, ik_joints, conf), solutions)) return solutions if get_all else select_solution(robot, ik_joints, solutions, **kwargs)
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 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 solve_pybullet_ik(self, world_from_tool, nearby_tolerance): start_time = time.time() # Most of the time is spent creating the robot # TODO: use the waypoint version that doesn't repeatedly create the robot current_conf = get_joint_positions(self.robot, self.arm_joints) full_conf = sub_inverse_kinematics( self.robot, self.arm_joints[0], self.tool_link, world_from_tool, custom_limits=self.custom_limits ) # , max_iterations=1) # , **kwargs) conf = get_joint_positions(self.robot, self.arm_joints) max_distance = get_distance(current_conf, conf, norm=INF) if nearby_tolerance < max_distance: return None print('Nearby) time: {:.3f} | distance: {:.5f}'.format( elapsed_time(start_time), max_distance)) return full_conf
def is_gripper_closed(self): # TODO: base this on the object type if self.holding is not None: obj_type = type_from_name(self.holding) if obj_type not in TIN_OBJECTS: return False # each joint in [0.00, 0.04] (units coincide with meters on the physical gripper) current_gq = get_joint_positions(self.world.robot, self.world.gripper_joints) gripper_width = sum(current_gq) return gripper_width <= MIN_GRASP_WIDTH
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 follow_curve(robot, joints, curve, goal_t=None, time_step=None, max_velocities=MAX_VELOCITIES, **kwargs): if goal_t is None: goal_t = curve.x[-1] if time_step is None: time_step = 10 * get_time_step() #distance_fn = get_distance_fn(robot, joints, weights=None, norm=2) distance_fn = get_duration_fn(robot, joints, velocities=max_velocities, norm=INF) # get_distance positions = np.array(get_joint_positions(robot, joints)) closest_dist, closest_t = find_closest(positions, curve, t_range=(curve.x[0], goal_t), max_time=1e-1, max_iterations=INF, distance_fn=distance_fn, verbose=True) print('Closest dist: {:.3f} | Closest time: {:.3f}'.format( closest_dist, closest_t)) target_t = closest_t # TODO: condition based on closest_dist while True: print('\nTarget time: {:.3f} | Goal time: {}'.format(target_t, goal_t)) target_positions = curve(target_t) target_velocities = curve(target_t, nu=1) # TODO: draw the velocity #print('Positions: {} | Velocities: {}'.format(target_positions, target_velocities)) handles = draw_waypoint(target_positions) is_goal = (target_t == goal_t) position_tol = 1e-2 if is_goal else 1e-2 for output in control_state(robot, joints, target_positions=target_positions, target_velocities=target_velocities, position_tol=position_tol, velocity_tol=INF, max_velocities=max_velocities, **kwargs): yield output remove_handles(handles) target_t = min(goal_t, target_t + time_step) if is_goal: break
def plan_water_motion(body, joints, end_conf, attachment, obstacles=None, attachments=[], self_collisions=True, disabled_collisions=set(), max_distance=MAX_DISTANCE, weights=None, resolutions=None, reference_pose=unit_pose(), custom_limits={}, **kwargs): assert len(joints) == len(end_conf) sample_fn = get_sample_fn(body, joints, custom_limits=custom_limits) distance_fn = get_distance_fn(body, joints, weights=weights) extend_fn = get_extend_fn(body, joints, resolutions=resolutions) collision_fn = get_collision_fn(body, joints, obstacles, {attachment} | set(attachments), self_collisions, disabled_collisions, max_distance=max_distance, custom_limits=custom_limits) def water_test(q): if attachment is None: return False set_joint_positions(body, joints, q) attachment.assign() attachment_pose = get_pose(attachment.child) pose = multiply(attachment_pose, reference_pose) # TODO: confirm not inverted roll, pitch, _ = euler_from_quat(quat_from_pose(pose)) violation = (MAX_ROTATION < abs(roll)) or (MAX_ROTATION < abs(pitch)) #if violation: # TODO: check whether different confs can be waypoints for each object # print(q, violation) # wait_for_user() return violation invalid_fn = lambda q, **kwargs: water_test(q) or collision_fn(q, **kwargs) start_conf = get_joint_positions(body, joints) if not check_initial_end(start_conf, end_conf, invalid_fn): return None return birrt(start_conf, end_conf, distance_fn, sample_fn, extend_fn, invalid_fn, **kwargs)
def test_ikfast(pr2): from pybullet_tools.pr2_ik.ik import forward_kinematics, inverse_kinematics, get_tool_pose, get_ik_generator left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm']) #right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm']) torso_joints = joints_from_names(pr2, PR2_GROUPS['torso']) torso_left = torso_joints + left_joints print(get_link_pose(pr2, link_from_name(pr2, 'l_gripper_tool_frame'))) print(forward_kinematics('left', get_joint_positions(pr2, torso_left))) print(get_tool_pose(pr2, 'left')) arm = 'left' pose = get_tool_pose(pr2, arm) generator = get_ik_generator(pr2, arm, pose, torso_limits=False) for i in range(100): solutions = next(generator) print(i, len(solutions)) for q in solutions: set_joint_positions(pr2, torso_left, q) wait_for_interrupt()
def control_state(robot, joints, target_positions, target_velocities=None, position_tol=INF, velocity_tol=INF, max_velocities=None, verbose=True): # TODO: max_accelerations if target_velocities is None: target_velocities = np.zeros(len(joints)) if max_velocities is None: max_velocities = get_max_velocities(robot, joints) assert (max_velocities > 0).all() max_velocity_control_joints(robot, joints, positions=target_positions, velocities=target_velocities, max_velocities=max_velocities) for i in irange(INF): current_positions = np.array(get_joint_positions(robot, joints)) position_error = get_distance(current_positions, target_positions, norm=INF) current_velocities = np.array(get_joint_velocities(robot, joints)) velocity_error = get_distance(current_velocities, target_velocities, norm=INF) if verbose: # print('Positions: {} | Target positions: {}'.format(current_positions.round(N_DIGITS), target_positions.round(N_DIGITS))) # print('Velocities: {} | Target velocities: {}'.format(current_velocities.round(N_DIGITS), target_velocities.round(N_DIGITS))) print( 'Step: {} | Position error: {:.3f}/{:.3f} | Velocity error: {:.3f}/{:.3f}' .format(i, position_error, position_tol, velocity_error, velocity_tol)) # TODO: draw the tolerance interval if (position_error <= position_tol) and (velocity_error <= velocity_tol): return # TODO: declare success or failure by yielding or throwing an exception yield i
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 draw_last_roadmap(robot, joints, only_checked=False, linear=True, down_sample=None, **kwargs): q0 = get_joint_positions(robot, joints) handles = [] if not ROADMAPS: return handles roadmap = ROADMAPS[-1] for q in roadmap.samples: q = q if len(q) == 3 else np.append(q[:2], q0[2:]) # TODO: make a function handles.extend(draw_pose2d(q, z=DRAW_Z)) for v1, v2 in roadmap.edges: color = BLACK if roadmap.is_colliding(v1, v2): color = RED elif roadmap.is_safe(v1, v2): color = GREEN elif only_checked: continue if linear: path = [roadmap.samples[v1], roadmap.samples[v2]] else: path = roadmap.get_path(v1, v2) if down_sample is not None: path = path[::down_sample] + [path[-1]] #handles.extend(draw_path(path, **kwargs)) points = list( map(point_from_pose, [ pose_from_pose2d( q if len(q) == 3 else np.append(q[:2], q0[2:]), z=DRAW_Z) for q in path ])) handles.extend( add_line(p1, p2, color=color) for p1, p2 in get_pairs(points)) return handles
def rest_for_duration(self, duration): if not self.execute_motor_control: return sim_duration = 0.0 body = self.robot position_gain = 0.02 with ClientSaver(self.client): # TODO: apply to all joints dt = get_time_step() movable_joints = get_movable_joints(body) target_conf = get_joint_positions(body, movable_joints) while sim_duration < duration: p.setJointMotorControlArray( body, movable_joints, p.POSITION_CONTROL, targetPositions=target_conf, targetVelocities=[0.0] * len(movable_joints), positionGains=[position_gain] * len(movable_joints), # velocityGains=[velocity_gain] * len(movable_joints), physicsClientId=self.client) step_simulation() sim_duration += dt
def optimize_angle(end_effector, element_pose, translation, direction, reverse, candidate_angles, collision_fn, nearby=True, max_error=TRANSLATION_TOLERANCE): robot = end_effector.robot movable_joints = get_movable_joints(robot) best_error, best_angle, best_conf = max_error, None, None initial_conf = get_joint_positions(robot, movable_joints) for angle in candidate_angles: grasp_pose = get_grasp_pose(translation, direction, angle, reverse) # Pose_{world,EE} = Pose_{world,element} * Pose_{element,EE} # = Pose_{world,element} * (Pose_{EE,element})^{-1} target_pose = multiply(element_pose, invert(grasp_pose)) set_pose(end_effector.body, multiply(target_pose, end_effector.tool_from_ee)) if nearby: set_joint_positions(robot, movable_joints, initial_conf) conf = solve_ik(end_effector, target_pose, nearby=nearby) if (conf is None) or collision_fn(conf): continue set_joint_positions(robot, movable_joints, conf) link_pose = get_link_pose(robot, end_effector.tool_link) error = get_distance(point_from_pose(target_pose), point_from_pose(link_pose)) if error < best_error: # TODO: error a function of direction as well best_error, best_angle, best_conf = error, angle, conf #break if best_conf is not None: set_joint_positions(robot, movable_joints, best_conf) return best_angle, best_conf
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 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 solve_collision_free(door, obstacle, max_iterations=100, step_size=math.radians(5), min_distance=2e-2, draw=True): joints = get_movable_joints(door) door_link = child_link_from_joint(joints[-1]) # print(get_com_pose(door, door_link)) # print(get_link_inertial_pose(door, door_link)) # print(get_link_pose(door, door_link)) # draw_pose(get_com_pose(door, door_link)) handles = [] success = False start_time = time.time() for iteration in range(max_iterations): current_conf = np.array(get_joint_positions(door, joints)) collision_infos = get_closest_points(door, obstacle, link1=door_link, max_distance=min_distance) if not collision_infos: success = True break collision_infos = sorted(collision_infos, key=lambda info: info.contactDistance) collision_infos = collision_infos[:1] # TODO: average all these if draw: for collision_info in collision_infos: handles.extend(draw_collision_info(collision_info)) wait_if_gui() [collision_info] = collision_infos[:1] distance = collision_info.contactDistance print( 'Iteration: {} | Collisions: {} | Distance: {:.3f} | Time: {:.3f}'. format(iteration, len(collision_infos), distance, elapsed_time(start_time))) if distance >= min_distance: success = True break # TODO: convergence or decay in step size direction = step_size * get_unit_vector( collision_info.contactNormalOnB) # B->A (already normalized) contact_point = collision_info.positionOnA #com_pose = get_com_pose(door, door_link) # TODO: be careful here com_pose = get_link_pose(door, door_link) local_point = tform_point(invert(com_pose), contact_point) #local_point = unit_point() translate, rotate = compute_jacobian(door, door_link, point=local_point) delta_conf = np.array([ np.dot(translate[mj], direction) # + np.dot(rotate[mj], direction) for mj in movable_from_joints(door, joints) ]) new_conf = current_conf + delta_conf if violates_limits(door, joints, new_conf): break set_joint_positions(door, joints, new_conf) if draw: wait_if_gui() remove_handles(handles) print('Success: {} | Iteration: {} | Time: {:.3f}'.format( success, iteration, elapsed_time(start_time))) #quit() return success
def follow_curve_old(robot, joints, curve, goal_t=None): # TODO: unify with /Users/caelan/Programs/open-world-tamp/open_world/simulation/control.py if goal_t is None: goal_t = curve.x[-1] time_step = get_time_step() target_step = 10 * time_step #distance_fn = get_distance_fn(robot, joints, weights=None, norm=2) distance_fn = get_duration_fn(robot, joints, velocities=MAX_VELOCITIES, norm=INF) for i in irange(INF): # if (i % 10) != 0: # continue current_p = np.array(get_joint_positions(robot, joints)) current_v = np.array(get_joint_velocities(robot, joints)) goal_dist = distance_fn(current_p, curve(goal_t)) print('Positions: {} | Velocities: {} | Goal distance: {:.3f}'.format( current_p.round(N_DIGITS), current_v.round(N_DIGITS), goal_dist)) if goal_dist < 1e-2: return True # _, connection = mpc(current_p, current_v, curve, v_max=MAX_VELOCITIES, a_max=MAX_ACCELERATIONS, # dt_max=1e-1, max_time=1e-1) # assert connection is not None # target_t = 0.5*connection.x[-1] # target_p = connection(target_t) # target_v = connection(target_t, nu=1) # #print(target_p) closest_dist, closest_t = find_closest(current_p, curve, t_range=None, max_time=1e-2, max_iterations=INF, distance_fn=distance_fn, verbose=True) target_t = min(closest_t + target_step, curve.x[-1]) target_p = curve(target_t) #target_v = curve(target_t, nu=1) target_v = curve(closest_t, nu=1) #target_v = MAX_VELOCITIES #target_v = INF*np.zeros(len(joints)) handles = draw_waypoint(target_p) #times, confs = time_discretize_curve(curve, verbose=False, resolution=resolutions) # max_velocities=v_max, # set_joint_positions(robot, joints, target_p) max_velocity_control_joints(robot, joints, positions=target_p, velocities=target_v, max_velocities=MAX_VELOCITIES) #next_t = closest_t + time_step #next_p = current_p + current_v*time_step yield target_t actual_p = np.array(get_joint_positions(robot, joints)) actual_v = np.array(get_joint_velocities(robot, joints)) next_p = current_p + actual_v * time_step print('Predicted: {} | Actual: {}'.format(next_p.round(N_DIGITS), actual_p.round(N_DIGITS))) remove_handles(handles)
def main(): np.set_printoptions(precision=N_DIGITS, suppress=True, threshold=3) # , edgeitems=1) #, linewidth=1000) parser = argparse.ArgumentParser() parser.add_argument( '-a', '--algorithm', default='rrt_connect', # choices=[], help='The motion planning algorithm to use.') parser.add_argument('-c', '--cfree', action='store_true', help='When enabled, disables collision checking.') # parser.add_argument('-p', '--problem', default='test_pour', choices=sorted(problem_fn_from_name), # help='The name of the problem to solve.') parser.add_argument( '--holonomic', action='store_true', # '-h', help='') parser.add_argument('-l', '--lock', action='store_false', help='') parser.add_argument('-r', '--reversible', action='store_true', help='') parser.add_argument( '-s', '--seed', default=None, type=int, # None | 1 help='The random seed to use.') parser.add_argument('-n', '--num', default=10, type=int, help='The number of obstacles.') parser.add_argument('-o', '--orientation', action='store_true', help='') parser.add_argument('-v', '--viewer', action='store_false', help='') args = parser.parse_args() connect(use_gui=args.viewer) #set_aabb_buffer(buffer=1e-3) #set_separating_axis_collisions() #seed = 0 #seed = time.time() seed = args.seed if seed is None: seed = random.randint(0, 10**3 - 1) print('Seed:', seed) set_random_seed(seed=seed) # None: 2147483648 = 2**31 set_numpy_seed(seed=seed) #print('Random seed:', get_random_seed(), random.random()) #print('Numpy seed:', get_numpy_seed(), np.random.random()) ######################### robot, base_limits, goal_conf, obstacles = problem1(n_obstacles=args.num) custom_limits = create_custom_base_limits(robot, base_limits) base_joints = joints_from_names(robot, BASE_JOINTS) draw_base_limits(base_limits) # draw_pose(get_link_pose(robot, base_link), length=0.5) start_conf = get_joint_positions(robot, base_joints) for conf in [start_conf, goal_conf]: draw_waypoint(conf) #resolutions = None #resolutions = np.array([0.05, 0.05, math.radians(10)]) plan_joints = base_joints[:2] if not args.orientation else base_joints d = len(plan_joints) holonomic = args.holonomic or (d != 3) resolutions = 1. * DEFAULT_RESOLUTION * np.ones( d) # TODO: default resolutions, velocities, accelerations fns #weights = np.reciprocal(resolutions) weights = np.array([1, 1, 1e-3])[:d] cost_fn = get_acceleration_fn(robot, plan_joints, max_velocities=MAX_VELOCITIES[:d], max_accelerations=MAX_ACCELERATIONS[:d]) # TODO: check that taking shortest turning direction (reversible affecting?) if args.cfree: obstacles = [] # for obstacle in obstacles: # draw_aabb(get_aabb(obstacle)) # Updates automatically #set_all_static() # Doesn't seem to affect #test_aabb(robot) #test_caching(robot, obstacles) #return ######################### # TODO: filter if straight-line path is feasible saver = WorldSaver() wait_for_duration(duration=1e-2) start_time = time.time() with LockRenderer(lock=args.lock): with Profiler(field='cumtime', num=25): # tottime | cumtime | None # TODO: draw the search tree path = plan_base_joint_motion( robot, plan_joints, goal_conf[:d], holonomic=holonomic, reversible=args.reversible, obstacles=obstacles, self_collisions=False, custom_limits=custom_limits, use_aabb=True, cache=True, max_distance=MIN_PROXIMITY, resolutions=resolutions, weights=weights, # TODO: use KDTrees circular={ 2: UNBOUNDED_LIMITS if holonomic else CIRCULAR_LIMITS }, cost_fn=cost_fn, algorithm=args.algorithm, verbose=True, restarts=5, max_iterations=50, smooth=None if holonomic else 100, smooth_time=1, # None | 100 | INF ) saver.restore() # TODO: draw ROADMAPS #wait_for_duration(duration=1e-3) ######################### solved = path is not None length = INF if path is None else len(path) cost = compute_cost(robot, base_joints, path, resolutions=resolutions[:len(plan_joints)]) print( 'Solved: {} | Length: {} | Cost: {:.3f} | Runtime: {:.3f} sec'.format( solved, length, cost, elapsed_time(start_time))) if path is None: wait_if_gui() disconnect() return # for i, conf in enumerate(path): # set_joint_positions(robot, plan_joints, conf) # wait_if_gui('{}/{}) Continue?'.format(i + 1, len(path))) path = extract_full_path(robot, plan_joints, path, base_joints) with LockRenderer(): draw_last_roadmap(robot, base_joints) # for i, conf in enumerate(path): # set_joint_positions(robot, base_joints, conf) # wait_if_gui('{}/{}) Continue?'.format(i+1, len(path))) iterate_path( robot, base_joints, path, step_size=2e-2, smooth=holonomic, custom_limits=custom_limits, resolutions=DEFAULT_RESOLUTION * np.ones(3), # resolutions obstacles=obstacles, self_collisions=False, max_distance=MIN_PROXIMITY) disconnect()
def get_base_conf(self): return get_joint_positions(self.robot, self.base_joints)
def main(): # TODO: teleporting kuka arm parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() client = connect(use_gui=args.viewer) add_data_path() #planeId = p.loadURDF("plane.urdf") table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107) box = create_box(.07, .05, .15) # boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation) #pr2 = p.loadURDF("pr2_description/pr2.urdf") pr2 = p.loadURDF("pr2_description/pr2_fixed_torso.urdf") #pr2 = p.loadURDF("/Users/caelan/Programs/Installation/pr2_drake/pr2_local2.urdf",) #useFixedBase=0,) #flags=p.URDF_USE_SELF_COLLISION) #flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT) #flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) #pr2 = p.loadURDF("pr2_drake/urdf/pr2_simplified.urdf", useFixedBase=False) initially_colliding = get_colliding_links(pr2) print len(initially_colliding) origin = (0, 0, 0) print p.getNumConstraints() # TODO: no way of controlling the base position by itself # TODO: PR2 seems to collide with itself frequently # real_time = False # p.setRealTimeSimulation(real_time) # left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] # control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM) # while True: # control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM) # if not real_time: # p.stepSimulation() # A CollisionMap robot allows the user to specify self-collision regions indexed by the values of two joints. # GetRigidlyAttachedLinks print pr2 # for i in range (10000): # p.stepSimulation() # time.sleep(1./240.) #print get_joint_names(pr2) print[get_joint_name(pr2, joint) for joint in get_movable_joints(pr2)] print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME)) #open_gripper(pr2, joint_from_name(pr2, LEFT_GRIPPER)) #print get_joint_limits(pr2, joint_from_name(pr2, LEFT_GRIPPER)) #print get_joint_position(pr2, joint_from_name(pr2, LEFT_GRIPPER)) print self_collision(pr2) """ print p.getNumConstraints() constraint = fixed_constraint(pr2, -1, box, -1) # table p.changeConstraint(constraint) print p.getNumConstraints() print p.getConstraintInfo(constraint) print p.getConstraintState(constraint) p.stepSimulation() raw_input('Continue?') set_point(pr2, (-2, 0, 0)) p.stepSimulation() p.changeConstraint(constraint) print p.getConstraintInfo(constraint) print p.getConstraintState(constraint) raw_input('Continue?') print get_point(pr2) raw_input('Continue?') """ # TODO: would be good if we could set the joint directly print set_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME), 0.2) # Updates automatically print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME)) #return left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] right_joints = [joint_from_name(pr2, name) for name in RIGHT_JOINT_NAMES] print set_joint_positions( pr2, left_joints, TOP_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM print set_joint_positions( pr2, right_joints, REST_RIGHT_ARM) # TOP_HOLDING_RIGHT_ARM | REST_RIGHT_ARM print get_body_name(pr2) print get_body_names() # print p.getBodyUniqueId(pr2) print get_joint_names(pr2) #for joint, value in zip(LEFT_ARM_JOINTS, REST_LEFT_ARM): # set_joint_position(pr2, joint, value) # for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM): # joint = joint_from_name(pr2, name) # #print name, joint, get_joint_position(pr2, joint), value # print name, get_joint_limits(pr2, joint), get_joint_type(pr2, joint), get_link_name(pr2, joint) # set_joint_position(pr2, joint, value) # #print name, joint, get_joint_position(pr2, joint), value # for name, value in zip(RIGHT_JOINT_NAMES, REST_RIGHT_ARM): # set_joint_position(pr2, joint_from_name(pr2, name), value) print p.getNumJoints(pr2) jointId = 0 print p.getJointInfo(pr2, jointId) print p.getJointState(pr2, jointId) # for i in xrange(10): # #lower, upper = BASE_LIMITS # #q = np.random.rand(len(lower))*(np.array(upper) - np.array(lower)) + lower # q = np.random.uniform(*BASE_LIMITS) # theta = np.random.uniform(*REVOLUTE_LIMITS) # quat = z_rotation(theta) # print q, theta, quat, env_collision(pr2) # #set_point(pr2, q) # set_pose(pr2, q, quat) # #p.getMouseEvents() # #p.getKeyboardEvents() # raw_input('Continue?') # Stalls because waiting for input # # # TODO: self collisions # for i in xrange(10): # for name in LEFT_JOINT_NAMES: # joint = joint_from_name(pr2, name) # value = np.random.uniform(*get_joint_limits(pr2, joint)) # set_joint_position(pr2, joint, value) # raw_input('Continue?') #start = (-2, -2, 0) #set_base_values(pr2, start) # #start = get_base_values(pr2) # goal = (2, 2, 0) # p.addUserDebugLine(start, goal, lineColorRGB=(1, 1, 0)) # addUserDebugText # print start, goal # raw_input('Plan?') # path = plan_base_motion(pr2, goal) # print path # if path is None: # return # print len(path) # for bq in path: # set_base_values(pr2, bq) # raw_input('Continue?') # left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] # for joint in left_joints: # print joint, get_joint_name(pr2, joint), get_joint_limits(pr2, joint), \ # is_circular(pr2, joint), get_joint_position(pr2, joint) # # #goal = np.zeros(len(left_joints)) # goal = [] # for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM): # joint = joint_from_name(pr2, name) # goal.append(wrap_joint(pr2, joint, value)) # # path = plan_joint_motion(pr2, left_joints, goal) # print path # for q in path:s # set_joint_positions(pr2, left_joints, q) # raw_input('Continue?') print p.JOINT_REVOLUTE, p.JOINT_PRISMATIC, p.JOINT_FIXED, p.JOINT_POINT2POINT, p.JOINT_GEAR # 0 1 4 5 6 movable_joints = get_movable_joints(pr2) print len(movable_joints) for joint in xrange(get_num_joints(pr2)): if is_movable(pr2, joint): print joint, get_joint_name(pr2, joint), get_joint_type( pr2, joint), get_joint_limits(pr2, joint) #joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] #set_joint_positions(pr2, joints, sample_joints(pr2, joints)) #print get_joint_positions(pr2, joints) # Need to print before the display updates? # set_base_values(pr2, (1, -1, -np.pi/4)) # movable_joints = get_movable_joints(pr2) # gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK)) # print gripper_pose # print get_joint_positions(pr2, movable_joints) # p.addUserDebugLine(origin, gripper_pose[0], lineColorRGB=(1, 0, 0)) # p.stepSimulation() # raw_input('Pre2 IK') # set_joint_positions(pr2, left_joints, SIDE_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM # print get_joint_positions(pr2, movable_joints) # p.stepSimulation() # raw_input('Pre IK') # conf = inverse_kinematics(pr2, gripper_pose) # Doesn't automatically set configuraitons # print conf # print get_joint_positions(pr2, movable_joints) # set_joint_positions(pr2, movable_joints, conf) # print get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK)) # #print get_joint_positions(pr2, movable_joints) # p.stepSimulation() # raw_input('Post IK') # return # print pose_from_tform(TOOL_TFORM) # gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK)) # #gripper_pose = multiply(gripper_pose, TOOL_POSE) # #gripper_pose = get_gripper_pose(pr2) # for i, grasp_pose in enumerate(get_top_grasps(box)): # grasp_pose = multiply(TOOL_POSE, grasp_pose) # box_pose = multiply(gripper_pose, grasp_pose) # set_pose(box, *box_pose) # print get_pose(box) # raw_input('Grasp {}'.format(i)) # return torso = joint_from_name(pr2, TORSO_JOINT_NAME) torso_point, torso_quat = get_link_pose(pr2, torso) #torso_constraint = p.createConstraint(pr2, torso, -1, -1, # p.JOINT_FIXED, jointAxis=[0] * 3, # JOINT_FIXED # parentFramePosition=torso_point, # childFramePosition=torso_quat) create_inverse_reachability(pr2, box, table) ir_database = load_inverse_reachability() print len(ir_database) return link = link_from_name(pr2, LEFT_ARM_LINK) point, quat = get_link_pose(pr2, link) print point, quat p.addUserDebugLine(origin, point, lineColorRGB=(1, 1, 0)) # addUserDebugText raw_input('Continue?') current_conf = get_joint_positions(pr2, movable_joints) #ik_conf = p.calculateInverseKinematics(pr2, link, point) #ik_conf = p.calculateInverseKinematics(pr2, link, point, quat) min_limits = [get_joint_limits(pr2, joint)[0] for joint in movable_joints] max_limits = [get_joint_limits(pr2, joint)[1] for joint in movable_joints] max_velocities = [ get_max_velocity(pr2, joint) for joint in movable_joints ] # Range of Jacobian print min_limits print max_limits print max_velocities ik_conf = p.calculateInverseKinematics(pr2, link, point, quat, lowerLimits=min_limits, upperLimits=max_limits, jointRanges=max_velocities, restPoses=current_conf) value_from_joint = dict(zip(movable_joints, ik_conf)) print[value_from_joint[joint] for joint in joints] #print len(ik_conf), ik_conf set_joint_positions(pr2, movable_joints, ik_conf) #print len(movable_joints), get_joint_positions(pr2, movable_joints) print get_joint_positions(pr2, joints) raw_input('Finish?') p.disconnect()
def get_joint_positions(self, joint_names): # Returns the configuration of the specified joints with ClientSaver(self.client): joints = joints_from_names(self.robot, joint_names) return get_joint_positions(self.robot, joints)
def follow_trajectory(self, joints, path, times_from_start=None, real_time_step=0.0, waypoint_tolerance=1e-2 * np.pi, goal_tolerance=5e-3 * np.pi, max_sim_duration=1.0, print_sim_frequency=1.0, **kwargs): # real_time_step = 1e-1 # Can optionally sleep to slow down visualization start_time = time.time() if times_from_start is not None: assert len(path) == len(times_from_start) current_positions = get_joint_positions(self.robot, joints) differences = [(np.array(q2) - np.array(q1)) / (t2 - t1) for q1, t1, q2, t2 in zip([current_positions] + path, [0.] + times_from_start, path, times_from_start)] velocity_path = differences[1:] + [np.zeros(len(joints)) ] # Using velocity at endpoints else: velocity_path = [None] * len(path) sim_duration = 0.0 sim_steps = 0 last_print_sim_duration = sim_duration with ClientSaver(self.client): dt = get_time_step() # TODO: fit using splines to get velocity info for num, positions in enumerate(path): if self.execute_motor_control: sim_start = sim_duration tolerance = goal_tolerance if (num == len(path) - 1) else waypoint_tolerance velocities = velocity_path[num] for _ in joint_controller_hold2(self.robot, joints, positions, velocities, tolerance=tolerance, **kwargs): step_simulation() # print(get_joint_velocities(self.robot, joints)) # print([get_joint_torque(self.robot, joint) for joint in joints]) sim_duration += dt sim_steps += 1 time.sleep(real_time_step) if print_sim_frequency <= (sim_duration - last_print_sim_duration): print( 'Waypoint: {} | Simulation steps: {} | Simulation seconds: {:.3f} | Steps/sec {:.3f}' .format(num, sim_steps, sim_duration, sim_steps / elapsed_time(start_time))) last_print_sim_duration = sim_duration if max_sim_duration <= (sim_duration - sim_start): print( 'Timeout of {:.3f} simulation seconds exceeded!' .format(max_sim_duration)) break # control_joints(self.robot, arm_joints, positions) else: set_joint_positions(self.robot, joints, positions) print( 'Followed {} waypoints in {:.3f} simulation seconds and {} simulation steps' .format(len(path), sim_duration, sim_steps))
def main(): parser = argparse.ArgumentParser() parser.add_argument('-c', '--cfree', action='store_true', help='When enabled, disables collision checking.') # parser.add_argument('-p', '--problem', default='test_pour', choices=sorted(problem_fn_from_name), # help='The name of the problem to solve.') parser.add_argument('--holonomic', action='store_true', # '-h', help='') parser.add_argument('-l', '--lock', action='store_false', help='') parser.add_argument('-s', '--seed', default=None, type=int, help='The random seed to use.') parser.add_argument('-v', '--viewer', action='store_false', help='') args = parser.parse_args() connect(use_gui=args.viewer) seed = args.seed #seed = 0 #seed = time.time() set_random_seed(seed=seed) # None: 2147483648 = 2**31 set_numpy_seed(seed=seed) print('Random seed:', get_random_seed(), random.random()) print('Numpy seed:', get_numpy_seed(), np.random.random()) ######################### robot, base_limits, goal_conf, obstacles = problem1() draw_base_limits(base_limits) custom_limits = create_custom_base_limits(robot, base_limits) base_joints = joints_from_names(robot, BASE_JOINTS) base_link = link_from_name(robot, BASE_LINK_NAME) if args.cfree: obstacles = [] # for obstacle in obstacles: # draw_aabb(get_aabb(obstacle)) # Updates automatically resolutions = None #resolutions = np.array([0.05, 0.05, math.radians(10)]) set_all_static() # Doesn't seem to affect region_aabb = AABB(lower=-np.ones(3), upper=+np.ones(3)) draw_aabb(region_aabb) step_simulation() # Need to call before get_bodies_in_region #update_scene() # TODO: https://github.com/bulletphysics/bullet3/pull/3331 bodies = get_bodies_in_region(region_aabb) print(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 #draw_pose(get_link_pose(robot, base_link), length=0.5) for conf in [get_joint_positions(robot, base_joints), goal_conf]: draw_pose(pose_from_pose2d(conf, z=DRAW_Z), length=DRAW_LENGTH) aabb = get_aabb(robot) #aabb = get_subtree_aabb(robot, base_link) draw_aabb(aabb) for link in [BASE_LINK, base_link]: print(link, get_collision_data(robot, link), pairwise_link_collision(robot, link, robot, link)) ######################### saver = WorldSaver() start_time = time.time() profiler = Profiler(field='tottime', num=50) # tottime | cumtime | None profiler.save() with LockRenderer(lock=args.lock): path = plan_motion(robot, base_joints, goal_conf, holonomic=args.holonomic, obstacles=obstacles, custom_limits=custom_limits, resolutions=resolutions, use_aabb=True, cache=True, max_distance=0., restarts=2, iterations=20, smooth=20) # 20 | None saver.restore() #wait_for_duration(duration=1e-3) profiler.restore() ######################### solved = path is not None length = INF if path is None else len(path) cost = sum(get_distance_fn(robot, base_joints, weights=resolutions)(*pair) for pair in get_pairs(path)) print('Solved: {} | Length: {} | Cost: {:.3f} | Runtime: {:.3f} sec'.format( solved, length, cost, elapsed_time(start_time))) if path is None: disconnect() return iterate_path(robot, base_joints, path) disconnect()
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()