def IP_step(IC, pars, t0 = 0):
"""
performs a SLIP step
:args:
IC (1x2): [phi, phi_dot]
pars (tuple): SLIP parameter: alpha, l0, gamma,
:returns:
t, y, FS: step time, step state, and final state (after impact in new
coordinate system)
"""
# calculate stance:
o = ode.odeDP5(dy_stance, pars=pars)
o.ODE_RTOL = 1e-6
o.ODE_ATOL = 1e-6
o.event = td_event
t, y = o(IC, t0, t0 + 3)
tt, yy = o.refine(lambda tf,yf: td_event_refine(tf, yf, pars))
# calculate impact! -> angular momentum (="tangential velocity") is
# conserved. This "tangential velocity" is the cosine of the between-leg
# angle alpha
vphi_plus = np.cos(pars[0]) * yy[1]
phi_plus = yy[0] + pars[0]
return (t,y,array([phi_plus, vphi_plus]))
def IP_step(IC, pars, t0=0):
"""
performs a SLIP step
:args:
IC (1x2): [phi, phi_dot]
pars (tuple): SLIP parameter: alpha, l0, gamma,
:returns:
t, y, FS: step time, step state, and final state (after impact in new
coordinate system)
"""
# calculate stance:
o = ode.odeDP5(dy_stance, pars=pars)
o.ODE_RTOL = 1e-6
o.ODE_ATOL = 1e-6
o.event = td_event
t, y = o(IC, t0, t0 + 3)
tt, yy = o.refine(lambda tf, yf: td_event_refine(tf, yf, pars))
# calculate impact! -> angular momentum (="tangential velocity") is
# conserved. This "tangential velocity" is the cosine of the between-leg
# angle alpha
vphi_plus = np.cos(pars[0]) * yy[1]
phi_plus = yy[0] + pars[0]
return (t, y, array([phi_plus, vphi_plus]))
def restore(self, filename):
"""
update the restore procedure: re-initialize the ODE solver!
:args:
filename (str): the filename where the model information is stored
"""
super(BSLIP, self).restore(filename)
self.ode = integro.odeDP5(self.dy_Stance, pars=self.params)
self.ode.ODE_RTOL = 1e-9
def restore(self, filename):
"""
update the restore procedure: re-initialize the ODE solver!
:args:
filename (str): the filename where the model information is stored
"""
super(BSLIP, self).restore(filename)
self.ode = integro.odeDP5(self.dy_Stance, pars=self.params)
self.ode.ODE_RTOL = 1e-9
def SLIP_step(IC, pars):
"""
performs a SLIP step
:args:
IC (1x3): the SLIP initial conditions (at apex): x, y, vx
pars (tuple): SLIP parameter: k, alpha, l0, m,
:returns:
FS (1x3): the final apex state or None if unsuccessful
"""
# first: calculate touchdown point
k, alpha, l0, m = pars
y_TD = l0 * np.sin(alpha)
y_fall = IC[1] - y_TD
if y_fall < 0:
return None
t_TD = np.sqrt(2. * y_fall / 9.81)
x_TD = IC[0] + t_TD * IC[2]
x_foot = x_TD + l0 * np.cos(alpha)
vy_TD = - t_TD * 9.81
p = list(pars) + [x_foot]
# calculate stance:
o = ode.odeDP5(dy_stance, pars=p)
o.ODE_RTOL = 1e-6
o.ODE_ATOL = 1e-6
o.event = to_event
t, y = o([x_TD, IC[2], y_TD, vy_TD], 0, 2)
tt, yy = o.refine(lambda tf,yf: to_event_refine(tf, yf, p))
# calcualte next apex
if yy[3] < 0:
# liftoff velocity negative. NOTE: technically, another step could
# occur, but here we look only for steps with apex during flight
return None
t_Apex = yy[3] / 9.81
y_Apex = yy[2] + .5 * 9.81 * t_Apex**2
x_Apex = yy[0] + yy[1] * t_Apex
vx_Apex = yy[1]
return array([x_Apex, y_Apex, vx_Apex])
def __init__(self, params=None, IC=None):
"""
The BSLIP is a bipedal walking SLIP model.
params (mutils.misc.Struct): parameter of the model
IC (array): initial conditions. [x, y, z, vx, vy, vz]
*NOTE* the system starts in single stance and *must* have
positive vertical velocity ("vy > 0")
"""
super(BSLIP, self).__init__()
self.params = deepcopy(params)
self.state = deepcopy(IC)
self.odd_step = True # leg 1 or leg two on ground?
self.dt = .01
self.ode = integro.odeDP5(self.dy_Stance, pars=self.params)
self.ode.ODE_RTOL = 1e-9
self.t = 0
self.t_td = 0
self.t_to = 0
self.singleStance = True
self.failed = False
self.skip_forces = False
self.ErrMsg = ""
# storage for ode solutions
self.feet1_seq = []
self.feet2_seq = []
self.t_ss_seq = []
self.t_ds_seq = []
self.y_ss_seq = []
self.y_ds_seq = []
self.forces_ss_seq = []
self.forces_ds_seq = []
self.DEBUG = False
if self.params is not None:
self.feet1_seq.append(self.params['foot1'])
self.feet2_seq.append(self.params['foot2'])
t (tuple of floats): absolute time before and after step
y (2xd): array, containing the "before-step" and "after-step" system state
traj (?): the trajectory event. Not sure this is really passed through
:returns:
True if an event has been detected, False otherwise
"""
#print pars
return t[1] > 1.935
if __name__=='__main__':
# TODO: implement event!
# define integrator object
o = ode.odeDP5(ydot, pars=-1.)
# define a stop event
o.event = efun
# run test integration
# just call the object: (y0, t0, t_end)
t, y = o([0, 1],0,5)
from pylab import figure, show, plot
fig = figure(1)
fig.clf()
plot(t,y,'.-')
show()
def init_ode(self):
""" re-initialize the ODE solver """
self.ode = integro.odeDP5(self.dy_Stance, pars=self.params)
self.ode.ODE_RTOL = 1e-9
def VPP_step(IC, pars, return_traj=True, with_forces=False):
"""
performs a walking step of the VPP model
:args:
IC : the initial conditions (at VLO): x, y, vx
pars (dict): model parameter: k, alpha, l0, m,
traj (bool): return trajectory (t,y) or only final state
with_forces (bool): also, return the forces and torques
:returns:
t, y, (x_foot1, x_foot2, t_td, t_to): if return_traj == True,
t, y, ( -"-), [Forces]: if additionally with_forces == True
FS (1x3): the final apex state or None if unsuccessful
([Forces] = [F1x, F1y, F2x, F2y, tau1, tau2])
"""
# Forces is a vector with [Fx
Forces = []
debug = False
P = deepcopy(pars)
# first phase: single support until touchdown of 2nd leg
o = ode.odeDP5(dy, pars=P)
o.ODE_RTOL = 1e-9
o.ODE_ATOL = 1e-9
o.event = td_event_spring
t, y = o(IC, 0, 2)
tt, yy = o.refine(lambda tf, yf: td_event_spring_refine(tf, yf, P))
# compute forces?
if with_forces:
f_pt = [
hstack(dy(ttt, yyy, P, output_forces=True))
for ttt, yyy in zip(t, y)
]
Forces.append(vstack(f_pt))
# transition: 1st leg -> 2nd leg; initialize new 1st leg (xfoot)
t_td = tt
tmpf, tmpp = deepcopy((P['legfun2'], P['legpars2']))
P['legfun2'] = P['legfun1']
P['legpars2'] = P['legpars1']
P['legfun1'] = tmpf
P['legpars1'] = tmpp
P['x_foot2'] = P['x_foot1']
hipx = yy[0] + P['r_hip'] * np.sin(yy[2])
P['x_foot1'] = hipx + P['legpars1']['l0'] * np.cos(P['legpars1']['alpha'])
# second phase: double support until takeoff of 1st leg
o = ode.odeDP5(dy, pars=P)
o.ODE_RTOL = 1e-9
o.ODE_ATOL = 1e-9
o.event = to_event_spring
tnew, ynew = o(yy, tt, tt + 2)
tt, yy = o.refine(lambda tf, yf: to_event_spring_refine(tf, yf, P))
t, y = np.hstack([t, tnew]), np.vstack([y, ynew])
if with_forces:
f_pt = [
hstack(dy(ttt, yyy, P, output_forces=True))
for ttt, yyy in zip(tnew, ynew)
]
# the last indexing switches leg 1 and 2
Forces.append(vstack(f_pt)[:, [2, 3, 0, 1, 5, 4]])
t_to = tt
# third phase: single support of leg1
P['x_foot2'] = None
o = ode.odeDP5(dy, pars=P)
o.ODE_RTOL = 1e-9
o.ODE_ATOL = 1e-9
o.event = VLO_event
tnew, ynew = o(yy, tt, tt + 2)
tt, yy = o.refine(lambda tf, yf: VLO_event_refine(tf, yf, P))
t, y = np.hstack([t, tnew]), np.vstack([y, ynew])
if with_forces:
f_pt = [
hstack(dy(ttt, yyy, P, output_forces=True))
for ttt, yyy in zip(tnew, ynew)
]
# the last indexing switches leg 1 and 2
Forces.append(vstack(f_pt)[:, [2, 3, 0, 1, 5, 4]])
if return_traj:
if with_forces:
return (t, y, (P['x_foot1'], P['x_foot2'], t_td, t_to),
vstack(Forces))
return t, y, (P['x_foot1'], P['x_foot2'], t_td, t_to)
return y[-1, :]
def init_ode(self):
""" re-initialize the ODE solver """
self.ode = integro.odeDP5(self.dy_Stance, pars=self.params)
self.ode.ODE_RTOL = 1e-9
def VPP_step(IC, pars, return_traj=True, with_forces=False):
"""
performs a walking step of the VPP model
:args:
IC : the initial conditions (at VLO): x, y, vx
pars (dict): model parameter: k, alpha, l0, m,
traj (bool): return trajectory (t,y) or only final state
with_forces (bool): also, return the forces and torques
:returns:
t, y, (x_foot1, x_foot2, t_td, t_to): if return_traj == True,
t, y, ( -"-), [Forces]: if additionally with_forces == True
FS (1x3): the final apex state or None if unsuccessful
([Forces] = [F1x, F1y, F2x, F2y, tau1, tau2])
"""
# Forces is a vector with [Fx
Forces = []
debug = False
P = deepcopy(pars)
# first phase: single support until touchdown of 2nd leg
o = ode.odeDP5(dy, pars=P)
o.ODE_RTOL = 1e-9
o.ODE_ATOL = 1e-9
o.event = td_event_spring
t, y = o(IC, 0, 2)
tt, yy = o.refine(lambda tf,yf: td_event_spring_refine (tf, yf, P))
# compute forces?
if with_forces:
f_pt = [hstack(dy(ttt, yyy, P, output_forces=True))
for ttt, yyy in zip(t, y)]
Forces.append(vstack(f_pt))
# transition: 1st leg -> 2nd leg; initialize new 1st leg (xfoot)
t_td = tt
tmpf, tmpp = deepcopy((P['legfun2'], P['legpars2']))
P['legfun2'] = P['legfun1']
P['legpars2'] = P['legpars1']
P['legfun1'] = tmpf
P['legpars1'] = tmpp
P['x_foot2'] = P['x_foot1']
hipx = yy[0] + P['r_hip'] * np.sin(yy[2])
P['x_foot1'] = hipx + P['legpars1']['l0'] * np.cos(P['legpars1']['alpha'])
# second phase: double support until takeoff of 1st leg
o = ode.odeDP5(dy, pars=P)
o.ODE_RTOL = 1e-9
o.ODE_ATOL = 1e-9
o.event = to_event_spring
tnew, ynew = o(yy, tt, tt+2)
tt, yy = o.refine(lambda tf,yf: to_event_spring_refine (tf, yf, P))
t, y = np.hstack([t, tnew]), np.vstack([y, ynew])
if with_forces:
f_pt = [hstack(dy(ttt, yyy, P, output_forces=True))
for ttt, yyy in zip(tnew, ynew)]
# the last indexing switches leg 1 and 2
Forces.append(vstack(f_pt)[:,[2,3,0,1,5,4]])
t_to = tt
# third phase: single support of leg1
P['x_foot2'] = None
o = ode.odeDP5(dy, pars=P)
o.ODE_RTOL = 1e-9
o.ODE_ATOL = 1e-9
o.event = VLO_event
tnew, ynew = o(yy, tt, tt + 2)
tt, yy = o.refine(lambda tf,yf: VLO_event_refine (tf, yf, P))
t, y = np.hstack([t, tnew]), np.vstack([y, ynew])
if with_forces:
f_pt = [hstack(dy(ttt, yyy, P, output_forces=True))
for ttt, yyy in zip(tnew, ynew)]
# the last indexing switches leg 1 and 2
Forces.append(vstack(f_pt)[:,[2,3,0,1,5,4]])
if return_traj:
if with_forces:
return (t, y, (P['x_foot1'], P['x_foot2'], t_td, t_to),
vstack(Forces) )
return t, y, (P['x_foot1'], P['x_foot2'], t_td, t_to)
return y[-1,:]