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':
