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