class CTSLIP_xst(csync.FlexSOD):
"Extended state -- includes auxvars"
Fields = (['t'] + csync.getFields(SRC, "CTSLIP_state", 2) +
csync.getFields(SRC, "CTSLIP_aux", 2))
assert len(Fields) == FASTODE_CTSLIP.WIDTH
def upd_dbg(self, name, val):
print "UPD called"
def cfgFromParam(self, pars):
self.vis_R = pars.len0 / 5
self.vis_W = self.vis_R * 0.75
self.vis_ofs = pars.stOfs
if abs(pars.stHalfSweep - pars.stHalfDuty) > 0.02:
self.vis_swp = pars.stHalfSweep
else:
self.vis_swp = 0
self.domain = int(pars.domain)
def plot(self):
X = self.com_x
Z = self.com_z + self.zOfs
l = plotCircle(X, Z, self.vis_R, 36, color='k', linewidth=2)
if self.vis_swp:
l.append(
plotCircle(X,
Z,
self.vis_R,
18,
arc=(self.vis_ofs - self.vis_swp,
self.vis_ofs + self.vis_swp),
color=[0.6, 0.9, 0.6],
linewidth=5))
l.append(
plot([X, X + self.vis_R * 1.6 * cos(self.clk)],
[Z, Z + self.vis_R * 1.6 * sin(self.clk)],
color='m',
linewidth=2))
for leg in xrange(2):
lx = getattr(self, "leg_x_%d" % leg)
lz = getattr(self, "leg_z_%d" % leg)
rx = getattr(self, "ref_x_%d" % leg)
rz = getattr(self, "ref_z_%d" % leg)
lc = 'brgmck'[leg]
l.append(plot([X + rx], [Z + rz], 'd' + lc))
lkw = {'color': lc, 'linewidth': 2}
if self.domain & (1 << leg):
l.extend(
plotZigZag(X + lx * 0.2, Z + lz * 0.2, X + lx * 0.8, Z +
lz * 0.8, self.vis_W, 7, **lkw) +
plot([X, X + lx * 0.2], [Z, Z + lz * 0.2], **lkw) +
plot([X + lx * 0.8, X + lx], [Z + lz * 0.8, Z +
lz], **lkw) +
plot([X + lx], [Z + lz], '^' + lc))
else:
l.extend(
plot([X, X + lx], [Z, Z + lz], '-', **lkw)
#+plot( [X+lx], [Z+lz], 'o'+lc )
)
return l
class CTSLIP_param(csync.FlexSOD):
Fields = csync.getFields(SRC, "CTSLIP_param", 2)
assert len(Fields) == FASTODE_CTSLIP.NPAR
def upd20_stWn(self, nm, val):
self.stK = val * val
self.stMu = self.stZeta * 2 * val
print "[upd] set stK, stMu from stWn, stZeta"
def upd09_hexed(self, nm, val):
tc, dc, phs, ph0, Kp, Kd = val
# Omega is 2*pi/(time-of-cycle), but direction is negative!
self.omega = -2 * pi / tc
# Our duty cycle is in radians
self.stHalfDuty = dc * pi
# Sweep angle is the same
self.stHalfSweep = phs / 2.0
# Zero angle is "down" for hexed data
self.stOfs = ph0 - pi / 2
# Proportional FB is just like the torsional stiffness
# NOTE: for small changes; we use sin(delta) in the eqn
self.tqKth = Kp
# Given warning if a nonzero Kd is requested
if Kd != 0:
print "[upd] WARNING: Kd=%g requested by Kd not supported" % Kd
print "[upd] applied hextuple ", repr(val)
def upd10_stDecayFrac(self, nm, val):
# arc subtended by decay time
ang = val * self.stHalfDuty
# Natural frequency must finish arc in time
self.stWn = abs(self.omega) * (2 * pi / ang)
print "[upd] set stWn from stDecayFrac"
def upd90_stHalfDuty(self, nm, val):
if val < 0 or val > pi:
raise ValueError, "Half duty cycle range is 0..PI"
if val == 0 or val == pi:
self.stHalfDuty += 1e-99
print "[upd] fixed invalid stHalfDuty -- don't use 0 and PI!"
def copy(self):
return COPY(self)
class CTSLIP_state(csync.FlexSOD):
Fields = ['t'] + csync.getFields(SRC, "CTSLIP_state", 2)
assert len(Fields) == FASTODE_CTSLIP.DIM + 1
def upd50_par(self, nm, val):
if not hasattr(val, 'clkTD') or val.clkTD is None:
return
if val.gravityZ >= 0:
return
# Estimate time 'till touchdown
t2td = sqrt(2 * self.com_z / -val.gravityZ)
# Create clk IC that cancels phase change while falling
self.clk = val.clkTD - t2td * val.omega
print "[upd] fixed initial clk for clkTD"
def upd90_mdl(self, nm, val):
x = CTSLIP_aux()
y0 = numpy.concatenate((self[:], zeros(len(x))))
val.computeAux(y0)
x.fromArray(y0[len(self):])
# If leg 0 is on COM --> move to ref
if self.leg_z_0 == 0 and self.leg_x_0 == 0:
self.leg_x_0 = x.ref_x_0
self.leg_z_0 = x.ref_z_0
print "[upd] Locked initial leg 0 position to reference"
# if a penetrating stance --> fix it
if self.com_z + self.leg_z_0 < 0:
self.leg_x_0 *= abs(self.com_z) / abs(self.leg_z_0)
self.leg_z_0 = -self.com_z
print "[upd] Initial leg 0 moved to Z=0"
# If leg 1 is on COM --> move to ref
if self.leg_z_1 == 0 and self.leg_x_1 == 0:
self.leg_x_1 = x.ref_x_1
self.leg_z_1 = x.ref_z_1
print "[upd] Locked initial leg 1 position to reference"
# if a penetrating stance --> fix it
if self.com_z + self.leg_z_0 < 1:
self.leg_x_1 *= abs(self.com_z) / abs(self.leg_z_1)
self.leg_z_1 = -self.com_z
print "[upd] Initial leg 1 moved to Z=0"
return
def upd50_plane(self, nm, coef):
"""
dot([1,state],coef)-val is the event function
coef -- len<5 -- (val, co_clk, co_vx, co_z, co_vz, co_x), zeros appended
"""
coef = array((list(coef) + [0] * 6)[:6], float)
if not any(coef):
coef[0] = 1000
print "[upd] plane event disabled"
else:
p = self.par
(p.co_value, p.co_clk, p.co_com_vx, p.co_com_z, p.co_com_vz,
p.co_com_x) = coef
print "[upd] plane is ", coef
def getPlane(self):
p = self.par
return array(
[p.co_clk, p.co_com_vx, p.co_com_z, p.co_com_vz, p.co_com_x])
def planeVal(self, dat):
p = self.par
co = self.getPlane()
dat = asarray(dat)
if dat.shape[-1] != len(co):
s = CTSLIP_xst()
dat = dat[..., [s.clk, s.com_vx, s.com_z, s.com_vz, s.com_x]]
return dot(dat, co) + p.co_value
def copy(self):
res = COPY(self)
if hasattr(res, 'par'):
res.par = res.par.copy()
return res
class CTSLIP_events(csync.FlexSOD):
Fields = csync.getFields(SRC, "CTSLIP_events", 2)
class CTSLIP_aux(csync.FlexSOD):
Fields = csync.getFields(SRC, "CTSLIP_aux", 2)
assert len(Fields) == FASTODE_CTSLIP.AUX