def test_multi_waypoint_converge():
speed = 1 # m/s
orientation = 0 # rad
robot_x = 0.0
robot_y = 1.0
waypoints = np.array([[0, 0, 0, 0], [1, 0, 0, 1], [1, 100, 0, 1]])
pursuit = VectorPursuit()
pursuit.set_motion_params(3.0, 4.5, -4.5)
pursuit.set_waypoints(waypoints)
last_speed = 0
fake_tm = time.monotonic()
for i in range(500):
theta, next_seg, over = pursuit.get_output(np.array([robot_x,
robot_y]),
last_speed,
current_tm=fake_tm)
robot_x, robot_y = position_delta_x_y(theta, speed, robot_x, robot_y,
orientation)
if over:
break
last_speed = speed
fake_tm += MagicRobot.control_loop_wait_time
assert abs(robot_x - 1) < 0.1
assert robot_y > 2
def test_single_waypoint_converge():
orientation = 0 # rad
robot_x = 0.0
robot_y = 1.0
waypoints = np.array([[0, 0, 0, 0], [100, 0, 0, 1]])
pursuit = VectorPursuit()
pursuit.set_motion_params(3.0, 4.5, -4.5)
pursuit.set_waypoints(waypoints)
last_speed = 0
for i in range(200):
theta, speed, next_seg, over = pursuit.get_output(np.array([robot_x, robot_y]), last_speed)
robot_x, robot_y = position_delta_x_y(theta, speed,
robot_x, robot_y, orientation)
if over:
break
last_speed = speed
assert robot_x > 2
assert abs(robot_y) < 0.1
def test_speed_control():
speed = 1 # m/s
orientation = 0 # rad
robot_x = 0.0
robot_y = 1.0
waypoints = np.array([[0, 0, 0, 0], [1, 0, 0, 1], [1, 2, 0, 2]])
pursuit = VectorPursuit()
pursuit.set_motion_params(3.0, 4.5, -4.5)
pursuit.set_waypoints(waypoints)
last_speed = 0
for i in range(500):
theta, speed, next_seg, over = pursuit.get_output(np.array([robot_x, robot_y]), last_speed)
robot_x, robot_y = position_delta_x_y(theta, speed,
robot_x, robot_y, orientation)
if over:
break
last_speed = speed
assert abs(speed - 2) < 0.2
def __init__(self):
self.enabled = False
self.pursuit = VectorPursuit()
self.last_heading_error = 0
class ChassisMotion:
chassis: Chassis
imu: IMU
# heading motion feedforward/back gains
kPh = 3 # proportional gain
kVh = 1 # feedforward gain
kIh = 0 # integral gain
kDh = 0 # derivative gain
kAh = 0.2
kP = 0.5
kI = 0
kD = 0
kV = 1
kA = 0.2
waypoint_corner_radius = 0.7
def __init__(self):
self.enabled = False
self.pursuit = VectorPursuit()
self.last_heading_error = 0
def set_trajectory(self,
waypoints: np.ndarray,
end_heading,
start_speed=0.0,
end_speed=0.25,
smooth=True,
motion_params=(2.5, 2, 1.5),
waypoint_corner_radius=None,
wait_for_rotate=False):
""" Pass as set of waypoints for the chassis to follow.
Args:
waypoints: A numpy array of waypoints that the chassis will follow.
Waypoints are themselves arrays, constructed as follows:
[x_in_meters, y_in_meters]
"""
print(f'original_waypoints {waypoints}')
if smooth:
if waypoint_corner_radius is None:
waypoint_corner_radius = self.waypoint_corner_radius
waypoints_smoothed = smooth_waypoints(
waypoints, radius=waypoint_corner_radius)
else:
waypoints_smoothed = [np.array(point) for point in waypoints]
print(f'smoothed_waypoints {waypoints_smoothed}')
print(f'end_heading {end_heading}')
trajectory_length = sum([
np.linalg.norm(waypoints_smoothed[i] - waypoints_smoothed[i - 1])
for i in range(1, len(waypoints_smoothed))
])
self.end_heading = end_heading
self.end_distance = trajectory_length
self.start_segment_tm = time.monotonic()
self.update_linear_profile(motion_params, start_speed, end_speed)
self.update_heading_profile()
self.waypoints = waypoints_smoothed
self.pursuit.set_waypoints(waypoints_smoothed)
self.wait_for_rotate = wait_for_rotate
self.started_moving = False
print(f'Wait for rotate {self.wait_for_rotate}')
self.enabled = True
if self.distance_traj_tm < self.heading_traj_tm:
print(
f'WARNING: Heading trajectory ({self.heading_traj_tm}s) longer than linear trajectory ({self.distance_traj_tm}s)'
)
self.chassis.heading_hold_off()
def update_linear_profile(self, motion_params, start_speed, end_speed):
v_max, a_pos, a_neg = motion_params
self.speed_function, self.distance_traj_tm = generate_trapezoidal_function(
0,
start_speed,
self.end_distance,
end_speed,
v_max=v_max,
a_pos=a_pos,
a_neg=a_neg)
self.linear_position = 0
self.last_position = self.chassis.position
print(f'start_position {self.last_position}')
self.last_linear_error = 0
self.linear_error_i = 0
def update_heading_profile(self):
heading = self.imu.getAngle()
# this ensures we always rotate the same way going from the switch to
# the scale, but that we do not do 360s while maniuplating cubes at the
# switch
delta = constrain_angle(self.end_heading - heading)
end_heading = heading + delta
self.heading_function, self.heading_traj_tm = generate_trapezoidal_function(
heading, 0, end_heading, 0, v_max=3, a_pos=2, a_neg=2)
self.last_heading_error = 0
self.heading_error_i = 0
@property
def trajectory_executing(self):
return self.enabled
@property
def average_speed(self):
return sum(module.wheel_vel for module in self.chassis.modules) / 4
def disable(self):
self.enabled = False
def on_enable(self):
self.waypoints = []
self.enabled = False
def execute(self):
if self.enabled:
self.chassis.field_oriented = True
self.chassis.hold_heading = False
# TODO: re-enable if we end up not using callback method
# self.chassis.update_odometry()
direction_of_motion, next_seg, over = self.pursuit.get_output(
self.chassis.position, self.chassis.speed)
speed_sp = self.run_speed_controller()
heading_output, heading_error = self.run_heading_controller()
vx = speed_sp * math.cos(direction_of_motion)
vy = speed_sp * math.sin(direction_of_motion)
if not self.started_moving:
if self.chassis.all_aligned:
self.started_moving = True
else:
self.chassis.set_inputs(vx / 100, vy / 100,
heading_output / 100)
self.chassis.set_inputs(vx, vy, heading_output)
SmartDashboard.putNumber('vector_pursuit_heading',
direction_of_motion)
SmartDashboard.putNumber('vector_pursuit_speed', speed_sp)
if over:
if not self.wait_for_rotate:
print(
f"Motion over at {self.chassis.position}, heading {self.imu.getAngle()}"
)
self.enabled = False
self.chassis.set_inputs(0, 0, 0)
else:
if abs(heading_error) < 0.1:
print(
f"Motion over at {self.chassis.position}, heading {self.imu.getAngle()}"
)
self.enabled = False
self.chassis.set_inputs(0, 0, 0)
def run_speed_controller(self):
chassis_pos = self.chassis.position
self.linear_position += np.linalg.norm(chassis_pos -
self.last_position)
profile_tm = time.monotonic() - self.start_segment_tm
if profile_tm > self.distance_traj_tm:
return 0.25
linear_seg = self.speed_function(profile_tm)
if linear_seg is None:
linear_seg = (0, 0, 0)
print("WARNING: Linear segment is 0")
SmartDashboard.putNumber("vector_pursuit_position",
self.linear_position)
# calculate the position errror
pos_error = linear_seg[0] - self.linear_position
# calucate the derivative of the position error
self.d_pos_error = (pos_error - self.last_linear_error)
# sum the position error over the timestep
self.linear_error_i += pos_error
# generate the linear output to the chassis (m/s)
speed_sp = (self.kP * pos_error + self.kV * linear_seg[1] +
self.kA * linear_seg[2] + self.kI * self.linear_error_i +
self.kD * self.d_pos_error)
self.last_position = chassis_pos
self.last_linear_error = pos_error
SmartDashboard.putNumber('linear_mp_sp', linear_seg[0])
SmartDashboard.putNumber('linear_mp_error', pos_error)
SmartDashboard.putNumber('linear_pos', self.linear_position)
return speed_sp
def run_heading_controller(self):
if self.heading_function is not None:
heading_time = time.monotonic() - self.start_segment_tm
if heading_time < self.heading_traj_tm:
heading_seg = self.heading_function(heading_time)
else:
self.heading_function = None
heading_seg = (self.end_heading, 0, 0)
else:
heading_seg = (self.end_heading, 0, 0)
# get the current heading of the robot since last reset
# getRawHeading has been swapped for getAngle
heading = self.imu.getAngle()
# calculate the heading error
heading_error = heading_seg[0] - heading
# wrap heading error, stops jumping by 2 pi from the imu
heading_error = constrain_angle(heading_error)
# sum the heading error over the timestep
self.heading_error_i += heading_error
# calculate the derivative of the heading error
d_heading_error = (heading_error - self.last_heading_error)
# generate the rotational output to the chassis
heading_output = (self.kPh * heading_error +
self.kAh * heading_seg[2] +
self.kVh * heading_seg[1] +
self.heading_error_i * self.kIh +
d_heading_error * self.kDh)
# store the current errors to be used to compute the
# derivatives in the next timestep
self.last_heading_error = heading_error
SmartDashboard.putNumber('heading_mp_sp', heading_seg[0])
SmartDashboard.putNumber('heading_mp_error', heading_error)
return heading_output, heading_error
@property
def waypoint_idx(self):
return self.pursuit.segment_idx
def __init__(self):
self.enabled = False
self.pursuit = VectorPursuit()
class ChassisMotion:
chassis: SwerveChassis
imu: NavX
# heading motion feedforward/back gains
kPh = 3 # proportional gain
kVh = 1 # feedforward gain
kIh = 0 # integral gain
kDh = 10 # derivative gain
def __init__(self):
self.enabled = False
self.pursuit = VectorPursuit()
def setup(self):
self.pursuit.set_motion_params(4.0, 4, -3)
def set_waypoints(self, waypoints: np.ndarray):
""" Pass as set of waypoints for the chassis to follow.
Args:
waypoints: A numpy array of waypoints that the chassis will follow.
Waypoints are themselves arrays, constructed as follows:
[x_in_meters, y_in_meters, orientation_in_radians, speed_in_meters]
"""
self.waypoints = waypoints
self.pursuit.set_waypoints(waypoints)
self.enabled = True
self.chassis.heading_hold_on()
self.update_heading_profile()
self.heading_error_i = 0
def update_heading_profile(self):
self.current_seg_distance = np.linalg.norm(self.pursuit.segment)
heading_end = self.waypoints[self.waypoint_idx+1][2]
self.heading_profile = generate_trapezoidal_trajectory(self.imu.getAngle(), self.imu.getHeadingRate(),
heading_end, 0, 3, 3, -3, 50)
self.last_heading_error = 0
def disable(self):
self.enabled = False
def on_enable(self):
pass
def execute(self):
if self.enabled:
self.chassis.field_oriented = True
self.chassis.hold_heading = False
odom_pos = np.array([self.chassis.odometry_x, self.chassis.odometry_y])
odom_vel = np.array([self.chassis.odometry_x_vel, self.chassis.odometry_y_vel])
speed = np.linalg.norm(odom_vel)
# print("Odom speed %s" % speed)
direction_of_motion, speed_sp, next_seg, over = self.pursuit.get_output(odom_pos, speed)
if next_seg:
self.update_heading_profile()
seg_end = self.pursuit.waypoints_xy[self.waypoint_idx+1]
seg_end_dist = (np.linalg.norm(self.pursuit.segment)
- np.linalg.norm(seg_end - odom_pos))
if seg_end_dist < 0:
seg_end_dist = 0
if self.heading_profile:
heading_seg = self.heading_profile.pop(0)
else:
heading_seg = (self.waypoints[self.waypoint_idx+1][2], 0, 0)
# get the current heading of the robot since last reset
# getRawHeading has been swapped for getAngle
heading = self.imu.getAngle()
# calculate the heading error
heading_error = heading_seg[0] - heading
# wrap heading error, stops jumping by 2 pi from the imu
heading_error = math.atan2(math.sin(heading_error),
math.cos(heading_error))
# sum the heading error over the timestep
self.heading_error_i += heading_error
# calculate the derivative of the heading error
d_heading_error = (heading_error - self.last_heading_error)
# generate the rotational output to the chassis
heading_output = (
self.kPh * heading_error + self.kVh * heading_seg[1]
+ self.heading_error_i*self.kIh + d_heading_error*self.kDh)
# store the current errors to be used to compute the
# derivatives in the next timestep
self.last_heading_error = heading_error
vx = speed_sp * math.cos(direction_of_motion)
vy = speed_sp * math.sin(direction_of_motion)
# self.chassis.set_velocity_heading(vx, vy, self.waypoints[self.waypoint_idx+1][2])
self.chassis.set_inputs(vx, vy, heading_output)
SmartDashboard.putNumber('vector_pursuit_heading', direction_of_motion)
SmartDashboard.putNumber('vector_pursuit_speed', speed_sp)
NetworkTables.flush()
if over:
print("Motion over")
self.enabled = False
if self.waypoints[-1][3] == 0:
self.chassis.set_inputs(0, 0, 0)
@property
def waypoint_idx(self):
return self.pursuit.segment_idx