def autonomousInit(self): # Set up the trajectory points = [pf.Waypoint(0, 0, 0), pf.Waypoint(9, 5, 0)] info, trajectory = pf.generate( points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, dt=self.getPeriod(), max_velocity=self.MAX_VELOCITY, max_acceleration=self.MAX_ACCELERATION, max_jerk=120.0, ) # Wheelbase Width = 2 ft modifier = pf.modifiers.TankModifier(trajectory).modify(2.0) # Do something with the new Trajectories... left = modifier.getLeftTrajectory() right = modifier.getRightTrajectory() leftFollower = pf.followers.EncoderFollower(left) leftFollower.configureEncoder(self.l_encoder.get(), self.ENCODER_COUNTS_PER_REV, self.WHEEL_DIAMETER) leftFollower.configurePIDVA(1.0, 0.0, 0.0, 1 / self.MAX_VELOCITY, 0) rightFollower = pf.followers.EncoderFollower(right) rightFollower.configureEncoder(self.r_encoder.get(), self.ENCODER_COUNTS_PER_REV, self.WHEEL_DIAMETER) rightFollower.configurePIDVA(1.0, 0.0, 0.0, 1 / self.MAX_VELOCITY, 0) self.leftFollower = leftFollower self.rightFollower = rightFollower # This code renders the followed path on the field in simulation (requires pyfrc 2018.2.0+) if wpilib.RobotBase.isSimulation(): from pyfrc.sim import get_user_renderer renderer = get_user_renderer() if renderer: renderer.draw_pathfinder_trajectory(left, color="#0000ff", offset=(-1, 0)) renderer.draw_pathfinder_trajectory(modifier.source, color="#00ff00", show_dt=1.0, dt_offset=0.0) renderer.draw_pathfinder_trajectory(right, color="#0000ff", offset=(1, 0))
def generate(path, dt=0.02, max_velocity=1.7, max_acceleration=2.0, max_jerk=60.0): return pathfinder.generate(path, pathfinder.FIT_HERMITE_CUBIC, pathfinder.SAMPLES_HIGH, dt=dt, max_velocity=max_velocity, max_acceleration=max_acceleration, max_jerk=max_jerk)[1]
def get_trajectory(points, period=.02, max_velocity=5, max_acceleration=6, max_jerk=120, wheelbase_width=2): # Set up the trajectory info, trajectory = pf.generate(points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, dt=period, max_velocity=max_velocity, max_acceleration=max_acceleration, max_jerk=max_jerk) modifier = pf.modifiers.TankModifier(trajectory).modify(wheelbase_width) left = modifier.getLeftTrajectory() right = modifier.getRightTrajectory() return left, modifier.source, right
def autonomousInit(self): # Set up the trajectory points = [pf.Waypoint(0, 0, 0), pf.Waypoint(9, 5, 0)] info, trajectory = pf.generate( points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, dt=self.getPeriod(), max_velocity=self.MAX_VELOCITY, max_acceleration=self.MAX_ACCELERATION, max_jerk=120.0, ) # Wheelbase Width = 2 ft modifier = pf.modifiers.TankModifier(trajectory).modify(2.0) # Do something with the new Trajectories... left = modifier.getLeftTrajectory() right = modifier.getRightTrajectory() leftFollower = pf.followers.EncoderFollower(left) leftFollower.configureEncoder( self.l_encoder.get(), self.ENCODER_COUNTS_PER_REV, self.WHEEL_DIAMETER ) leftFollower.configurePIDVA(.8, 0.0, 0.15, 1 / self.MAX_VELOCITY, 0) rightFollower = pf.followers.EncoderFollower(right) rightFollower.configureEncoder( self.r_encoder.get(), self.ENCODER_COUNTS_PER_REV, self.WHEEL_DIAMETER ) rightFollower.configurePIDVA(.8, 0.0, 0.15, 1 / self.MAX_VELOCITY, 0) self.leftFollower = leftFollower self.rightFollower = rightFollower # This code renders the followed path on the field in simulation (requires pyfrc 2018.2.0+) if wpilib.RobotBase.isSimulation(): from pyfrc.sim import get_user_renderer renderer = get_user_renderer() if renderer: renderer.draw_pathfinder_trajectory( left, color="#0000ff", offset=(-1, 0) ) renderer.draw_pathfinder_trajectory( modifier.source, color="#00ff00", show_dt=1.0, dt_offset=0.0 ) renderer.draw_pathfinder_trajectory( right, color="#0000ff", offset=(1, 0) )
def generate_path(): #XXX need to pick good waypoints # waypoints initialize at 0, 0 waypoints = [ pf.Waypoint(0, 0, 0), pf.Waypoint(1, 1, 1), pf.Waypoint(2, 2, 2) ] # This returns a tuple of TrajectoryInfo and a trajectory constituting # a list of points to follow over time) info, trajectory = pf.generate( waypoints, pf.FIT_HERMITE_CUBIC, #XXX not sure what that does pf.SAMPLES_HIGH, #XXX dt means change in time in seconds? # 50ms dt=0.05, #XXX max velocity is probably in the units that your encoder and gyro # calculate it in max_velocity=1, max_acceleration=2, max_jerk=3) # pass in wheelbase width in meters #XXX wrong width modifier = pf.modifiers.TankModifier(trajectory).modify(1) mx, my = zip(*[(m.y, m.x) for m in points]) plt.scatter(mx, my, c="r") # plot the trajectory x, y = zip(*[(seg.y, seg.x) for seg in trajectory]) plt.plot(x, y) # annotate with time for i in range(0, len(trajectory), int(0.5 / dt)): plt.annotate( "t=%.2f" % (i * dt, ), xy=(x[i], y[i]), xytext=(-20, 20), textcoords="offset points", arrowprops=dict(arrowstyle="->", connectionstyle="arc3,rad=0"), ) plt.show() return modifier
def __init__(self): if wpilib.RobotBase.isSimulation(): # Generate trajectories during testing paths = { 'forward': [ pf.Waypoint(-11, 0, 0), pf.Waypoint(0, 0, 0) ], 'left': [ pf.Waypoint(0, 0, 0), pf.Waypoint(6, -6, 0), pf.Waypoint(11, -6, 0), pf.Waypoint(8, 0, math.radians(-90)), #pf.Waypoint(18, 0, 0) #pf.Waypoint(14, 14, 0) #pf.Waypoint(10, 15, 0), #pf.Waypoint(14, 14, math.radians(-90)) #pf.Waypoint(-8, -5, 0), #pf.Waypoint(-4, -1, math.radians(-45.0)), # Waypoint @ x=-4, y=-1, exit angle=-45 degrees #pf.Waypoint(-2, -2, 0), # Waypoint @ x=-2, y=-2, exit angle=0 radians ] } trajectories = {} for path in paths: info, trajectory = pf.generate(paths[path], pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, dt = 0.05, max_velocity = 1.7, max_acceleration = 2, max_jerk = 100 ) trajectories[path] = trajectory self.trajectories = trajectories # and then write it out with open(pickle_file, 'wb') as fp: pickle.dump(self.trajectories, fp) else: with open(pickle_file, 'rb') as fp: self.trajectories = pickle.load(fp)
def create_profiles(self, dt=0.1): if self.points and len(self.points) > 1: try: _, self.middle_profile = pf.generate(self.points, pf.FIT_HERMITE_QUINTIC, pf.SAMPLES_HIGH, dt=dt, max_velocity=3, max_acceleration=5, max_jerk=25) self.modifier = pf.modifiers.TankModifier( self.middle_profile).modify(0.6) self.left_profile = self.modifier.getRightTrajectory() self.right_profile = self.modifier.getLeftTrajectory() if self.drag_mode is None: self.chart.setProfiles(self.middle_profile, self.left_profile, self.right_profile) except: print("Couldn't create path")
def __init__(self, robot): super().__init__() points = [pf.Waypoint(0, 0, 0), pf.Waypoint(13, 0, 0)] info, trajectory = pf.generate( points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, max_velocity=PassAutoLine.MAXVELOCITY, max_acceleration=PassAutoLine.MAXACCELERATION, max_jerk=120.0, ) # Wheelbase Width = 2 ft modifier = pf.modifiers.TankModifier(trajectory).modify( robotmap.robotDiameter / 12) # Do something with the new Trajectories... left = modifier.getLeftTrajectory() right = modifier.getRightTrajectory() leftFollower = pf.followers.EncoderFollower(left) leftFollower.configureEncoder(robot.drivetrain.encoderLeft, robotmap.encoderPerRev, robotmap.wheelDiameter) leftFollower.configurePIDVA(1.0, 0.0, 0.0, 1 / PassAutoLine.MAXVELOCITY, 0) rightFollower = pf.followers.EncoderFollower(right) rightFollower.configureEncoder(robot.drivetrain.encoderRight, robotmap.encoderPerRev, robotmap.wheelDiameter) rightFollower.configurePIDVA(1.0, 0.0, 0.0, 1 / PassAutoLine.MAXVELOCITY, 0) self.leftFollower = leftFollower self.rightFollower = rightFollower self.addSequential(driveForwards.DriveForward(robot, 160))
def _generate_trajectories(): """ Generate trajectory from waypoints. """ generated_trajectories = {} for trajectory_name, trajectory in trajectories.items(): generated_trajectory = pf.generate( trajectory, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, dt=0.02, # 20ms max_velocity=MAX_GENERATION_VELOCITY, # These are in ft/sec and max_acceleration= MAX_GENERATION_ACCELERATION, # set the units for distance to ft. max_jerk=MAX_GENERATION_JERK)[1] # The 0th element is just info modifier = pf.modifiers.TankModifier(generated_trajectory).modify( WHEELBASE_WIDTH) generated_trajectories[trajectory_name] = ( modifier.getLeftTrajectory(), modifier.getRightTrajectory()) if wpilib.RobotBase.isSimulation(): from pyfrc.sim import get_user_renderer renderer = get_user_renderer() if renderer: renderer.draw_pathfinder_trajectory(modifier.getLeftTrajectory(), '#0000ff', offset=(-0.9, 0)) renderer.draw_pathfinder_trajectory(modifier.source, '#00ff00', show_dt=True) renderer.draw_pathfinder_trajectory(modifier.getRightTrajectory(), '#0000ff', offset=(0.9, 0)) return generated_trajectories
def move_to(self, x_position, y_position, angle=0, first=False): """ Generate path and set path variable :param x_position: The x distance. :param y_position: The y distance. :param angle: The angle difference in degrees. :param first: Whether or not this path should be completed next. """ waypoint = pf.Waypoint(float(x_position), float(y_position), radians(angle)) info, trajectory = pf.generate([pf.Waypoint(0, 0, 0), waypoint], pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, 0.05, 1.7, 2.0, 60.0) modifier = pf.modifiers.TankModifier(trajectory).modify(0.5) right_trajectory = modifier.getRightTrajectory() left_trajectory = modifier.getLeftTrajectory() with self.cond: if self.left_follower or self.right_follower is None: self.right_follower = pf.followers.EncoderFollower( right_trajectory) self.left_follower = pf.followers.EncoderFollower( left_trajectory) if first: self.trajectories.appendLeft({ 'right': right_trajectory, 'left': left_trajectory }) else: self.trajectories.append({ 'right': right_trajectory, 'left': left_trajectory }) self.cond.notify()
def _gen_trajectory(self, path: List[Pose], cruise: float, acc: float, jerk: float): fname = f"{self._get_path_hash(path, cruise, acc, jerk)}.pickle" pickle_file = os.path.join(get_basedir(), 'paths', fname) if wpilib.RobotBase.isSimulation(): # generate the trajectory here info, trajectory = pf.generate( list(convert_waypoints(path)), pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, dt=0.05, # 50ms max_velocity=cruise, max_acceleration=acc, max_jerk=jerk) # and then write it out with open(pickle_file, 'wb') as fp: pickle.dump(trajectory, fp) else: with open('fname', 'rb') as fp: trajectory = pickle.load(fp) return trajectory
def start_compute_server(self): ''' This is run on the desktop to manage generating the paths for the bot ''' print("STARTING REMOTE PATH COMPUTE SERVER") while True: if not self.is_finished_generating(): print("GENERATING PATH") start = time.time() _, trajectory = pf.generate( self.get_path_to_generate(), pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, dt=0.05, max_velocity=self.MAX_V, max_acceleration=self.MAX_A, max_jerk=60.0 ) trajectory = pf.modifiers.TankModifier(trajectory) self.write_to_trajectory(trajectory) print("DONE IN {} SECONDS".format(time.time() - start))
#!/usr/bin/env python3 import pathfinder as pf import math if __name__ == "__main__": points = [ pf.Waypoint(-4, -1, math.radians(-45.0)), pf.Waypoint(-2, -2, 0), pf.Waypoint(0, 0, 0), ] info, trajectory = pf.generate( points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, dt=0.05, # 50ms max_velocity=1.7, max_acceleration=2.0, max_jerk=60.0, ) # Do something with the new Trajectory...
class AutoPath(): WHEEL_DIAMETER = 0.5 # 6 inches ENCODER_COUNTS_PER_REV = 256 MAX_VELOCITY = 6 #ft/s MAX_ACCELERATION = 5 def __init__(self, sensors, drivetrain): self.sensors = sensors self.drivetrain = drivetrain def path_init(self): # Set up the trajectory points = [ pf.Waypoint(0, 0, 0), pf.Waypoint(6, -6, 0), pf.Waypoint(11, -6, 0), pf.Waypoint(8, 0, math.radians(-90)), #pf.Waypoint(23.5, 8, 0), ] info, trajectory = pf.generate(points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, dt=self.getPeriod(), max_velocity=self.MAX_VELOCITY, max_acceleration=self.MAX_ACCELERATION, max_jerk=120.0) # because of a quirk in pyfrc, this must be in a subdirectory # or the file won't get copied over to the robot pickle_file = os.path.join(os.path.dirname(__file__), 'trajectory.pickle') if wpilib.RobotBase.isSimulation(): # generate the trajectory here # and then write it out with open(pickle_file, 'wb') as fp: pickle.dump(trajectory, fp) else: with open('fname', 'rb') as fp: trajectory = pickle.load(fp) # Wheelbase Width = 2 ft modifier = pf.modifiers.TankModifier(trajectory).modify(2.0) # Do something with the new Trajectories... left = modifier.getLeftTrajectory() right = modifier.getRightTrajectory() leftFollower = pf.followers.EncoderFollower(left) leftFollower.configureEncoder(self.sensors.getLeftDistance(), self.ENCODER_COUNTS_PER_REV, self.WHEEL_DIAMETER) leftFollower.configurePIDVA(1.0, 0.0, 0.0, 1 / self.MAX_VELOCITY, 0) rightFollower = pf.followers.EncoderFollower(right) rightFollower.configureEncoder(self.sensors.getRightDistance(), self.ENCODER_COUNTS_PER_REV, self.WHEEL_DIAMETER) rightFollower.configurePIDVA(1.0, 0.0, 0.0, 1 / self.MAX_VELOCITY, 0) self.leftFollower = leftFollower self.rightFollower = rightFollower def run(self): l = self.leftFollower.calculate(sself.sensors.getLeftDistance()) r = self.rightFollower.calculate(self.sensors.getRightDistance()) # REPLACE THIS WITH NAVX gyro_heading = -self.sensors.getCurrentAngle( ) # Assuming the gyro is giving a value in degrees desired_heading = pf.r2d( self.leftFollower.getHeading()) # Should also be in degrees # This is a poor man's P controller angleDifference = pf.boundHalfDegrees(desired_heading - gyro_heading) turn = 5 * (-1.0 / 80.0) * angleDifference l = l + turn r = r - turn # -1 is forward, so invert both values self.drivetrain().moveRobot(-l, -r)
def on_enable(self): super().on_enable() points = [pf.Waypoint(0, 0, 0), pf.Waypoint(13, -3, 0)] info, trajectory = pf.generate( points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, dt=0.02, max_velocity=self.MAX_VELOCITY, max_acceleration=self.MAX_ACCELERATION, max_jerk=120.0, ) # Wheelbase Width = 2 ft modifier = pf.modifiers.TankModifier(trajectory).modify( self.DRIVE_WIDTH) # Do something with the new Trajectories... left = modifier.getLeftTrajectory() right = modifier.getRightTrajectory() leftFollower = pf.followers.DistanceFollower(left) leftFollower.configurePIDVA( 0.001, 0.0, 0.0, 2.0 / (3 * self.MAX_VELOCITY), 1.0 / (12 * self.MAX_ACCELERATION), ) rightFollower = pf.followers.DistanceFollower(right) rightFollower.configurePIDVA( 0.001, 0.0, 0.0, 2.0 / (3 * self.MAX_VELOCITY), 1.0 / (12 * self.MAX_ACCELERATION), ) self.leftFollower = leftFollower self.rightFollower = rightFollower # This code renders the followed path on the field in simulation # (requires pyfrc 2018.2.0+) if wpilib.RobotBase.isSimulation(): from pyfrc.sim import get_user_renderer renderer = get_user_renderer() if renderer: renderer.draw_pathfinder_trajectory(left, color="#0000ff", offset=(-1, 0)) renderer.draw_pathfinder_trajectory( modifier.source, color="#00ff00", show_dt=1.0, dt_offset=0.0, ) renderer.draw_pathfinder_trajectory(right, color="#0000ff", offset=(1, 0))
# drivetrain.set_position(100, 0) # drivetrain.follow_path(bezier, 1, 0.01, 0) # drivetrain.drive_pure_pursuit_2(opposite_scale_auto, 10, 5, 0.1, True) # drivetrain.drive_forward_PID(100, 0.1, 0) # print(1) # drivetrain.turn_to_angle_PID(180, 0.1, 0) # drivetrain.drive_pure_pursuit_4(bezier, 10, 5, 0, True) # drivetrain.graph() # print(1) # print(bezier_2.get_closest_point(0, 0)) right_switch_auto = pf.generate( [pf.Waypoint(0, 0, math.radians(0)), pf.Waypoint(5, 5, math.radians(0))], pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, dt=0.02, # 50WWms max_velocity=5, max_acceleration=2.0, max_jerk=60.0)[1] backwards_auto = pf.generate( [pf.Waypoint(0, 0, math.radians(0)), pf.Waypoint(-5, 5, math.radians(0))], pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, dt=0.02, # 50WWms max_velocity=5, max_acceleration=2.0, max_jerk=60.0)[1]
#!/usr/bin/env python3 import pathfinder as pf import math if __name__ == '__main__': points = [ pf.Waypoint(-4, -1, math.radians(-45.0)), pf.Waypoint(-2, -2, 0), pf.Waypoint(0, 0, 0), ] info, trajectory = pf.generate(points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, 0.05, 1.7, 2.0, 60.0) # Do something with the new Trajectory...
def path(points, max_vel=1.7, max_accel=2.0, max_jerk=60.0, dt=0.05): info, trajectory = pf.generate(points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, dt, max_vel, max_accel, max_jerk) return info, trajectory
MAX_ACCELERATION = 4 #ft/s/s PERIOD = 0.02 MAX_JERK = 120.0 # Set up the trajectory points = [ pf.Waypoint(0, 0, 0), pf.Waypoint(6, 3, math.pi / 2), pf.Waypoint(0, 6, 0) ] info, trajectory = pf.generate( points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, dt=PERIOD, max_velocity=MAX_VELOCITY, max_acceleration=MAX_ACCELERATION, max_jerk=MAX_JERK, ) ''' with open('/home/ubuntu/out.csv', 'w+') as points: points_writer = csv.writer(points, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL) points_writer.writerow(['dt', 'x', 'y', 'position', 'velocity', 'acceleration', 'jerk', 'heading']) for i in trajectory: points_writer.writerow([i.dt, i.x, i.y, i.position, i.velocity, i.acceleration, i.jerk, i.heading]) ''' with open("/home/ubuntu/traj", "wb") as fp: pickle.dump(trajectory, fp)
def GeneratePath(path_name, file_name, waypoints, settings, reverse=False, heading_overide=False, headingValue=0.0): """ This function will take a set of pathfinder waypoints and create the trajectories to follow a path going through the waypoints. This path is specific for the drivetrain controllers, so the position will use the CTRE quadrature encoders and the velocity will use the feed-forward in units of Volts. """ # Generate the path using pathfinder. info, trajectory = pf.generate(waypoints, settings.order, settings.samples, settings.period, settings.maxVelocity, settings.maxAcceleration, settings.maxJerk) # Modify the path for the differential drive based on the calibrated wheelbase modifier = pf.modifiers.TankModifier(trajectory).modify(ROBOT_WHEELBASE_FT) # Ge the left and right trajectories...left and right are reversed rightTrajectory = modifier.getLeftTrajectory() leftTrajectory = modifier.getRightTrajectory() # Grab the position, velocity + acceleration for feed-forward, heading, and duration path = {"left": [], "right": []} headingOut = [] for i in range(len(leftTrajectory)): if not reverse: if heading_overide: heading = headingValue elif abs(pf.r2d(leftTrajectory[i].heading)) > 180: heading = -(pf.r2d(leftTrajectory[i].heading) - 360) else: heading = -pf.r2d(leftTrajectory[i].heading) else: if heading_overide: heading = headingValue else: heading = pf.r2d(leftTrajectory[i].heading) - 180 headingOut.append(heading) path["left"].append([leftTrajectory[i].position * 4096 / # Position: CTRE SRX Mag encoder: 4096 units per rotation (ROBOT_WHEEL_DIAMETER_FT * math.pi), # Voltage / Feed-Forward CalculateFeedForwardVoltage(True, leftTrajectory[i].velocity, leftTrajectory[i].acceleration), heading / 360, # Pigeon IMU setup for 3600 units per rotation int(leftTrajectory[i].dt * 1000)]) # Duration path["right"].append([rightTrajectory[i].position * 4096 / (ROBOT_WHEEL_DIAMETER_FT * math.pi), CalculateFeedForwardVoltage(False, rightTrajectory[i].velocity, rightTrajectory[i].acceleration), heading / 360, int(rightTrajectory[i].dt * 1000)]) # Dump the path into a pickle file which will be read up later by the RoboRIO robot code with open(os.path.join(path_name, file_name+".pickle"), "wb") as fp: pickle.dump(path, fp) # Plot the data for review x = list(i * (settings.period) for i, _ in enumerate(leftTrajectory)) plt.figure() # plt.plot(aspect=0.5) plt.title("Trajectory") drawField(plt) plt.plot([segment.y for segment in leftTrajectory], [segment.x for segment in leftTrajectory], marker='.', color='b') plt.plot([segment.y for segment in rightTrajectory], [segment.x for segment in rightTrajectory], marker='.', color='r') plt.gca().set_yticks(np.arange(0, 30.1, 1.0), minor=True) plt.gca().set_yticks(np.arange(0, 30.1, 3)) plt.gca().set_xticks(np.arange(0, 27.1, 1.0), minor=True) plt.gca().set_xticks(np.arange(0, 27.1, 3)) plt.grid(which='minor', color='grey', linestyle='--', alpha=0.25) plt.grid(which='major', color='grey', linestyle='-', alpha=0.75) # Plot the heading plt.figure() plt.title("Heading") plt.plot(x, headingOut, marker='.') # Plot the velocity and acceleration and look for any discontinuities plt.figure() plt.subplot(2, 1, 1) plt.title("Velocity") plt.plot(x, [segment.velocity for segment in leftTrajectory], marker='.', color='b') plt.plot(x, [segment.velocity for segment in rightTrajectory], marker='.', color='r') plt.yticks(np.arange(0, DRIVETRAIN_MAX_VELOCITY + 0.1, 1.0)) plt.grid() plt.subplot(2, 1, 2) plt.title("Acceleration") plt.plot(x, [segment.acceleration for segment in leftTrajectory], marker='.', color='b') plt.plot(x, [segment.acceleration for segment in rightTrajectory], marker='.', color='r') plt.yticks(np.arange(-DRIVETRAIN_MAX_ACCELERATION, DRIVETRAIN_MAX_ACCELERATION + 1.1, 2.0)) plt.grid() plt.tight_layout() plt.show()
right_leg1 = (align_pt_right - start_pos_middle) * 0.0254 right_leg2 = (right_switch - start_pos_middle) * 0.0254 ldiv_leg1 = (np.array((36, 48.5)) - start_pos_left) * 0.0254 ldiv_leg2 = (staging_left - start_pos_left) * 0.0254 rdiv_leg1 = (np.array((36, 279.5)) - start_pos_right) * 0.0254 rdiv_leg2 = (staging_right - start_pos_right) * 0.0254 straight_fwd1 = np.array((36, 0)) * 0.0254 straight_fwd2 = np.array((132, 0)) * 0.0254 _, trajectories['left'] = pf.generate([ pf.Waypoint(left_leg1[0], left_leg1[1], 0), pf.Waypoint(left_leg2[0], left_leg2[1], 0), pf.Waypoint(left_leg3[0], left_leg3[1], 0), ], pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, _trajectory_dt, _max_speed, 2.0, 60.0) _, trajectories['divert-left'] = pf.generate([ pf.Waypoint(ldiv_leg1[0], ldiv_leg1[1], 0), pf.Waypoint(ldiv_leg2[0], ldiv_leg2[1], 0), ], pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, _trajectory_dt, _max_speed, 2.0, 60.0) _, trajectories['right'] = pf.generate([ pf.Waypoint(right_leg1[0], right_leg1[1], 0), pf.Waypoint(right_leg2[0], right_leg2[1], 0), ], pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, _trajectory_dt, _max_speed, 2.0, 60.0)
def autonomousInit(self): self.robot_drive.setSafetyEnabled(False) self.navx.reset() self.l_motor_master.setSelectedSensorPosition(0, 0, 10) self.r_motor_master.setSelectedSensorPosition(0, 0, 10) self.l_motor_master.getSensorCollection().setQuadraturePosition(0, 10) self.r_motor_master.getSensorCollection().setQuadraturePosition(0, 10) if self.isSimulation(): # Set up the trajectory points = [pf.Waypoint(0, 0, 0), pf.Waypoint(9, 6, 0)] info, trajectory = pf.generate( points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, dt=self.getPeriod(), max_velocity=self.MAX_VELOCITY, max_acceleration=self.MAX_ACCELERATION, max_jerk=120.0, ) with open("/home/ubuntu/traj", "wb") as fp: pickle.dump(trajectory, fp) else: with open("/home/lvuser/traj", "rb") as fp: trajectory = pickle.load(fp) print("len of read trajectory: " + str(len(trajectory))) print("autonomousInit: " + str(len(trajectory))) if self.isSimulation(): with open('/home/ubuntu/out.csv', 'w+') as points: points_writer = csv.writer(points, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL) points_writer.writerow([ 'dt', 'x', 'y', 'position', 'velocity', 'acceleration', 'jerk', 'heading' ]) for i in trajectory: points_writer.writerow([ i.dt, i.x, i.y, i.position, i.velocity, i.acceleration, i.jerk, i.heading ]) # Wheelbase Width = 2 ft modifier = pf.modifiers.TankModifier(trajectory).modify(2.1) # Do something with the new Trajectories... left = modifier.getLeftTrajectory() right = modifier.getRightTrajectory() leftFollower = pf.followers.EncoderFollower(left) leftFollower.configureEncoder( self.l_motor_master.getSelectedSensorPosition(0), self.ENCODER_COUNTS_PER_REV, self.WHEEL_DIAMETER) leftFollower.configurePIDVA(1.0, 0.0, 0.0, 1 / self.MAX_VELOCITY, 0) rightFollower = pf.followers.EncoderFollower(right) rightFollower.configureEncoder( self.r_motor_master.getSelectedSensorPosition(0), self.ENCODER_COUNTS_PER_REV, self.WHEEL_DIAMETER) rightFollower.configurePIDVA(1.0, 0.0, 0.0, 1 / self.MAX_VELOCITY, 0) self.leftFollower = leftFollower self.rightFollower = rightFollower # This code renders the followed path on the field in simulation (requires pyfrc 2018.2.0+) if wpilib.RobotBase.isSimulation(): from pyfrc.sim import get_user_renderer renderer = get_user_renderer() if renderer: renderer.draw_pathfinder_trajectory(left, color="#0000ff", offset=(-1, 0)) renderer.draw_pathfinder_trajectory(modifier.source, color="#00ff00", show_dt=1.0, dt_offset=0.0) renderer.draw_pathfinder_trajectory(right, color="#0000ff", offset=(1, 0))
import os import pathfinder as pf from utilities.functions import GenerateMotionProfile SAMPLE_PERIOD = 10 # MS DISTANCE = 8.05 # rotations (825 native units) MAX_VELOCITY = 4.0 # rotations / second MAX_ACCELERATION = 8.0 # rotations / second^2 MAX_JERK = 15.0 # rotations / second^3 POSITION_UNITS = 1023 * (1 / 10) # 10-bit ADC / 10-turn pot...rotations VELOCITY_UNITS = 1023 / 100 # 10-bit ADC / 100ms...natural units of Talon velocity # Generate the path info, trajectory = pf.generate( [pf.Waypoint(0.0, 0.0, 0.0), pf.Waypoint(DISTANCE, 0.0, 0.0)], pf.FIT_HERMITE_CUBIC, 1000000, SAMPLE_PERIOD / 1000, MAX_VELOCITY, MAX_ACCELERATION, MAX_JERK) GenerateMotionProfile(os.path.dirname(__file__), "boom_intake_to_scale", trajectory, POSITION_UNITS, VELOCITY_UNITS)
paths = {} for key, points in TRAJECTORIES.items(): if key in TRAJECTORY_OPTIONS: opts = TRAJECTORY_OPTIONS[key] else: opts = {} max_velocity = opts.get('max_velocity', MAX_VELOCITY) max_accel = opts.get('max_accel', MAX_ACCEL) max_jerk = opts.get('max_jerk', MAX_JERK) info, trajectory = pf.generate(points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH, DT, max_velocity, max_accel, max_jerk) modifier = pf.modifiers.TankModifier( trajectory).modify(WHEELBASE_WIDTH) print(key, info) left_traj = modifier.getLeftTrajectory() right_traj = modifier.getRightTrajectory() paths[key + '-l'] = left_traj paths[key + '-r'] = right_traj paths[key] = trajectory if len(sys.argv) > 2 and sys.argv[2] == '--dump-csvs':