def legODE1D(state, t, params):
"""
This function defines the simpleModels axial leg as an ODE. It can be used
to simulate the behavior.
Parameters
----------
state : *array* (1x3)
State of the system: [x1, x2, x3] alias [l, \dot{l} and l_d]
t : *float*
the time. This is ignored since the system is time-independent.
params : *dict*
a dictionary
Returns
-------
dx/dt : *array* (1x3)
The derivative of the state as a function of the state: dx/dt = f(x)
"""
P = mi.Struct(params) # allow struct-style access to keys
x1, x2, x3 = state
x1_dot = x2
# F_S = -1. * P.k * (x1 - x3 - P.ls0)
x2_dot = -1. * P.k / P.m * (x1 - x3 - P.ls0) + P.g
x3_dot = -1. / P.d * (-1. * P.k * (x1 - x3 - P.ls0) + P.kd * (x3 - P.ld0))
return array([x1_dot, x2_dot, x3_dot])
def VLO_event_refine(t, state, pars):
"""
calcualtes the vertical leg orientation instant - refinement
"""
P = mi.Struct(pars)
x, y, phi, vx, vy, vphi = state
hip = (x + np.sin(phi) * P.r_hip, y - np.cos(phi) * P.r_hip)
return hip[0] - P.x_foot1
def VLO_event(t, states, traj, pars):
"""
calcualtes the vertical leg orientation instant
"""
P = mi.Struct(pars)
x1, y1, phi1, vx1, vy1, vphi1 = states[0]
x2, y2, phi2, vx2, vy2, vphi2 = states[1]
hip1 = (x1 + np.sin(phi1) * P.r_hip, y1 - np.cos(phi1) * P.r_hip)
hip2 = (x2 + np.sin(phi2) * P.r_hip, y2 - np.cos(phi2) * P.r_hip)
return hip1[0] < P.x_foot1 and hip2[0] >= P.x_foot1
def td_event_spring_refine(t, state, pars):
"""
refinement function for SLIP 2nd leg touchdown event.
This function returns some value whose zero-crossing indicates the event to
be detected.
"""
x, y, phi, vx, vy, vphi = state
P = mi.Struct(pars)
hip = (x + np.sin(phi) * P.r_hip, y - np.cos(phi) * P.r_hip)
y_foot = hip[1] - np.sin(P.legpars2['alpha']) * P.legpars2['l0']
return -y_foot
def to_event_spring_refine(t, state, pars):
"""
refinement function for SLIP 2nd leg takeoff event.
This function returns some value whose zero-crossing indicates the event to
be detected.
"""
P = mi.Struct(pars)
x, y, phi, vx, vy, vphi = state
hip = (x + np.sin(phi) * P.r_hip, y - np.cos(phi) * P.r_hip)
l = np.sqrt((hip[0] - P.x_foot2)**2 + hip[1]**2)
return l - P.legpars2['l0']
def roi(d, Params):
"""
a root for this expression has to be found!
THIS IS A SIMPLIFICATION OF THE ABOVE EXPRESSION WITH k = 3*kd
"""
P = mi.Struct(Params)
res = (((P.kd / P.m - 16 * P.kd**2 / (9 * d**2))**3 +
(-P.kd**2 / (d * P.m) + 128 * P.kd**3 /
(27 * d**3))**2 / 4.)**(1 / 2) - P.kd**2 / (2 * d * P.m) +
64 * P.kd**3 / (27. * d**3))**(1. / 3.)
return res
def remTerm(d, Params):
"""
test function to visualize if there are any roots possible
"""
P = mi.Struct(Params)
kd = P.k / 3.
res = (((P.k / (3. * P.m) - (P.k + kd)**2 / (9. * d**2))**3 +
(P.k * kd / (d * P.m) - P.k * (P.k + kd) / (3. * d * P.m) + 2. *
(P.k + kd)**3 / (27. * d**3))**2 / 4.)**(1. / 2.) + P.k * kd /
(2. * d * P.m) - P.k * (P.k + kd) / (6. * d * P.m) + (P.k + kd)**3 /
(27. * d**3))**(1. / 3.) - (P.k + kd) / (3. * d)
return res
def to_event_spring(t, states, traj, pars):
"""
triggers the takeoff of the 2nd leg.
*Note* it is assumed that *always* the 2nd leg is the trailing leg. This
has to be ensured by appropriate transitions.
Condition is: rest length is reached.
*Note* There could be further conditions: e.g., leg *must* be behind CoM,
behind hip, ... whatever (useful for more detailled model investigations)
"""
P = mi.Struct(pars)
x1, y1, phi1, vx1, vy1, vphi1 = states[0]
x2, y2, phi2, vx2, vy2, vphi2 = states[1]
hip1 = (x1 + np.sin(phi1) * P.r_hip, y1 - np.cos(phi1) * P.r_hip)
l1 = np.sqrt((hip1[0] - P.x_foot2)**2 + hip1[1]**2)
hip2 = (x2 + np.sin(phi2) * P.r_hip, y2 - np.cos(phi2) * P.r_hip)
l2 = np.sqrt((hip2[0] - P.x_foot2)**2 + hip2[1]**2)
return l1 < P.legpars2['l0'] and l2 >= P.legpars2['l0']
def VPP_steps(IC, pars, n=2, count_only=False):
"""
performs up to n steps of the VPP model
:args:
IC (1x6 float): initial condition
pars (dict): parameters for VPP model
n (int): steps to perform at most
count_only (bool): return only the number of successful steps
:returns:
t, y: the time and the state of the model, or
n: the number of steps reached before falling down
"""
localpars = deepcopy(pars)
niter = 0
t, y = [0], [IC]
last_t0 = 0
while niter < n:
# on update: DO NOT FORGET TO UPDATE THE FEET LOCATIONS
tt, yy, feet = VPP_step(IC, localpars, return_traj=True)
localpars['x_foot1'] = feet[0]
localpars['x_foot2'] = feet[1]
# skip first element - this is just the IC
y.append(yy[1:])
t.append(tt[1:] + last_t0)
last_t0 = t[-1][-1]
niter += 1
# check if touchdown is possible (foot position above ground):
P = mi.Struct(pars)
hip_y = yy[-1, 1] - np.cos(yy[-1, 2]) * P.r_hip
if hip_y - np.sin(P.legpars2['alpha']) * P.legpars2['l0'] <= 0:
print "no touchdown possible (n=", niter, ")"
print "hip_y: ", hip_y
break
IC = yy[-1, :]
if not count_only:
return np.hstack(t), np.vstack(y)
else:
return niter
def td_event_spring(t, states, traj, pars):
"""
triggers the touchdown event for leg 2 (which will become leg 1 at
touchdown)
Condition is: the leg touches ground (here, a fixed angle w.r.t. world is
assumed. To change this, edit (a) this trigger and (b) the appropriate
touchdown transitions.
"""
P = mi.Struct(pars)
x1, y1, phi1, vx1, vy1, vphi1 = states[0]
x2, y2, phi2, vx2, vy2, vphi2 = states[1]
hip1 = (x1 + np.sin(phi1) * P.r_hip, y1 - np.cos(phi1) * P.r_hip)
hip2 = (x2 + np.sin(phi2) * P.r_hip, y2 - np.cos(phi2) * P.r_hip)
y_foot1 = hip1[1] - P.legpars2['l0'] * np.sin(P.legpars2['alpha'])
y_foot2 = hip2[1] - P.legpars2['l0'] * np.sin(P.legpars2['alpha'])
return y_foot1 > 0 and y_foot2 <= 0
def dy(t, state, pars, output_forces=False):
"""
equations of motion for single- and double stance
:args:
t (float): absolute simulation time
state (array): system state: x, y, phi, vx, vy, vphi
pars (dict): system parameters. required keys:
legfun1(function: t, l, l_dot, legpars -> float): the leg force law
legpars1(dict): the parameters for the leg force law
legfun2(function: t, l, l_dot, legpars -> float): the leg force law
for the 2nd leg. Can be (None) if not present
legpars2(dict): the parameters for the leg force law
J (float): inertia of trunk
m (float): mass of trunk
r_h (float): distance hip-CoM
r_vpp1 (float): distance CoM-vpp, for leg 1
a_vpp1 (float): angle between trunk and line CoM-VPP, for leg 1
r_vpp2 (float): distance CoM-vpp, for leg 2
a_vpp2 (float): angle between trunk and line CoM-VPP, for leg 2
x_foot1 (float): x-position of foot 1, or None
x_foot2 (float): x-position of foot 2, or None
g (1x2 float): acceleration due to gravity (e.g. [0, -9.81])
output_foces (bool): if True, output the forces and torques
"""
P = mi.Struct(pars)
x, y, phi, vx, vy, vphi = state
hip = (x + np.sin(phi) * P.r_hip, y - np.cos(phi) * P.r_hip)
vhip = (vx + np.cos(phi) * vphi * P.r_hip,
vy + np.sin(phi) * vphi * P.r_hip)
# calculate (axial) force for first leg, if present
Fleg1 = 0
if (P.x_foot1 is not None) and P.legfun1:
l1 = np.sqrt((hip[0] - P.x_foot1)**2 + hip[1]**2)
ldot1 = .5 / np.sqrt(l1) * (2. * (hip[0] - P.x_foot1) * vhip[0] +
2. * hip[1] * vhip[1])
Fleg1 = P.legfun1(t, l1, ldot1, P.legpars1)
# calculate (axial) force for second leg, if present
Fleg2 = 0
if (P.x_foot2 is not None) and P.legfun2:
l2 = np.sqrt((hip[0] - P.x_foot2)**2 + hip[1]**2)
ldot2 = .5 / np.sqrt(l2) * (2. * (hip[0] - P.x_foot2) * vhip[0] +
2. * hip[1] * vhip[1])
Fleg2 = P.legfun2(t, l2, ldot2, P.legpars2)
# calculate required torques at each hip (1 & 2)
T1 = 0
F1 = np.array([0, 0])
if Fleg1:
vpp1 = (x - P.r_vpp1 * np.sin(P.a_vpp1 + phi),
y + P.r_vpp1 * np.cos(P.a_vpp1 + phi))
dir_f1 = np.array([vpp1[0] - P.x_foot1, vpp1[1]])
dir_f1 = dir_f1 / li.norm(dir_f1)
F1 = Fleg1 * dir_f1
dir_l1 = np.array([P.x_foot1 - hip[0], -hip[1]])
dir_l1 = dir_l1 / li.norm(dir_l1)
leg1 = l1 * dir_l1
T1_T = leg1[0] * F1[1] - leg1[1] * F1[0]
# don't forget the torque created by Fxr_hip
T1_F = (hip[0] - x) * F1[1] - (hip[1] - y) * F1[0]
T1 = T1_T + T1_F
T2 = 0
F2 = np.array([0, 0])
if Fleg2:
# trunk leg angle
vpp2 = (x - P.r_vpp2 * np.sin(P.a_vpp2 + phi),
y + P.r_vpp2 * np.cos(P.a_vpp2 + phi))
dir_f2 = np.array([vpp2[0] - P.x_foot2, vpp2[1]])
dir_f2 = dir_f2 / li.norm(dir_f2)
F2 = Fleg2 * dir_f2
dir_l2 = np.array([P.x_foot2 - hip[0], -hip[1]])
dir_l2 = dir_l2 / li.norm(dir_l2)
leg2 = l2 * dir_l2
T2_T = leg2[0] * F2[1] - leg2[1] * F2[0]
# don't forget the torque created by Fxr_hip
T2_F = (hip[0] - x) * F2[1] - (hip[1] - y) * F2[0]
T2 = T2_T + T2_F
F = F1 + F2
if not output_forces:
return [
vx, vy, vphi, F[0] / P.m + P.g[0], F[1] / P.m + P.g[1],
(T1 + T2) / P.J
]
else:
return [F1, F2, T1, T2]