def __init__(self, pose, color=0xFFFFFF, options=None):
# create shape
self._shapes = Struct()
self._shapes.base_plate = np.array([[0.0335, 0.0534, 1],
[0.0429, 0.0534, 1],
[0.0639, 0.0334, 1],
[0.0686, 0.0000, 1],
[0.0639, -0.0334, 1],
[0.0429, -0.0534, 1],
[0.0335, -0.0534, 1],
[-0.0465, -0.0534, 1],
[-0.0815, -0.0534, 1],
[-0.1112, -0.0387, 1],
[-0.1112, 0.0387, 1],
[-0.0815, 0.0534, 1],
[-0.0465, 0.0534, 1]])
self._shapes.bbb = np.array([[-0.0914, -0.0406, 1],
[-0.0944, -0.0376,
1], [-0.0944, 0.0376, 1],
[-0.0914, 0.0406,
1], [-0.0429, 0.0406, 1],
[-0.0399, 0.0376, 1],
[-0.0399, -0.0376, 1],
[-0.0429, -0.0406, 1]])
self._shapes.bbb_rail_l = np.array([[-0.0429, -0.0356, 1],
[-0.0429, 0.0233, 1],
[-0.0479, 0.0233, 1],
[-0.0479, -0.0356, 1]])
self._shapes.bbb_rail_r = np.array([[-0.0914, -0.0356, 1],
[-0.0914, 0.0233, 1],
[-0.0864, 0.0233, 1],
[-0.0864, -0.0356, 1]])
self._shapes.bbb_eth = np.array([[-0.0579, 0.0436, 1],
[-0.0579, 0.0226, 1],
[-0.0739, 0.0226, 1],
[-0.0739, 0.0436, 1]])
self._shapes.left_wheel = np.array([[0.0254, 0.0595, 1],
[0.0254, 0.0335, 1],
[-0.0384, 0.0335, 1],
[-0.0384, 0.0595, 1]])
self._shapes.left_wheel_ol = np.array([[0.0254, 0.0595, 1],
[0.0254, 0.0335, 1],
[-0.0384, 0.0335, 1],
[-0.0384, 0.0595, 1]])
self._shapes.right_wheel_ol = np.array([[0.0254, -0.0595, 1],
[0.0254, -0.0335, 1],
[-0.0384, -0.0335, 1],
[-0.0384, -0.0595, 1]])
self._shapes.right_wheel = np.array([[0.0254, -0.0595, 1],
[0.0254, -0.0335, 1],
[-0.0384, -0.0335, 1],
[-0.0384, -0.0595, 1]])
self._shapes.ir_1 = np.array([[-0.0732, 0.0534,
1], [-0.0732, 0.0634, 1],
[-0.0432, 0.0634, 1],
[-0.0432, 0.0534, 1]])
self._shapes.ir_2 = np.array([[0.0643, 0.0214, 1], [0.0714, 0.0285, 1],
[0.0502, 0.0497, 1], [0.0431, 0.0426,
1]])
self._shapes.ir_3 = np.array([[0.0636, -0.0042, 1],
[0.0636, 0.0258, 1], [0.0736, 0.0258, 1],
[0.0736, -0.0042, 1]])
self._shapes.ir_4 = np.array([[0.0643, -0.0214,
1], [0.0714, -0.0285, 1],
[0.0502, -0.0497, 1],
[0.0431, -0.0426, 1]])
self._shapes.ir_5 = np.array([[-0.0732, -0.0534, 1],
[-0.0732, -0.0634, 1],
[-0.0432, -0.0634, 1],
[-0.0432, -0.0534, 1]])
self._shapes.bbb_usb = np.array([[-0.0824, -0.0418, 1],
[-0.0694, -0.0418, 1],
[-0.0694, -0.0278, 1],
[-0.0824, -0.0278, 1]])
ir_sensor_poses = [
Pose(-0.0474, 0.0534, np.radians(90)),
Pose(0.0613, 0.0244, np.radians(45)),
Pose(0.0636, 0.0, np.radians(0)),
Pose(0.0461, -0.0396, np.radians(-45)),
Pose(-0.0690, -0.0534, np.radians(-90))
]
self.info = Struct()
self.info.wheels = Struct()
self.info.wheels.radius = 0.0325
self.info.wheels.base_length = 0.09925
self.info.wheels.ticks_per_rev = 16
self.info.wheels.left_ticks = 0
self.info.wheels.right_ticks = 0
self.info.wheels.max_velocity = 2 * pi * 130 / 60 # 130 RPM
self.info.wheels.min_velocity = 2 * pi * 30 / 60 # 30 RPM
self.info.wheels.vel_left = 0
self.info.wheels.vel_right = 0
self.info.ir_sensors = Struct()
self.info.ir_sensors.poses = ir_sensor_poses
self.info.ir_sensors.rmax = 0.3
self.info.ir_sensors.rmin = 0.04
self.info.ir_sensors.readings = [133] * len(self.info.ir_sensors.poses)
self.walls = Cloud(0) # Black, no readings
# The constructor is called here because otherwise set_pose fails
RealBot.__init__(self, pose, color)
# Connect to bot...
# This code will raise an exception if not able to connect
if options is None:
self.log("No IP/port supplied to connect to the robot")
qb_comm.__init__(self, "localhost", "localhost", 5005)
else:
try:
qb_comm.__init__(self, options.baseIP, options.robotIP,
options.port)
except AttributeError:
self.log(
"No IP/port supplied in the options to connect to the robot"
)
qb_comm.__init__(self, "localhost", "localhost", 5005)
self.ping() # Check if the robot is there
self.pause() # Initialize self.__paused
def __init__(self, pose, color = 0xFFFFFF, options = None):
# create shape
self._shapes = Struct()
self._shapes.base_plate = np.array([[ 0.0335, 0.0534, 1],
[ 0.0429, 0.0534, 1],
[ 0.0639, 0.0334, 1],
[ 0.0686, 0.0000, 1],
[ 0.0639,-0.0334, 1],
[ 0.0429,-0.0534, 1],
[ 0.0335,-0.0534, 1],
[-0.0465,-0.0534, 1],
[-0.0815,-0.0534, 1],
[-0.1112,-0.0387, 1],
[-0.1112, 0.0387, 1],
[-0.0815, 0.0534, 1],
[-0.0465, 0.0534, 1]])
self._shapes.bbb = np.array([[-0.0914,-0.0406, 1],
[-0.0944,-0.0376, 1],
[-0.0944, 0.0376, 1],
[-0.0914, 0.0406, 1],
[-0.0429, 0.0406, 1],
[-0.0399, 0.0376, 1],
[-0.0399,-0.0376, 1],
[-0.0429,-0.0406, 1]])
self._shapes.bbb_rail_l = np.array([[-0.0429, -0.0356,1],
[-0.0429, 0.0233,1],
[-0.0479, 0.0233,1],
[-0.0479,-0.0356,1]])
self._shapes.bbb_rail_r = np.array([[-0.0914,-0.0356,1],
[-0.0914, 0.0233,1],
[-0.0864, 0.0233,1],
[-0.0864,-0.0356,1]])
self._shapes.bbb_eth = np.array([[-0.0579, 0.0436, 1],
[-0.0579, 0.0226, 1],
[-0.0739, 0.0226, 1],
[-0.0739, 0.0436, 1]])
self._shapes.left_wheel = np.array([[ 0.0254, 0.0595, 1],
[ 0.0254, 0.0335, 1],
[-0.0384, 0.0335, 1],
[-0.0384, 0.0595, 1]])
self._shapes.left_wheel_ol = np.array([[ 0.0254, 0.0595, 1],
[ 0.0254, 0.0335, 1],
[-0.0384, 0.0335, 1],
[-0.0384, 0.0595, 1]])
self._shapes.right_wheel_ol = np.array([[ 0.0254,-0.0595, 1],
[ 0.0254,-0.0335, 1],
[-0.0384,-0.0335, 1],
[-0.0384,-0.0595, 1]])
self._shapes.right_wheel = np.array([[ 0.0254,-0.0595, 1],
[ 0.0254,-0.0335, 1],
[-0.0384,-0.0335, 1],
[-0.0384,-0.0595, 1]])
self._shapes.ir_1 = np.array([[-0.0732, 0.0534, 1],
[-0.0732, 0.0634, 1],
[-0.0432, 0.0634, 1],
[-0.0432, 0.0534, 1]])
self._shapes.ir_2 = np.array([[ 0.0643, 0.0214, 1],
[ 0.0714, 0.0285, 1],
[ 0.0502, 0.0497, 1],
[ 0.0431, 0.0426, 1]])
self._shapes.ir_3 = np.array([[ 0.0636,-0.0042, 1],
[ 0.0636, 0.0258, 1],
[ 0.0736, 0.0258, 1],
[ 0.0736,-0.0042, 1]])
self._shapes.ir_4 = np.array([[ 0.0643,-0.0214, 1],
[ 0.0714,-0.0285, 1],
[ 0.0502,-0.0497, 1],
[ 0.0431,-0.0426, 1]])
self._shapes.ir_5 = np.array([[-0.0732,-0.0534, 1],
[-0.0732,-0.0634, 1],
[-0.0432,-0.0634, 1],
[-0.0432,-0.0534, 1]])
self._shapes.bbb_usb = np.array([[-0.0824,-0.0418, 1],
[-0.0694,-0.0418, 1],
[-0.0694,-0.0278, 1],
[-0.0824,-0.0278, 1]])
ir_sensor_poses = [
Pose(-0.0474, 0.0534, np.radians(90)),
Pose( 0.0613, 0.0244, np.radians(45)),
Pose( 0.0636, 0.0, np.radians(0)),
Pose( 0.0461,-0.0396, np.radians(-45)),
Pose(-0.0690,-0.0534, np.radians(-90))
]
self.info = Struct()
self.info.wheels = Struct()
self.info.wheels.radius = 0.0325
self.info.wheels.base_length = 0.09925
self.info.wheels.ticks_per_rev = 16
self.info.wheels.left_ticks = 0
self.info.wheels.right_ticks = 0
self.info.wheels.max_velocity = 2*pi*130/60 # 130 RPM
self.info.wheels.min_velocity = 2*pi*30/60 # 30 RPM
self.info.wheels.vel_left = 0
self.info.wheels.vel_right = 0
self.info.ir_sensors = Struct()
self.info.ir_sensors.poses = ir_sensor_poses
self.info.ir_sensors.rmax = 0.3
self.info.ir_sensors.rmin = 0.04
self.info.ir_sensors.readings = [133]*len(self.info.ir_sensors.poses)
self.walls = Cloud(0) # Black, no readings
# The constructor is called here because otherwise set_pose fails
RealBot.__init__(self, pose, color)
# Connect to bot...
# This code will raise an exception if not able to connect
if options is None:
self.log("No IP/port supplied to connect to the robot")
qb_comm.__init__(self, "localhost", "localhost", 5005)
else:
try:
qb_comm.__init__(self, options.baseIP, options.robotIP, options.port)
except AttributeError:
self.log("No IP/port supplied in the options to connect to the robot")
qb_comm.__init__(self, "localhost", "localhost", 5005)
self.ping() # Check if the robot is there
self.pause() # Initialize self.__paused
class QuickBot(RealBot, qb_comm):
"""Communication with a QuickBot"""
ir_coeff = np.array([
8.56495710e-18, -3.02930608e-14, 4.43025017e-11, -3.49052288e-08,
1.61452174e-05, -4.44025236e-03, 6.74137385e-1
])
beta = (1.0, 0.0)
def __init__(self, pose, color=0xFFFFFF, options=None):
# create shape
self._shapes = Struct()
self._shapes.base_plate = np.array([[0.0335, 0.0534, 1],
[0.0429, 0.0534, 1],
[0.0639, 0.0334, 1],
[0.0686, 0.0000, 1],
[0.0639, -0.0334, 1],
[0.0429, -0.0534, 1],
[0.0335, -0.0534, 1],
[-0.0465, -0.0534, 1],
[-0.0815, -0.0534, 1],
[-0.1112, -0.0387, 1],
[-0.1112, 0.0387, 1],
[-0.0815, 0.0534, 1],
[-0.0465, 0.0534, 1]])
self._shapes.bbb = np.array([[-0.0914, -0.0406, 1],
[-0.0944, -0.0376,
1], [-0.0944, 0.0376, 1],
[-0.0914, 0.0406,
1], [-0.0429, 0.0406, 1],
[-0.0399, 0.0376, 1],
[-0.0399, -0.0376, 1],
[-0.0429, -0.0406, 1]])
self._shapes.bbb_rail_l = np.array([[-0.0429, -0.0356, 1],
[-0.0429, 0.0233, 1],
[-0.0479, 0.0233, 1],
[-0.0479, -0.0356, 1]])
self._shapes.bbb_rail_r = np.array([[-0.0914, -0.0356, 1],
[-0.0914, 0.0233, 1],
[-0.0864, 0.0233, 1],
[-0.0864, -0.0356, 1]])
self._shapes.bbb_eth = np.array([[-0.0579, 0.0436, 1],
[-0.0579, 0.0226, 1],
[-0.0739, 0.0226, 1],
[-0.0739, 0.0436, 1]])
self._shapes.left_wheel = np.array([[0.0254, 0.0595, 1],
[0.0254, 0.0335, 1],
[-0.0384, 0.0335, 1],
[-0.0384, 0.0595, 1]])
self._shapes.left_wheel_ol = np.array([[0.0254, 0.0595, 1],
[0.0254, 0.0335, 1],
[-0.0384, 0.0335, 1],
[-0.0384, 0.0595, 1]])
self._shapes.right_wheel_ol = np.array([[0.0254, -0.0595, 1],
[0.0254, -0.0335, 1],
[-0.0384, -0.0335, 1],
[-0.0384, -0.0595, 1]])
self._shapes.right_wheel = np.array([[0.0254, -0.0595, 1],
[0.0254, -0.0335, 1],
[-0.0384, -0.0335, 1],
[-0.0384, -0.0595, 1]])
self._shapes.ir_1 = np.array([[-0.0732, 0.0534,
1], [-0.0732, 0.0634, 1],
[-0.0432, 0.0634, 1],
[-0.0432, 0.0534, 1]])
self._shapes.ir_2 = np.array([[0.0643, 0.0214, 1], [0.0714, 0.0285, 1],
[0.0502, 0.0497, 1], [0.0431, 0.0426,
1]])
self._shapes.ir_3 = np.array([[0.0636, -0.0042, 1],
[0.0636, 0.0258, 1], [0.0736, 0.0258, 1],
[0.0736, -0.0042, 1]])
self._shapes.ir_4 = np.array([[0.0643, -0.0214,
1], [0.0714, -0.0285, 1],
[0.0502, -0.0497, 1],
[0.0431, -0.0426, 1]])
self._shapes.ir_5 = np.array([[-0.0732, -0.0534, 1],
[-0.0732, -0.0634, 1],
[-0.0432, -0.0634, 1],
[-0.0432, -0.0534, 1]])
self._shapes.bbb_usb = np.array([[-0.0824, -0.0418, 1],
[-0.0694, -0.0418, 1],
[-0.0694, -0.0278, 1],
[-0.0824, -0.0278, 1]])
ir_sensor_poses = [
Pose(-0.0474, 0.0534, np.radians(90)),
Pose(0.0613, 0.0244, np.radians(45)),
Pose(0.0636, 0.0, np.radians(0)),
Pose(0.0461, -0.0396, np.radians(-45)),
Pose(-0.0690, -0.0534, np.radians(-90))
]
self.info = Struct()
self.info.wheels = Struct()
self.info.wheels.radius = 0.0325
self.info.wheels.base_length = 0.09925
self.info.wheels.ticks_per_rev = 16
self.info.wheels.left_ticks = 0
self.info.wheels.right_ticks = 0
self.info.wheels.max_velocity = 2 * pi * 130 / 60 # 130 RPM
self.info.wheels.min_velocity = 2 * pi * 30 / 60 # 30 RPM
self.info.wheels.vel_left = 0
self.info.wheels.vel_right = 0
self.info.ir_sensors = Struct()
self.info.ir_sensors.poses = ir_sensor_poses
self.info.ir_sensors.rmax = 0.3
self.info.ir_sensors.rmin = 0.04
self.info.ir_sensors.readings = [133] * len(self.info.ir_sensors.poses)
self.walls = Cloud(0) # Black, no readings
# The constructor is called here because otherwise set_pose fails
RealBot.__init__(self, pose, color)
# Connect to bot...
# This code will raise an exception if not able to connect
if options is None:
self.log("No IP/port supplied to connect to the robot")
qb_comm.__init__(self, "localhost", "localhost", 5005)
else:
try:
qb_comm.__init__(self, options.baseIP, options.robotIP,
options.port)
except AttributeError:
self.log(
"No IP/port supplied in the options to connect to the robot"
)
qb_comm.__init__(self, "localhost", "localhost", 5005)
self.ping() # Check if the robot is there
self.pause() # Initialize self.__paused
def set_pose(self, rpose):
RealBot.set_pose(self, rpose)
# We have to update walls here. WHY?
for pose, dst in zip(
self.info.ir_sensors.poses,
np.polyval(self.ir_coeff, self.info.ir_sensors.readings)):
if dst / self.info.ir_sensors.rmax <= 0.99:
self.walls.add_point(Pose(dst) >> pose >> self.get_pose())
def draw(self, r):
self.walls.draw(r)
r.set_pose(self.get_pose())
r.set_pen(0)
r.set_brush(0)
r.draw_polygon(self._shapes.ir_1)
r.draw_polygon(self._shapes.ir_2)
r.draw_polygon(self._shapes.ir_3)
r.draw_polygon(self._shapes.ir_4)
r.draw_polygon(self._shapes.ir_5)
r.draw_polygon(self._shapes.left_wheel)
r.draw_polygon(self._shapes.right_wheel)
r.set_pen(0x01000000)
r.set_brush(self.get_color())
r.draw_polygon(self._shapes.base_plate)
r.set_pen(0x10000000)
r.set_brush(None)
r.draw_polygon(self._shapes.left_wheel)
r.draw_polygon(self._shapes.right_wheel)
r.set_pen(None)
r.set_brush(0x333333)
r.draw_polygon(self._shapes.bbb)
r.set_brush(0)
r.draw_polygon(self._shapes.bbb_rail_l)
r.draw_polygon(self._shapes.bbb_rail_r)
r.set_brush(0xb2b2b2)
r.draw_polygon(self._shapes.bbb_eth)
r.draw_polygon(self._shapes.bbb_usb)
def get_envelope(self):
return self._shapes.base_plate
def draw_sensor(self, renderer, pose, dst):
renderer.push_state()
renderer.add_pose(pose)
phi = np.radians(3) # 6/2
rmin = self.info.ir_sensors.rmin
rmax = self.info.ir_sensors.rmax
if dst / rmax > 0.99:
renderer.set_brush(0x33FF5566)
else:
renderer.set_brush(0xCCFF5566)
renderer.draw_ellipse(0, 0, min(1, rmin / 2), min(1, rmin / 2))
renderer.draw_polygon([(rmin * cos(phi), rmin * sin(phi)),
(dst * cos(phi), dst * sin(phi)), (dst, 0),
(dst * cos(phi), -dst * sin(phi)),
(rmin * cos(phi), -rmin * sin(phi))])
renderer.pop_state()
def draw_sensors(self, renderer):
"""Draw the sensors that this robot has"""
renderer.set_pose(self.get_pose())
distances = np.polyval(self.ir_coeff, self.info.ir_sensors.readings)
for pose, distance in zip(self.info.ir_sensors.poses, distances):
self.draw_sensor(renderer, pose, distance)
def get_info(self):
return self.info
def v2pwm(self, vl, vr):
"""Convert from angular velocity to PWM"""
pwm_l = sign(vl) * (abs(vl) - self.beta[1]) / self.beta[0]
pwm_r = sign(vr) * (abs(vr) - self.beta[1]) / self.beta[0]
return pwm_l, pwm_r
def pwm2v(self, pwm_l, pwm_r):
"""Convert from PWM to angular velocity"""
vl = sign(pwm_l) * (self.beta[0] * abs(pwm_l) + self.beta[1])
vr = sign(pwm_r) * (self.beta[0] * abs(pwm_r) + self.beta[1])
return vl, vr
def set_inputs(self, inputs):
pwm_l, pwm_r = self.v2pwm(*inputs)
with self.connect() as connection:
self.set_pwm(pwm_l, pwm_r, connection)
speeds = self.get_pwm(connection)
if speeds is None:
raise RuntimeError("Communication with QuickBot failed")
if speeds is not None:
self.info.wheels.vel_left, self.info.wheels.vel_right = self.pwm2v(
*speeds)
def update_external_info(self):
"Communicate with the robot"
if self.__paused:
return
with self.connect() as connection:
ticks = self.get_encoder_ticks(connection)
if ticks is None:
raise RuntimeError("Communication with QuickBot failed")
self.info.wheels.left_ticks, self.info.wheels.right_ticks = ticks
irs = self.get_ir_raw_values(connection)
if irs is None:
raise RuntimeError("Communication with QuickBot failed")
self.info.ir_sensors.readings = irs
def reset(self):
self.__paused = True
self.info.wheels.vel_left, self.info.wheels.vel_right = 0, 0
self.send_reset()
def pause(self):
self.__paused = True
self.send_halt()
def resume(self):
self.__paused = False
self.set_inputs(
(self.info.wheels.vel_left, self.info.wheels.vel_right))
class QuickBot(RealBot, qb_comm):
"""Communication with a QuickBot"""
ir_coeff = np.array([ 8.56495710e-18, -3.02930608e-14,
4.43025017e-11, -3.49052288e-08,
1.61452174e-05, -4.44025236e-03,
6.74137385e-1])
beta = (1.0, 0.0)
def __init__(self, pose, color = 0xFFFFFF, options = None):
# create shape
self._shapes = Struct()
self._shapes.base_plate = np.array([[ 0.0335, 0.0534, 1],
[ 0.0429, 0.0534, 1],
[ 0.0639, 0.0334, 1],
[ 0.0686, 0.0000, 1],
[ 0.0639,-0.0334, 1],
[ 0.0429,-0.0534, 1],
[ 0.0335,-0.0534, 1],
[-0.0465,-0.0534, 1],
[-0.0815,-0.0534, 1],
[-0.1112,-0.0387, 1],
[-0.1112, 0.0387, 1],
[-0.0815, 0.0534, 1],
[-0.0465, 0.0534, 1]])
self._shapes.bbb = np.array([[-0.0914,-0.0406, 1],
[-0.0944,-0.0376, 1],
[-0.0944, 0.0376, 1],
[-0.0914, 0.0406, 1],
[-0.0429, 0.0406, 1],
[-0.0399, 0.0376, 1],
[-0.0399,-0.0376, 1],
[-0.0429,-0.0406, 1]])
self._shapes.bbb_rail_l = np.array([[-0.0429, -0.0356,1],
[-0.0429, 0.0233,1],
[-0.0479, 0.0233,1],
[-0.0479,-0.0356,1]])
self._shapes.bbb_rail_r = np.array([[-0.0914,-0.0356,1],
[-0.0914, 0.0233,1],
[-0.0864, 0.0233,1],
[-0.0864,-0.0356,1]])
self._shapes.bbb_eth = np.array([[-0.0579, 0.0436, 1],
[-0.0579, 0.0226, 1],
[-0.0739, 0.0226, 1],
[-0.0739, 0.0436, 1]])
self._shapes.left_wheel = np.array([[ 0.0254, 0.0595, 1],
[ 0.0254, 0.0335, 1],
[-0.0384, 0.0335, 1],
[-0.0384, 0.0595, 1]])
self._shapes.left_wheel_ol = np.array([[ 0.0254, 0.0595, 1],
[ 0.0254, 0.0335, 1],
[-0.0384, 0.0335, 1],
[-0.0384, 0.0595, 1]])
self._shapes.right_wheel_ol = np.array([[ 0.0254,-0.0595, 1],
[ 0.0254,-0.0335, 1],
[-0.0384,-0.0335, 1],
[-0.0384,-0.0595, 1]])
self._shapes.right_wheel = np.array([[ 0.0254,-0.0595, 1],
[ 0.0254,-0.0335, 1],
[-0.0384,-0.0335, 1],
[-0.0384,-0.0595, 1]])
self._shapes.ir_1 = np.array([[-0.0732, 0.0534, 1],
[-0.0732, 0.0634, 1],
[-0.0432, 0.0634, 1],
[-0.0432, 0.0534, 1]])
self._shapes.ir_2 = np.array([[ 0.0643, 0.0214, 1],
[ 0.0714, 0.0285, 1],
[ 0.0502, 0.0497, 1],
[ 0.0431, 0.0426, 1]])
self._shapes.ir_3 = np.array([[ 0.0636,-0.0042, 1],
[ 0.0636, 0.0258, 1],
[ 0.0736, 0.0258, 1],
[ 0.0736,-0.0042, 1]])
self._shapes.ir_4 = np.array([[ 0.0643,-0.0214, 1],
[ 0.0714,-0.0285, 1],
[ 0.0502,-0.0497, 1],
[ 0.0431,-0.0426, 1]])
self._shapes.ir_5 = np.array([[-0.0732,-0.0534, 1],
[-0.0732,-0.0634, 1],
[-0.0432,-0.0634, 1],
[-0.0432,-0.0534, 1]])
self._shapes.bbb_usb = np.array([[-0.0824,-0.0418, 1],
[-0.0694,-0.0418, 1],
[-0.0694,-0.0278, 1],
[-0.0824,-0.0278, 1]])
ir_sensor_poses = [
Pose(-0.0474, 0.0534, np.radians(90)),
Pose( 0.0613, 0.0244, np.radians(45)),
Pose( 0.0636, 0.0, np.radians(0)),
Pose( 0.0461,-0.0396, np.radians(-45)),
Pose(-0.0690,-0.0534, np.radians(-90))
]
self.info = Struct()
self.info.wheels = Struct()
self.info.wheels.radius = 0.0325
self.info.wheels.base_length = 0.09925
self.info.wheels.ticks_per_rev = 16
self.info.wheels.left_ticks = 0
self.info.wheels.right_ticks = 0
self.info.wheels.max_velocity = 2*pi*130/60 # 130 RPM
self.info.wheels.min_velocity = 2*pi*30/60 # 30 RPM
self.info.wheels.vel_left = 0
self.info.wheels.vel_right = 0
self.info.ir_sensors = Struct()
self.info.ir_sensors.poses = ir_sensor_poses
self.info.ir_sensors.rmax = 0.3
self.info.ir_sensors.rmin = 0.04
self.info.ir_sensors.readings = [133]*len(self.info.ir_sensors.poses)
self.walls = Cloud(0) # Black, no readings
# The constructor is called here because otherwise set_pose fails
RealBot.__init__(self, pose, color)
# Connect to bot...
# This code will raise an exception if not able to connect
if options is None:
self.log("No IP/port supplied to connect to the robot")
qb_comm.__init__(self, "localhost", "localhost", 5005)
else:
try:
qb_comm.__init__(self, options.baseIP, options.robotIP, options.port)
except AttributeError:
self.log("No IP/port supplied in the options to connect to the robot")
qb_comm.__init__(self, "localhost", "localhost", 5005)
self.ping() # Check if the robot is there
self.pause() # Initialize self.__paused
def set_pose(self,rpose):
RealBot.set_pose(self,rpose)
# We have to update walls here. WHY?
for pose, dst in zip(self.info.ir_sensors.poses,np.polyval(self.ir_coeff, self.info.ir_sensors.readings)):
if dst/self.info.ir_sensors.rmax <= 0.99:
self.walls.add_point(Pose(dst) >> pose >> self.get_pose())
def draw(self,r):
self.walls.draw(r)
r.set_pose(self.get_pose())
r.set_pen(0)
r.set_brush(0)
r.draw_polygon(self._shapes.ir_1)
r.draw_polygon(self._shapes.ir_2)
r.draw_polygon(self._shapes.ir_3)
r.draw_polygon(self._shapes.ir_4)
r.draw_polygon(self._shapes.ir_5)
r.draw_polygon(self._shapes.left_wheel)
r.draw_polygon(self._shapes.right_wheel)
r.set_pen(0x01000000)
r.set_brush(self.get_color())
r.draw_polygon(self._shapes.base_plate)
r.set_pen(0x10000000)
r.set_brush(None)
r.draw_polygon(self._shapes.left_wheel)
r.draw_polygon(self._shapes.right_wheel)
r.set_pen(None)
r.set_brush(0x333333)
r.draw_polygon(self._shapes.bbb)
r.set_brush(0)
r.draw_polygon(self._shapes.bbb_rail_l)
r.draw_polygon(self._shapes.bbb_rail_r)
r.set_brush(0xb2b2b2)
r.draw_polygon(self._shapes.bbb_eth)
r.draw_polygon(self._shapes.bbb_usb)
def get_envelope(self):
return self._shapes.base_plate
def draw_sensor(self,renderer,pose,dst):
renderer.push_state()
renderer.add_pose(pose)
phi = np.radians(3) # 6/2
rmin = self.info.ir_sensors.rmin
rmax = self.info.ir_sensors.rmax
if dst/rmax > 0.99:
renderer.set_brush(0x33FF5566)
else:
renderer.set_brush(0xCCFF5566)
renderer.draw_ellipse(0,0,min(1,rmin/2),min(1,rmin/2))
renderer.draw_polygon([(rmin*cos(phi),rmin*sin(phi)),
(dst*cos(phi),dst*sin(phi)),
(dst,0),
(dst*cos(phi),-dst*sin(phi)),
(rmin*cos(phi),-rmin*sin(phi))])
renderer.pop_state()
def draw_sensors(self,renderer):
"""Draw the sensors that this robot has"""
renderer.set_pose(self.get_pose())
distances = np.polyval(self.ir_coeff,self.info.ir_sensors.readings)
for pose, distance in zip(self.info.ir_sensors.poses, distances):
self.draw_sensor(renderer,pose,distance)
def get_info(self):
return self.info
def v2pwm(self,vl,vr):
"""Convert from angular velocity to PWM"""
pwm_l = sign(vl)*(abs(vl) - self.beta[1])/self.beta[0];
pwm_r = sign(vr)*(abs(vr) - self.beta[1])/self.beta[0];
return pwm_l, pwm_r
def pwm2v(self,pwm_l,pwm_r):
"""Convert from PWM to angular velocity"""
vl = sign(pwm_l)*(self.beta[0]*abs(pwm_l) + self.beta[1])
vr = sign(pwm_r)*(self.beta[0]*abs(pwm_r) + self.beta[1])
return vl, vr
def set_inputs(self,inputs):
pwm_l, pwm_r = self.v2pwm(*inputs)
with self.connect() as connection:
self.set_pwm(pwm_l, pwm_r, connection)
speeds = self.get_pwm(connection)
if speeds is None:
raise RuntimeError("Communication with QuickBot failed")
if speeds is not None:
self.info.wheels.vel_left, self.info.wheels.vel_right = self.pwm2v(*speeds)
def update_external_info(self):
"Communicate with the robot"
if self.__paused:
return
with self.connect() as connection:
ticks = self.get_encoder_ticks(connection)
if ticks is None:
raise RuntimeError("Communication with QuickBot failed")
self.info.wheels.left_ticks, self.info.wheels.right_ticks = ticks
irs = self.get_ir_raw_values(connection)
if irs is None:
raise RuntimeError("Communication with QuickBot failed")
self.info.ir_sensors.readings = irs
def reset(self):
self.__paused = True
self.info.wheels.vel_left, self.info.wheels.vel_right = 0,0
self.send_reset()
def pause(self):
self.__paused = True
self.send_halt()
def resume(self):
self.__paused = False
self.set_inputs((self.info.wheels.vel_left, self.info.wheels.vel_right))
