def drawSimulation(self, timed_trajectory: TimedTrajectory) -> None:
if RobotBase.isSimulation() and get_user_renderer() != None:
points = np.empty((0, 2))
for timed_state in timed_trajectory.timed_states:
state = timed_state.state
point = np.array([state.pose.x,
-state.pose.y + 4.1148]).reshape((1, 2))
points = np.append(points, point, axis=0)
get_user_renderer().draw_line(
points,
scale=(units.feet_per_meter, units.feet_per_meter),
color="#FFFFFF",
width=2,
)
def __init__(self, physics_controller):
self.logger = logging.getLogger("PhysicsEngine")
self.physics_controller = physics_controller
self.drivetrain = drivetrains.MecanumDrivetrain()
self.initial = True
self.simulated_position = 0.0
self.user_renderer = get_user_renderer()
def initialize(self):
poz = pose_estimator.get_current_pose()
self.target_angle = (self.lookat - poz).angle() * 180 / math.pi
if hal.isSimulation():
from pyfrc.sim import get_user_renderer
render = get_user_renderer()
if render is not None: # If running the standalone (pyplot) sim, this is None
render.draw_line([(poz.x, -poz.y + 14), (self.lookat.x, -self.lookat.y + 14)], color="#00ff00", arrow=False)
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 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_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 initialize(self):
self.pp_controller.init()
print("Started pursuit")
cur_pose = pose_estimator.get_current_pose()
poz = pose_estimator.get_current_pose()
line_pts = []
for t in range(1000):
t0 = t / 1000
pt = self.pp_controller.path.spline.get_point(t0)
line_pts += [(pt.x, -pt.y + 14)]
dashboard2.draw(
list(map(lambda x: Vector2(x[0], -(x[1] - 14)), line_pts)))
if hal.isSimulation():
from pyfrc.sim import get_user_renderer
render = get_user_renderer()
render.draw_line(line_pts, robot_coordinates=False)
dashboard2.add_graph("CTE", self.pp_controller.get_cte)
dashboard2.add_graph("Lookahead",
lambda: self.pp_controller.current_lookahead)
dashboard2.add_graph("Goal Distance", lambda: self.goal_dist)
dashboard2.add_graph("Speed", lambda: self.speed)
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))
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))