class QuickBotSupervisor(Supervisor):
"""The QuickBotSupervisor inherits from the superclass 'supervisor.Supervisor' to implement detailed calculations for any inheriting QuickBot supervisor.
Most importantly, the QuickBotSupervisor object implements the system functions necessary to operate a QuickBot, namely the uni2diff unicycle to differential motion model conversion, the Jacobian problem, and any other computationally complex interface.
The UI may use the get_parameters function interface to create docker windows for real-time update of the PID parameters. This is an advanced implementation and is not required for students to properly implement their own supervisors."""
def __init__(self, robot_pose, robot_info):
"""Initialize internal variables"""
Supervisor.__init__(self, robot_pose, robot_info)
# initialize memory registers
self.prev_left_ticks = robot_info.wheels.left_ticks
self.prev_right_ticks = robot_info.wheels.right_ticks
# Create tracker
self.tracker = Path(robot_pose, 0)
# Create & set the controller
self.current = self.create_controller('GoToAngle', self.parameters)
def init_default_parameters(self):
"""Sets the default PID parameters, goal, and velocity"""
p = Struct()
p.goal = 45.0
p.velocity = 0.2
p.pgain = 3.0
self.parameters = p
def get_ui_description(self, p=None):
"""Returns the UI description for the docker"""
if p is None:
p = self.parameters
return [(('goal', 'Target angle'), p.goal), ('velocity', p.velocity),
(('pgain', "Proportional gain"), p.pgain)]
def set_parameters(self, params):
"""Set parameters for itself and the controllers"""
self.parameters.goal = params.goal
self.parameters.velocity = params.velocity
self.parameters.pgain = params.pgain
self.current.set_parameters(self.parameters)
def uni2diff(self, uni):
"""Convert from unicycle model to differential model"""
(v, w) = uni
#Insert Week 2 Assignment Code Here
# R = self.robot.wheels.radius
# L = self.robot.wheels.base_length
vl = 0
vr = 0
#End Week 2 Assignment Code
return (vl, vr)
def estimate_pose(self):
"""Update self.pose_est using odometry"""
#Insert Week 2 Assignment Code Here
# Get tick updates
#self.robot.wheels.left_ticks
#self.robot.wheels.right_ticks
# Save the wheel encoder ticks for the next estimate
#Get the present pose estimate
x, y, theta = self.pose_est
#Use your math to update these variables...
theta_new = 0
x_new = 0
y_new = 0
#End Week 2 Assignment Code
return Pose(x_new, y_new, (theta_new + pi) % (2 * pi) - pi)
def get_ir_distances(self):
"""Converts the IR distance readings into a distance in meters"""
#Insert Week 2 Assignment Code Here
ir_distances = [0] * len(
self.robot.ir_sensors.readings) #populate this list
#End Assignment week2
return ir_distances
def execute(self, robot_info, dt):
"""Inherit default supervisor procedures and return unicycle model output (x, y, theta)"""
output = Supervisor.execute(self, robot_info, dt)
self.tracker.add_point(self.pose_est)
return self.uni2diff(output)
def process_state_info(self, state):
"""Update state parameters for the controllers and self"""
Supervisor.process_state_info(self, state)
self.parameters.pose = self.pose_est
def draw(self, renderer):
"""Draw a circular goal, path and ir_sensors"""
# Draw goal
renderer.set_pose(
Pose(self.pose_est.x, self.pose_est.y,
pi * self.parameters.goal / 180))
renderer.set_pen(0)
# renderer.draw_line(0.03,0,100,0)
renderer.draw_arrow(0.05, 0, 0.5, 0, close=True)
# Draw robot path
self.tracker.draw(renderer)
renderer.set_pose(self.pose_est)
renderer.set_brush(0)
renderer.draw_ellipse(0, 0, 0.01, 0.01)
# Draw IR distances
crosses = array([
dot(p.get_transformation(), [d, 0, 1]) for d, p in zip(
self.get_ir_distances(), self.robot.ir_sensors.poses)
])
renderer.set_pen(0)
for c in crosses:
x, y, z = c
renderer.push_state()
renderer.translate(x, y)
renderer.rotate(atan2(y, x))
renderer.draw_line(0.01, 0.01, -0.01, -0.01)
renderer.draw_line(0.01, -0.01, -0.01, 0.01)
renderer.pop_state()
def get_controller_state(self):
return self.parameters
class QBGTGSupervisor(QuickBotSupervisor):
"""QBGTG supervisor uses one go-to-goal controller to make the robot reach the goal."""
def __init__(self, robot_pose, robot_info):
"""Create the controller"""
QuickBotSupervisor.__init__(self, robot_pose, robot_info)
# Create the tracker
self.tracker = Path(robot_pose, 0)
# Create the controller
self.gtg = self.create_controller('week3.GoToGoal', self.parameters)
# Set the controller
self.current = self.gtg
def set_parameters(self, params):
"""Set parameters for itself and the controllers"""
QuickBotSupervisor.set_parameters(self, params)
self.gtg.set_parameters(self.parameters)
def process_state_info(self, state):
"""Update state parameters for the controllers and self"""
QuickBotSupervisor.process_state_info(self, state)
# The pose for controllers
self.parameters.pose = self.pose_est
# Update the trajectory
self.tracker.add_point(self.pose_est)
def draw_background(self, renderer):
"""Draw controller info"""
QuickBotSupervisor.draw_background(self, renderer)
# Draw robot path
self.tracker.draw(renderer)
def draw_foreground(self, renderer):
"""Draw controller info"""
QuickBotSupervisor.draw_foreground(self, renderer)
renderer.set_pose(Pose(self.pose_est.x, self.pose_est.y))
arrow_length = self.robot_size * 5
# Draw arrow to goal
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0, 0, arrow_length * cos(self.gtg.heading_angle),
arrow_length * sin(self.gtg.heading_angle))
def at_goal(self):
distance_to_goal = sqrt( \
(self.pose_est.x - self.parameters.goal.x)**2
+ (self.pose_est.y - self.parameters.goal.y)**2)
return distance_to_goal < 0.02
def ensure_w(self, v_lr):
v_l, v_r = v_lr
#Week 3 Assignment Code:
#End Week 3 Assigment
return v_l, v_r
def execute(self, robot_info, dt):
"""Inherit default supervisor procedures and return unicycle model output (vl,vr)"""
if not self.at_goal():
output = Supervisor.execute(self, robot_info, dt)
return self.ensure_w(self.uni2diff(output))
else:
return 0, 0
class QBGTGSupervisor(QuickBotSupervisor):
"""QBGTG supervisor uses one go-to-goal controller to make the robot reach the goal."""
def __init__(self, robot_pose, robot_info):
"""Create the controller"""
QuickBotSupervisor.__init__(self, robot_pose, robot_info)
# Create the tracker
self.tracker = Path(robot_pose, 0)
# Create the controller
self.gtg = self.create_controller('week3.GoToGoal', self.parameters)
# Set the controller
self.current = self.gtg
def set_parameters(self,params):
"""Set parameters for itself and the controllers"""
QuickBotSupervisor.set_parameters(self,params)
self.gtg.set_parameters(self.parameters)
def process_state_info(self, state):
"""Update state parameters for the controllers and self"""
QuickBotSupervisor.process_state_info(self,state)
# The pose for controllers
self.parameters.pose = self.pose_est
# Update the trajectory
self.tracker.add_point(self.pose_est)
def draw_background(self, renderer):
"""Draw controller info"""
QuickBotSupervisor.draw_background(self,renderer)
# Draw robot path
self.tracker.draw(renderer)
def draw_foreground(self, renderer):
"""Draw controller info"""
QuickBotSupervisor.draw_foreground(self,renderer)
renderer.set_pose(Pose(self.pose_est.x,self.pose_est.y))
arrow_length = self.robot_size*5
# Draw arrow to goal
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0,0,
arrow_length*cos(self.gtg.heading_angle),
arrow_length*sin(self.gtg.heading_angle))
def at_goal(self):
distance_to_goal = sqrt( \
(self.pose_est.x - self.parameters.goal.x)**2
+ (self.pose_est.y - self.parameters.goal.y)**2)
return distance_to_goal < 0.02
def ensure_w(self,v_lr):
v_l, v_r = v_lr
#Week 3 Assignment Code:
#End Week 3 Assigment
return v_l, v_r
def execute(self, robot_info, dt):
"""Inherit default supervisor procedures and return unicycle model output (vl,vr)"""
if not self.at_goal():
output = Supervisor.execute(self, robot_info, dt)
return self.ensure_w(self.uni2diff(output))
else:
return 0,0
class QuickBotSupervisor(Supervisor):
"""The QuickBotSupervisor inherits from the superclass 'supervisor.Supervisor' to implement detailed calculations for any inheriting QuickBot supervisor.
Most importantly, the QuickBotSupervisor object implements the system functions necessary to operate a QuickBot, namely the uni2diff unicycle to differential motion model conversion, the Jacobian problem, and any other computationally complex interface.
The UI may use the get_parameters function interface to create docker windows for real-time update of the PID parameters. This is an advanced implementation and is not required for students to properly implement their own supervisors."""
def __init__(self, robot_pose, robot_info):
"""Initialize internal variables"""
Supervisor.__init__(self, robot_pose, robot_info)
# initialize memory registers
self.prev_left_ticks = robot_info.wheels.left_ticks
self.prev_right_ticks = robot_info.wheels.right_ticks
# Create tracker
self.tracker = Path(robot_pose, 0)
# Create & set the controller
self.current = self.create_controller("GoToAngle", self.parameters)
def init_default_parameters(self):
"""Sets the default PID parameters, goal, and velocity"""
p = Struct()
p.goal = 45.0
p.velocity = 0.2
p.pgain = 3.0
self.parameters = p
def get_ui_description(self, p=None):
"""Returns the UI description for the docker"""
if p is None:
p = self.parameters
return [(("goal", "Target angle"), p.goal), ("velocity", p.velocity), (("pgain", "Proportional gain"), p.pgain)]
def set_parameters(self, params):
"""Set parameters for itself and the controllers"""
self.parameters.goal = params.goal
self.parameters.velocity = params.velocity
self.parameters.pgain = params.pgain
self.current.set_parameters(self.parameters)
def uni2diff(self, uni):
"""Convert from unicycle model to differential model"""
(v, w) = uni
# Insert Week 2 Assignment Code Here
# R = self.robot.wheels.radius
# L = self.robot.wheels.base_length
vl = 0
vr = 0
# End Week 2 Assignment Code
return (vl, vr)
def estimate_pose(self):
"""Update self.pose_est using odometry"""
# Insert Week 2 Assignment Code Here
# Get tick updates
# self.robot.wheels.left_ticks
# self.robot.wheels.right_ticks
# Save the wheel encoder ticks for the next estimate
# Get the present pose estimate
x, y, theta = self.pose_est
# Use your math to update these variables...
theta_new = 0
x_new = 0
y_new = 0
# End Week 2 Assignment Code
return Pose(x_new, y_new, (theta_new + pi) % (2 * pi) - pi)
def get_ir_distances(self):
"""Converts the IR distance readings into a distance in meters"""
# Insert Week 2 Assignment Code Here
ir_distances = [0] * len(self.robot.ir_sensors.readings) # populate this list
# End Assignment week2
return ir_distances
def execute(self, robot_info, dt):
"""Inherit default supervisor procedures and return unicycle model output (x, y, theta)"""
output = Supervisor.execute(self, robot_info, dt)
self.tracker.add_point(self.pose_est)
return self.uni2diff(output)
def process_state_info(self, state):
"""Update state parameters for the controllers and self"""
Supervisor.process_state_info(self, state)
self.parameters.pose = self.pose_est
def draw(self, renderer):
"""Draw a circular goal, path and ir_sensors"""
# Draw goal
renderer.set_pose(Pose(self.pose_est.x, self.pose_est.y, pi * self.parameters.goal / 180))
renderer.set_pen(0)
# renderer.draw_line(0.03,0,100,0)
renderer.draw_arrow(0.05, 0, 0.5, 0, close=True)
# Draw robot path
self.tracker.draw(renderer)
renderer.set_pose(self.pose_est)
renderer.set_brush(0)
renderer.draw_ellipse(0, 0, 0.01, 0.01)
# Draw IR distances
crosses = array(
[
dot(p.get_transformation(), [d, 0, 1])
for d, p in zip(self.get_ir_distances(), self.robot.ir_sensors.poses)
]
)
renderer.set_pen(0)
for c in crosses:
x, y, z = c
renderer.push_state()
renderer.translate(x, y)
renderer.rotate(atan2(y, x))
renderer.draw_line(0.01, 0.01, -0.01, -0.01)
renderer.draw_line(0.01, -0.01, -0.01, 0.01)
renderer.pop_state()
def get_controller_state(self):
return self.parameters
class QBGTGSupervisor(QuickBotSupervisor):
"""QBGTG supervisor uses one go-to-goal controller to make the robot reach the goal."""
def __init__(self, robot_pose, robot_info):
"""Create the controller"""
QuickBotSupervisor.__init__(self, robot_pose, robot_info)
# Create the tracker
self.tracker = Path(robot_pose, 0)
# Create the controller
self.gtg = self.create_controller('week3.GoToGoal', self.parameters)
# Set the controller
self.current = self.gtg
def set_parameters(self,params):
"""Set parameters for itself and the controllers"""
QuickBotSupervisor.set_parameters(self,params)
self.gtg.set_parameters(self.parameters)
def process_state_info(self, state):
"""Update state parameters for the controllers and self"""
QuickBotSupervisor.process_state_info(self,state)
# The pose for controllers
self.parameters.pose = self.pose_est
# Update the trajectory
self.tracker.add_point(self.pose_est)
def draw_background(self, renderer):
"""Draw controller info"""
QuickBotSupervisor.draw_background(self,renderer)
# Draw robot path
self.tracker.draw(renderer)
def draw_foreground(self, renderer):
"""Draw controller info"""
QuickBotSupervisor.draw_foreground(self,renderer)
renderer.set_pose(Pose(self.pose_est.x,self.pose_est.y))
arrow_length = self.robot_size*5
# Draw arrow to goal
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0,0,
arrow_length*cos(self.gtg.heading_angle),
arrow_length*sin(self.gtg.heading_angle))
def at_goal(self):
distance_to_goal = sqrt( \
(self.pose_est.x - self.parameters.goal.x)**2
+ (self.pose_est.y - self.parameters.goal.y)**2)
return distance_to_goal < 0.02
def ensure_w(self,v_lr):
#Week 3 Assignment Code:
#End Week 3 Assigment
# This code is taken directly from Sim.I.Am week 4
# I'm sure one can do better.
v_max = self.robot.wheels.max_velocity
v_min = self.robot.wheels.min_velocity
R = self.robot.wheels.radius
L = self.robot.wheels.base_length
def diff2uni(vl, vr):
return (vl + vr) * R / 2, (vr - vl) * R / L
v, w = diff2uni(*v_lr)
if v == 0:
# Robot is stationary, so we can either not rotate, or
# rotate with some minimum/maximum angular velocity
w_min = R / L * (2 * v_min);
w_max = R / L * (2 * v_max);
if abs(w) > w_min:
w = copysign(max(min(abs(w), w_max), w_min), w)
else:
w = 0
return self.uni2diff((0, w))
else:
# 1. Limit v,w to be possible in the range [vel_min, vel_max]
# (avoid stalling or exceeding motor limits)
v_lim = max(min(abs(v), (R / 2) * (2 * v_max)), (R / 2) * (2 * v_min))
w_lim = max(min(abs(w), (R / L) * (v_max - v_min)), 0)
# 2. Compute the desired curvature of the robot's motion
vl, vr = self.uni2diff((v_lim, w_lim))
# 3. Find the max and min vel_r/vel_l
v_lr_max = max(vl, vr);
v_lr_min = min(vl, vr);
# 4. Shift vr and vl if they exceed max/min vel
if (v_lr_max > v_max):
vr -= v_lr_max - v_max
vl -= v_lr_max - v_max
elif (v_lr_min < v_min):
vr += v_min - v_lr_min
vl += v_min - v_lr_min
# 5. Fix signs (Always either both positive or negative)
v_shift, w_shift = diff2uni(vl, vr)
v = copysign(v_shift, v)
w = copysign(w_shift, w)
return self.uni2diff((v, w))
#return v_l, v_r
def execute(self, robot_info, dt):
"""Inherit default supervisor procedures and return unicycle model output (vl,vr)"""
if not self.at_goal():
output = Supervisor.execute(self, robot_info, dt)
#print(output)
return self.ensure_w(self.uni2diff(output))
#return output
else:
return 0,0