def __init__(self, n_steps, step_length, step_height, step_time):
assert n_steps >= 2, "Must specify at least two steps"
self.n_steps = n_steps # number of steps to take
self.step_length = step_length # how far forward each step goes
self.step_height = step_height # maximum height of the swing foot
self.step_time = step_time # how long each swing phase lasts
self.n_phases = 2 * n_steps + 1 # how many different phases (double support, left support,
# right support, etc. there are throughout the whole motion.
self.total_time = self.step_time * self.n_phases
# initial CoM and foot positions
self.x_com_init = np.asarray([[0.0], [0.0], [1.04]])
self.xd_com_init = np.asarray([[0.0], [0.0], [0.0]])
self.fc_offset = -0.065 # The foot frame is this far from the foot's center
self.x_right_init = np.asarray([[self.fc_offset], [-0.138], [0.1]])
self.x_left_init = np.asarray([[self.fc_offset], [0.138], [0.1]])
self.foot_w1 = 0.08 # width left of foot frame
self.foot_w2 = 0.08 # width right of foot frame
self.foot_l1 = 0.2 # length in front of foot
self.foot_l2 = 0.07 # length behind foot
# LIP parameters
self.h = self.x_com_init[2]
self.g = 9.81
# Generate foot ground contact placements for both feet
self.right_foot_placements = [self.x_right_init]
self.left_foot_placements = [self.x_left_init]
for step in range(self.n_steps):
if (step == 0) or (step == self.n_steps - 1):
# This is the first or last step, so shorten the stride
l = self.step_length / 2
else:
l = self.step_length
if step % 2 == 0:
# Step with the right foot
x_right = pycopy(self.right_foot_placements[-1])
x_right[0, 0] += l
self.right_foot_placements.append(x_right)
else:
# Step with the left foot
x_left = pycopy(self.left_foot_placements[-1])
x_left[0, 0] += l
self.left_foot_placements.append(x_left)
# Generate ZMP trajectory as function of time
self._generate_zmp_trajectory()
# Generate CoM trajectory as function of time using the LIP
self._generate_com_trajectory()
# Generate foot trajectories as functions of time.
self._generate_foot_trajectories()
def RightFootTrajectory(self, time):
"""
Specify a desired position and velocity for the right foot
"""
x_right = pycopy(self.x_right_init)
xd_right = np.array([[0.0], [0.0], [0.0]])
return (pycopy(x_right), pycopy(xd_right))
def get_raw_array(self):
namelength = self.read_element()[0]
names = self.read_element()
field_names = [names[i:i+namelength].tostring().strip('\x00')
for i in xrange(0,len(names),namelength)]
tupdims = tuple(self.header['dims'][::-1])
length = np.product(tupdims)
if self.struct_as_record:
if not len(field_names):
# If there are no field names, there is no dtype
# representation we can use, falling back to empty
# object
return np.empty(tupdims, dtype=object).T
dtype = [(field_name, object) for field_name in field_names]
result = np.empty(length, dtype=dtype)
for i in range(length):
for field_name in field_names:
result[i][field_name] = self.read_element()
else: # Backward compatibility with previous format
self.obj_template = mat_struct()
self.obj_template._fieldnames = field_names
result = np.empty(length, dtype=object)
for i in range(length):
item = pycopy(self.obj_template)
for name in field_names:
item.__dict__[name] = self.read_element()
result[i] = item
return result.reshape(tupdims).T
def LeftFootTrajectory(self, time):
"""
Specify a desired position and velocity for the left foot
"""
x_left = pycopy(self.x_left_init)
xd_left = np.array([[0.0], [0.0], [0.0]])
return (x_left, xd_left)
def copy (self): """ Copy a channel using `copy.copy` @returns: The copied channel """ return pycopy(self)
def expand (self, col = 0, pattern = "*"): """ expand the channel according to the files in <col>, other cols will keep the same `[(dir1/dir2, 1)].expand (0, "*")` will expand to `[(dir1/dir2/file1, 1), (dir1/dir2/file2, 1), ...]` length: 1 -> N width: M -> M @params: `col`: the index of the column used to expand `pattern`: use a pattern to filter the files/dirs, default: `*` @returns: The expanded channel Note, self is also changed """ from glob import glob import os folder = self[0][col] files = glob (os.path.join(folder, pattern)) tmp = list (self[0]) for i, f in enumerate(files): n = pycopy(tmp) n [col] = f if i == 0: self[i] = tuple(n) else: self.append (tuple(n)) return self
def get_raw_array(self):
namelength = self.read_element()[0]
names = self.read_element()
field_names = [
names[i:i + namelength].tostring().strip('\x00')
for i in xrange(0, len(names), namelength)
]
tupdims = tuple(self.header['dims'][::-1])
length = np.product(tupdims)
if self.struct_as_record:
if not len(field_names):
# If there are no field names, there is no dtype
# representation we can use, falling back to empty
# object
return np.empty(tupdims, dtype=object).T
dtype = [(field_name, object) for field_name in field_names]
result = np.empty(length, dtype=dtype)
for i in range(length):
for field_name in field_names:
result[i][field_name] = self.read_element()
else: # Backward compatibility with previous format
self.obj_template = mat_struct()
self.obj_template._fieldnames = field_names
result = np.empty(length, dtype=object)
for i in range(length):
item = pycopy(self.obj_template)
for name in field_names:
item.__dict__[name] = self.read_element()
result[i] = item
return result.reshape(tupdims).T
def ComTrajectory(self, time):
"""
Return a desired center of mass position and velocity for the given timestep
"""
x_com = pycopy(self.x_com_init)
xd_com = np.array([[0.0], [0.0], [0.0]])
xdd_com = np.array([[0.0], [0.0], [0.0]])
return (x_com, xd_com, xdd_com)
def goStraight(vehicle, nextNode, accumCost, route, Q, d):
withoutDepotCost = float("inf")
vehicleCopy = pycopy(vehicle)
distanceToNextNode = distCustomers(vehicleCopy.currPos, nextNode)
#Updating Accum Cost to reaching the new node
accumCost = accumCost + distanceToNextNode
#Advancing the vehicle to reach client
vehicleCopy.currPos = nextNode
#Serve client
withoutDepotCost = analyzeExpectedCostForAllDemand(accumCost, vehicleCopy, route, Q, d)
return withoutDepotCost
def satisfyDemand(demand, vehicle, accumCost, route, Q, d):
vehicleCopy = pycopy(vehicle)
distanceToDepot = distCustomers(vehicleCopy.currPos, route[-1])
if vehicleCopy.currQ < demand:
#Failure, we go to the depot and back
vehicleCopy.currQ = vehicleCopy.currQ - demand + Q
accumCost = accumCost + 2 * distanceToDepot
else:
#update the Q of the vehicle
vehicleCopy.currQ = vehicleCopy.currQ - demand
return expLenCost(accumCost, vehicleCopy, route, Q, d)
def goToDepot(vehicle, nextNode, accumCost, route, Q, d):
withDepotCost = float("inf")
vehicleCopy = pycopy(vehicle)
distanceToDepot = distCustomers(vehicle.currPos, route[-1])
distanceFromDepotToNext = distCustomers(nextNode, route[-1])
#Don't go to the depot if Im there or coming from there, or going there next
if not (vehicle.currQ == Q or nextNode.ID == 0):
#If we go to the depot, the cost needs to include the trip
accumCost = accumCost + distanceToDepot + distanceFromDepotToNext
#Load up the truck again
vehicleCopy.currQ = Q
#Advancing the vehicle to the next node
vehicleCopy.currPos = nextNode
#Analyze demand with the vehicle in the next node
withDepotCost = analyzeExpectedCostForAllDemand(accumCost, vehicleCopy, route, Q, d)
return withDepotCost
def expLen(accumCost, vehicleGiven, routeGiven, Q, d):
route = routeGiven[:]
vehicle = pycopy(vehicleGiven)
if len(route) == 0:
return accumCost
nextNode = route[0]
#If the vehicle is already at the next node, just advance the route
if vehicle.currPos.ID == nextNode.ID:
#advance the algo
route.pop(0)
return expLen(accumCost, vehicle, route, Q, d)
#If the vehicle is in the depot, load it up
if vehicle.currPos.ID == 0:
vehicle.currQ = Q
withDepotCost = goToDepot(vehicle, nextNode, accumCost, route, Q, d)
withoutDepotCost = goStraight(vehicle, nextNode, accumCost, route, Q, d)
return (withDepotCost, withoutDepotCost)
def get_item(self):
item = pycopy(self.obj_template)
for element in item._fieldnames:
item.__dict__[element] = self.read_element()
return item
def get_item(self):
item = pycopy(self.obj_template)
for element in item._fieldnames:
item.__dict__[element] = self.read_element()
return item
def emit(self, record):
"""
Emit the record.
"""
from .jobmgr import Jobmgr
try:
pbar = record.pbar if hasattr(record, 'pbar') else None
if pbar == 'next':
if self.prevbar:
self.stream.write("\n")
self._emit(record, "\n")
elif pbar is None:
# break pbars
if not "\n" in record.msg:
self._emit(record, "\n")
else:
msgs = record.msg.splitlines()
for i, m in enumerate(msgs):
rec = pycopy(record)
rec.msg = m
if i == len(msgs) - 1 and m.startswith('...... max='):
delattr(rec, 'jobidx')
self._emit(rec, "\n")
self.prevbar = None
elif pbar is True:
# pbar, replace previous pbar
self.prevbar = record
self._emit(record, "\r")
elif not self.prevbar:
# not pbar and not prev pbar
justlen = Jobmgr.PBAR_SIZE + 32
if hasattr(record, 'proc'):
justlen += len(record.proc) + 2
if hasattr(record, 'jobidx'):
justlen += len(str(record.joblen)) * 3
justlen = max(justlen, Jobmgr.PBAR_SIZE + 32)
if not "\n" in record.msg:
record.msg = record.msg.ljust(justlen)
self._emit(record, "\n")
else:
msgs = record.msg.splitlines()
for i, m in enumerate(msgs):
rec = pycopy(record)
if i == len(msgs) - 1 and m.startswith('...... max='):
rec.msg = m.ljust(justlen)
delattr(rec, 'jobidx')
else:
rec.msg = m.ljust(justlen)
self._emit(rec, "\n")
else:
# not pbar but prev pbar
justlen = Jobmgr.PBAR_SIZE + 32
if hasattr(self.prevbar, 'proc'):
justlen += len(self.prevbar.proc) + 2
if hasattr(self.prevbar, 'jobidx'):
justlen += len(str(self.prevbar.joblen)) * 3
justlen = max(justlen, Jobmgr.PBAR_SIZE + 32)
if not "\n" in record.msg:
record.msg = record.msg.ljust(justlen)
self._emit(record, "\n")
else:
msgs = record.msg.splitlines()
for i, m in enumerate(msgs):
rec = pycopy(record)
if i == len(msgs) - 1 and m.startswith('...... max='):
rec.msg = m.ljust(justlen)
delattr(rec, 'jobidx')
else:
rec.msg = m.ljust(justlen)
self._emit(rec, "\n")
self._emit(self.prevbar, "\r")
except (KeyboardInterrupt, SystemExit, IOError,
EOFError): # pragma: no cover
raise
except Exception: # pragma: no cover
self.handleError(record)
def _generate_foot_trajectories(self):
"""
Generate a desired (x,y,z) trajectory for each foot as a piecewise polynomial.
Once this method has been called, the foot positions at a given time t can be
accessed like
left_foot_position = self.left_foot_trajectory.value(t).
"""
# Specify break points for piecewise linear interpolation
break_times = np.asarray([[0.5 * i * self.step_time]
for i in range(2 * self.n_phases + 1)])
# Knot points are positions of the right and left feet at the break times
lf_knots = np.zeros((3, len(break_times)))
rf_knots = np.zeros((3, len(break_times)))
lf_dot_knots = np.zeros((3, len(break_times)))
rf_dot_knots = np.zeros((3, len(break_times)))
lf_idx = 0
rf_idx = 0
for i in range(len(break_times)):
t = break_times[i, 0]
phase_time = (t / self.step_time) % 4
if 1 < phase_time and phase_time < 2:
# Right foot is in the middle of its swing phase
rf_ref = pycopy(self.right_foot_placements[rf_idx])
rf_ref[
0] += self.step_length / 2 # move the foot forward to the midpoint of the stride
rf_ref[2] += self.step_height # and up the designated hight
# Slight forward velocity of the swing foot
rf_dot_knots[0, i] = self.step_length / self.step_time
# Next setpoint will be at the next foot placement
rf_idx += 1
# left foot remains at the same place
lf_ref = self.left_foot_placements[lf_idx]
elif 3 < phase_time and phase_time < 4:
# Left foot is in the middle of its swing phase
lf_ref = pycopy(self.left_foot_placements[lf_idx])
lf_ref[
0] += self.step_length / 2 # move the foot forward to the midpoint of the stride
lf_ref[2] += self.step_height # and up the designated hight
# Next setpoint will be at the next foot placement
lf_idx += 1
# Slight forward velocity of the swing foot
lf_dot_knots[0, i] = self.step_length / self.step_time
# right foot remains at the same place
rf_ref = self.right_foot_placements[rf_idx]
else:
# We're in double support: both feet stay at the same position
lf_ref = self.left_foot_placements[lf_idx]
rf_ref = self.right_foot_placements[rf_idx]
lf_knots[:, i] = lf_ref[:, 0]
rf_knots[:, i] = rf_ref[:, 0]
# Perform piecewise linear interpolation
#self.left_foot_trajectory = PiecewisePolynomial.FirstOrderHold(break_times,lf_knots)
#self.right_foot_trajectory = PiecewisePolynomial.FirstOrderHold(break_times,rf_knots)
self.left_foot_trajectory = PiecewisePolynomial.Cubic(
break_times, lf_knots, lf_dot_knots)
self.right_foot_trajectory = PiecewisePolynomial.Cubic(
break_times, rf_knots, rf_dot_knots)
