def __init__(self, robot):
# Set up robot.
self.robot = robot
# Set error.
self.epsilon = 1e-6
# Set up Usc.
try:
self.usc = Usc()
logger.info("Successfully attached to Maestro's low-level interface.")
except ConnectionError:
self.usc = Dummy()
logger.warn("Failed to attached to Maestro's low-level interface. "
"If not debugging, consider this a fatal error.")
# Set up virtual COM and TTL ports.
try:
self.maestro = Maestro()
logger.info("Successfully attached to Maestro's command port.")
except ConnectionError:
self.maestro = Dummy()
logger.warn("Failed to attached to Maestro's command port. "
"If not debugging, consider this a fatal error.")
# Emergency stop.
self.emergency = Event()
# Zero.
self.zero()
class Agility:
def __init__(self, robot):
# Set up robot.
self.robot = robot
# Set error.
self.epsilon = 1e-6
# Set up Usc.
try:
self.usc = Usc()
logger.info("Successfully attached to Maestro's low-level interface.")
except ConnectionError:
self.usc = Dummy()
logger.warn("Failed to attached to Maestro's low-level interface. "
"If not debugging, consider this a fatal error.")
# Set up virtual COM and TTL ports.
try:
self.maestro = Maestro()
logger.info("Successfully attached to Maestro's command port.")
except ConnectionError:
self.maestro = Dummy()
logger.warn("Failed to attached to Maestro's command port. "
"If not debugging, consider this a fatal error.")
# Emergency stop.
self.emergency = Event()
# Zero.
self.zero()
def stop(self):
"""
Emergency stop. Stop all wait functions.
"""
self.emergency.set()
def clear(self):
"""
Clear emergency flag.
"""
self.emergency.clear()
def head_rotation(self):
"""
Provides head rotation.
:return: Head rotation in degrees.
"""
servo = self.robot.head[0]
self.maestro.get_position(servo)
return servo.get_position()
def set_head(self, target, t=0):
"""
Move the head to a given position.
Blocks until completion.
:param target: (LR, UD).
:param t: Time in ms. 0 for max speed.
"""
head = self.robot.head
servos = self.robot.head_servos
head[0].set_target(target[0])
head[1].set_target(target[1])
self.maestro.end_together(servos, t, True)
self.wait(servos)
def look_at(self, x, y):
"""
Move the head to look at a given target.
Note that this is an approximation. Best used in a PID loop.
:param x: x-coordinate of target.
:param y: y-coordinate of target.
"""
head = self.robot.head
camera = head.camera
# Define velocity constant.
k = 1.5
# Compute deltas.
dx = (x - 0.5 * camera.width) * -1
dy = (y - 0.5 * camera.height) * -1
dt = dx / camera.width * (camera.fx / 2)
dp = dy / camera.height * (camera.fy / 2)
# Compute suggested velocity. Balance between blur and speed.
vt = int(round(abs(dt * k)))
vp = int(round(abs(dt * k)))
# Construct array.
data = [dt, vt, dp, vp]
# Perform motion.
self.move_head(data)
# Update target.
head.target = [x, y]
return data
def scan(self, t, direction=None, block=False):
"""
Scans head in a direction. If no direction is given, scans toward bound of last known location.
If at minimum of maximum bounds, automatically selects opposite direction.
Blocks until completely scanned towards one direction.
:param t: Time in milliseconds.
:param direction: A direction, either None, 1, or -1.
:param block: Whether to wait until completion.
"""
# Obtain definitions.
head = self.robot.head
camera = head.camera
servo = head.servos[0]
# Get bounds.
low, high = servo.get_range()
# Update servo.
self.maestro.get_position(servo)
# Check bound.
bound = head.at_bound()
# Create direction.
if bound != 0:
direction = bound * -1
if direction is None:
if head.target[0] < 0.5 * camera.width:
direction = 1
else:
direction = -1
# Execute.
if direction == 1:
servo.set_target(high)
else:
servo.set_target(low)
self.maestro.end_in(servo, t)
if block:
self.wait(servo)
def center_head(self, t=0):
"""
Returns head to original position.
:param t: The time in ms.
"""
# Obtain definitions.
head = self.robot.head
servos = head.servos
# Target zero.
for servo in servos:
servo.set_target(0)
# Reset to zero.
head.angles = [0, 0]
# Execute.
self.maestro.end_together(servos, t, True)
self.wait(servos)
def move_head(self, data):
"""
Move head based on data parameters. Does not wait for completion.
:param data: An array given by look_at.
"""
# Obtain definitions.
head = self.robot.head
servos = head.servos
# Update positions.
self.maestro.get_multiple_positions(servos)
for i in range(2):
servo = head[i]
current = servo.get_position()
# Get data.
delta = data[i * 2]
velocity = data[i * 2 + 1]
if velocity == 0:
# Already at target. Do nothing.
servo.target = servo.pwm
target = current
else:
# Ensure that head is within bounds.
low, high = servo.get_range()
target = current + delta
if target < low:
target = low
elif target > high:
target = high
servo.set_target(target)
# Update.
head.angles[i] = target
# Set speed.
self.maestro.set_speed(servo, velocity)
# Execute.
self.maestro.set_target(servo)
@staticmethod
def plot_gait(frames):
"""
Plot a gait given some frames. Used for debugging.
:param frames: Frames generated by execute.
"""
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_xlabel('X Axis')
ax.set_ylabel('Y Axis')
ax.set_zlabel('Z Axis')
x = frames[:, 0, 0]
y = frames[:, 0, 1]
z = frames[:, 0, 2]
ax.plot(x, y, z, marker='o')
plt.show()
def execute_forever(self, frames, dt):
"""
Like execute_frames(), except it runs forever.
:param frames: An array of frames.
:param dt: Delta t.
:return:
"""
# Get all legs and servos for quick access.
legs = self.robot.legs
servos = self.robot.leg_servos
# Update initial leg locations.
self.maestro.get_multiple_positions(servos)
for leg in legs:
leg.get_position()
while True:
for frame in frames:
for i in range(4):
legs[i].target_point(frame[i])
self.maestro.end_together(servos, dt)
self.wait(servos)
def execute_frames(self, frames, dt):
"""
Execute some frames with a constant dt.
:param frames: An array of frames.
:param dt: Delta t.
"""
# Get all legs and servos for quick access.
legs = self.robot.legs
servos = self.robot.leg_servos
# Update initial leg locations.
self.maestro.get_multiple_positions(servos)
for frame in frames:
for i in range(4):
legs[i].target_point(frame[i])
self.maestro.end_together(servos, dt)
self.wait(servos)
def execute_long(self, prev_frame, frames, dt):
"""
Execute frames with constant but possibly long dt.
Automatically computes distance, and, if necessary, interpolates to get more accurate synchronization.
:param prev_frame: The previous frame.
:param frames: An array of frames.
:param dt: Delta t.
"""
# Get all legs and servos for quick access.
legs = self.robot.legs
servos = self.robot.leg_servos
# Define break constant (ms / cm).
k = 100
# Update initial leg locations.
self.maestro.get_multiple_positions(servos)
for frame in frames:
# Compute max distance.
d = max(np.linalg.norm(frame - prev_frame, axis=1))
# Less than break. Too long. Linearly interpolate.
if dt / d > k:
n = int(round(dt / d / k)) + 1
l_frames = self.smooth(prev_frame, frame, n)
l_frames = l_frames[1:]
# Compute time.
t = dt / n
# Execute intermediate frames.
for l_frame in l_frames:
for i in range(4):
legs[i].target_point(l_frame[i])
self.maestro.end_together(servos, t)
self.wait(servos)
else:
t = dt
for i in range(4):
legs[i].target_point(frame[i])
self.maestro.end_together(servos, t)
self.wait(servos)
prev_frame = frame
def execute_variable(self, frames, dts):
"""
Execute some frames with different dt.
:param frames: An array of frames.
:param dts: An array of dt.
"""
# Get all legs and servos for quick access.
legs = self.robot.legs
servos = self.robot.leg_servos
# Update initial leg locations.
self.maestro.get_multiple_positions(servos)
# Assertion check.
assert len(frames) == len(dts)
for t in range(len(frames)):
for i in range(4):
legs[i].target_point(frames[t][i])
self.maestro.end_together(servos, dts[t])
self.wait(servos)
def execute_angles(self, angles, dt):
"""
Like execute_frames(), but uses angles instead.
:param angles: An array of angles.
:param dt: Delta t.
"""
# Get all legs and servos for quick access.
legs = self.robot.legs
servos = self.robot.leg_servos
# Update initial leg locations.
self.maestro.get_multiple_positions(servos)
for angle in angles:
for i in range(4):
legs[i].target_angle(angle)
self.maestro.end_together(servos, dt)
self.wait(servos)
def anglify(self, frames):
"""
Converts frames generated by self.prepare to angles.
:param frames: The input frames.
:return: The output angles ready for execution.
"""
# Get all legs and servos for quick access.
legs = self.robot.legs
# Allocate memory.
angles = np.empty(frames.shape)
for i in range(len(frames)):
for l in range(4):
a = legs[l].get_angles(frames[i][l])
angles[i][l] = a
return angles
@staticmethod
def smooth(a, b, n):
"""
Create a smooth transition from a to b in n steps.
:param a: The first array.
:param b: The second array.
:param n: The number of steps.
:return: An array from [a, b).
"""
assert(a.shape == b.shape)
assert(n > 1)
# Compute delta.
delta = (b - a) / n
# Allocate n-1 with dimension d+1.
shape = (n, *a.shape)
inter = np.empty(shape)
for i in range(n):
inter[i] = a + i * delta
return inter
def get_pose(self):
"""
Get the relative pose of the robot.
:return: A (4 x 3) matrix representing the current state of the robot.
"""
# Get all legs for quick access.
legs = self.robot.legs
# Iterate through all legs.
pose = []
for leg in legs:
position = leg.get_position()
pose.append(position)
return np.array(pose, dtype=float)
def target_point(self, leg, point, t):
"""
Move a leg to a given point in t time.
Blocks until completion.
:param leg: Leg index.
:param point: (x, y, z).
:param t: Time in milliseconds.
"""
# Assertion check.
assert(0 <= leg <= 3)
# Get legs for quick access.
legs = self.robot.legs
# Target.
leg = legs[leg]
leg.target_point(point)
# Execute.
servos = leg.servos
self.maestro.end_together(servos, t, True)
# Block until completion.
self.wait(servos)
def lift_leg(self, leg, lift, t):
"""
Lift a leg (change pose) in t time.
Blcoks until completion.
:param leg: The leg index.
:param lift: How high to lift leg.
:param t: Time to execute pose change.
"""
# Assertion check.
assert (0 <= leg <= 3)
# Get legs for quick access.
legs = self.robot.legs
# Define ground.
ground = -max([leg.length for leg in legs]) + 1
# Empty pose.
pose = np.zeros((4, 3))
# Leg lift.
pose[:, 2] = ground
pose[leg][2] = ground + lift
# Execute.
self.target_pose(pose, t)
def target_pose(self, target, t, lift=2):
"""
Get the robot from its current pose to a new pose. Block until completion.
The robot will lift legs appropriately to eliminate dragging.
Automatically adjusts the center of mass during transition and target if necessary.
:param target: The target pose.
:param t: The total time for the adjustment.
:param lift: How much to lift each leg.
:return: (frames, dt) ready for execution.
"""
# Get body for quick access.
body = self.robot.body
# Create data array.
frames = []
# Get pose. Assume updated.
pose = self.get_pose()
# Early exit.
if np.array_equal(pose, target):
return
# Get ground, which is the lowest point.
curr_g = np.min(pose[:, 2])
next_g = np.min(target[:, 2])
# Generate leg state arrays.
pose_state = np.greater(pose[:, 2], (curr_g + self.epsilon)) # Defines which legs are in the air.
target_state = np.greater(target[:, 2], (next_g + self.epsilon)) # Defines which legs are in the air.
# Get all legs to (0, 0, curr_g) if they are in the air.
if any(pose_state):
f1 = pose.copy()
for i in range(4):
if pose_state[i]:
f1[i] = (0, 0, curr_g)
frames.append(f1)
# Define optimization procedure.
def up_down(ground):
# For every leg that is not at the right (x, y) and is on the ground in target, lift and down.
for i in range(4):
if not np.array_equal(pose[i][:2], target[i][:2]) and not target_state[i]:
# Get previous frame.
prev = frames[-1]
f4, f5 = prev.copy(), prev.copy()
# Move leg to target (x, y) in air.
x, y = target[i][:2]
f4[i] = (x, y, ground + lift)
# Compute bias and adjust.
s = [False, False, False, False]
s[i] = True
bias = body.adjust(s, f4, 1)
f3 = prev - bias
f4 -= bias
# Move leg down to target. Keep bias.
f5[i] = target[i]
f5 -= bias
# Append data.
frames.extend((f3, f4, f5))
def to_next():
f2 = pose.copy()
f2[:, 2] = next_g
frames.append(f2)
# Different optimization order.
if next_g > curr_g:
# For body high -> low, get legs to next height first.
to_next()
up_down(next_g)
elif curr_g > next_g:
# For body low -> high, get legs to next height last.
up_down(curr_g)
to_next()
# Move to final target if necessary.
if not np.array_equal(frames[-1], target):
if any(target_state):
prev = frames[-1]
bias = body.adjust(target_state, target)
frames.extend((prev - bias, target - bias))
else:
frames.append(target)
# Compute times. Assume equal dt.
dt = t / len(frames)
self.execute_long(pose, frames, dt)
def prepare_frames(self, frames, dt, ground):
"""
Prepare some frames which are non-circular (last frame not linked to first frame).
:param frames: The input frames.
:param dt: dt.
:param ground: Ground.
:param loop: Whether the gait loops.
:return: (frames, dt) ready for execution.
"""
# Define body for quick access.
body = self.robot.body
# Create array for biases.
biases = np.empty(frames.shape)
# Generate leg state arrays.
state1 = np.greater(frames[:, :, 2], (ground + self.epsilon)) # Defines which legs are in the air.
state2 = state1.sum(1) # The number of legs in the air.
# Define.
steps = len(frames)
for t in range(steps - 1):
# Look ahead and pass data to center of mass adjustment algorithms.
next_frame = frames[t]
# Determine which legs are off.
off = state1[t]
count = state2[t]
# Perform center of mass adjustments accordingly.
biases[t] = body.adjust(off, next_frame, count)
# Adjust frames.
frames -= biases
return frames, dt
def prepare_gait(self, gait, debug=False):
"""
Prepare a given gait class.
:param gait: The gait class.
:param debug: Show gait in a graph.
:return: (frames, dt) ready for execution.
"""
# Define body for quick access.
body = self.robot.body
# Get gait properties.
steps = gait.steps
ground = gait.ground
dt = gait.time / steps
ts = np.linspace(0, 1000, num=steps, endpoint=False)
# Get all legs for quick access.
legs = self.robot.legs
# Compute shape.
shape = (steps, 4, 3)
# Evaluate gait.
f = [gait.evaluate(leg, ts) for leg in legs]
frames = np.concatenate(f).reshape(shape, order='F')
# Debugging.
if debug:
self.plot_gait(frames)
# Create array for biases.
biases = np.empty(shape)
# Generate leg state arrays.
state1 = np.greater(biases[:, :, 2], (ground + 1e-6)) # Defines which legs are in the air.
state2 = state1.sum(1) # The number of legs in the air.
# Iterate and perform static analysis.
for t in range(steps):
# Look ahead and pass data to center of mass adjustment algorithms.
next_frame = frames[(t + 1) % steps]
# Determine which legs are off.
off = state1[t]
count = state2[t]
# Perform center of mass adjustments accordingly.
biases[t] = body.adjust(off, next_frame, count)
# Adjust frames.
frames -= biases
return frames, dt
def prepare_smoothly(self, gait):
"""
Prepare a gait by intelligently applying smoothing. Only works for planar COM adjustments.
Plus, who doesn't like smooth things? (I'm really tired right now.)
:param gait: The gait object.
:return: (frames, dt) ready for execution.
"""
# Define body for quick access.
body = self.robot.body
# Get gait properties.
steps = gait.steps
ground = gait.ground
dt = gait.time / steps
ts = np.linspace(0, 1000, num=steps, endpoint=False)
# Get all legs for quick access.
legs = self.robot.legs
# Compute shape.
shape = (steps, 4, 3)
# Evaluate gait.
f = [gait.evaluate(leg, ts) for leg in legs]
frames = np.concatenate(f).reshape(shape, order='F')
# Generate leg state arrays.
state1 = np.greater(frames[:, :, 2], (ground + 1e-6)) # Defines which legs are in the air.
state2 = state1.sum(1) # The number of legs in the air.
# Get indices of legs in air.
air = np.where(state2 != 0)[0]
air = air.tolist()
# Create array for biases.
biases = np.empty(shape)
# Keep track of last air -> ground.
t = air[-1]
if state2[(t + 1) % steps] == 0:
# Last air frame is an air -> ground transition.
last_ag = t
else:
# There will
last_ag = None
# Compute biases for each frame that is not on the ground.
for i in range(len(air)):
# Get the index relative to all frames.
t = air[i]
# Compute bias as usual.
next_frame = frames[(t + 1) % steps]
off = state1[t]
count = state2[t]
biases[t] = body.adjust(off, next_frame, count)
# Checks if the current frame represents a ground -> air transition.
if state2[t - 1] == 0:
curr_bias = biases[t]
prev_bias = biases[last_ag]
# Smooth from [t, last_ag).
if t > last_ag:
n = t - last_ag
inter = self.smooth(prev_bias, curr_bias, n)
biases[last_ag:t] = inter
else:
n = steps - last_ag + t
inter = self.smooth(prev_bias, curr_bias, n)
biases[last_ag:] = inter[:(steps - last_ag)]
biases[:t] = inter[(steps - last_ag):]
# Check if the current frame represents an air -> ground transition.
if state2[(t + 1) % steps] == 0:
last_ag = t
# Adjust frames.
frames -= biases
return frames, dt
def move_body(self, x, y, z, t=0):
"""
Move the body some x, y, and z.
:param x: Move x.
:param y: Move y.
:param z: Move z.
:param t: The time in ms.
"""
legs = self.robot.legs
servos = self.robot.leg_servos
self.maestro.get_multiple_positions(servos)
for leg in legs:
a, b, c = leg.get_position()
a -= x
b -= y
c -= z
leg.target_point((-x, -y, -leg.length - z))
self.maestro.end_together(servos, t)
self.wait(servos)
def configure(self):
"""
Configure the Maestro by writing home positions and other configuration data to the device.
"""
settings = self.usc.getUscSettings()
settings.serialMode = uscSerialMode.SERIAL_MODE_USB_DUAL_PORT
for leg in self.robot.legs:
for servo in leg:
servo.zero()
channel = settings.channelSettings[servo.channel]
channel.mode = ChannelMode.Servo
channel.homeMode = HomeMode.Goto
channel.home = servo.target
channel.minimum = (servo.min_pwm // 64) * 64
channel.maximum = -(-servo.max_pwm // 64) * 64
for servo in self.robot.head:
servo.zero()
channel = settings.channelSettings[servo.channel]
channel.mode = ChannelMode.Servo
channel.homeMode = HomeMode.Goto
channel.home = servo.target
channel.minimum = (servo.min_pwm // 64) * 64
channel.maximum = -(-servo.max_pwm // 64) * 64
self.usc.setUscSettings(settings, False)
self.usc.reinitialize(500)
def go_home(self):
"""
Let the Maestro return all servos to home.
"""
self.maestro.go_home()
def ready(self, z, t=2000):
"""
Ready a gait by lower robot to plane.
:param z: Height of gait.
:param t: Time in milliseconds
"""
# Compute desired pose.
pose = np.zeros((4, 3))
pose[:, 2] = z
# Execute position.
self.target_pose(pose, t)
def zero(self):
"""
Manual return home by resetting all leg servo targets.
"""
# Get all legs and servos for quick access.
legs = self.robot.legs
s1 = self.robot.leg_servos
for leg in legs:
z = -leg.length
leg.target_point((0, 0, z))
# Execute.
self.set_head((0, 0), 1000)
self.maestro.end_together(s1, 1000, True)
# Wait until completion.
self.wait()
def wait(self, servos=None):
"""
Block until all servos have reached their targets.
:param servos: An array of servos. If None, checks if all servos have reached their targets (more efficient).
"""
while not self.is_at_target(servos=servos) and not self.emergency.is_set():
time.sleep(0.001)
def is_at_target(self, servos=None):
"""
Check if servos are at their target.
:param servos: One or more servo objects. If None, checks if all servos have reached their targets (more efficient).
:return: True if all servos are at their targets, False otherwise.
"""
if servos is None:
return not self.maestro.get_moving_state()
elif isinstance(servos, Servo):
self.maestro.get_position(servos)
if servos.at_target():
return True
return False
else:
self.maestro.get_multiple_positions(servos)
if all(servo.at_target() for servo in servos):
return True
return False
