In [1]:
# define system
def dy(state, t, p):
"""
the right-hand side of the ode. Here: Lorentz' Equation
arguments:
t: (float) time
state: (3-dimensional float) state
p: (3-tuple) parameters
returns:
dy/dt (3x float) derivative of the function
These equations build the famous Lorenz attractor and are taken from wikipedia.
"""
# rename so that it is easier to write down the equations
rho, sigma, beta = p # name the parameters
x, y, z = state # name the variables
dy_dt = zeros(3) # a 3-dimensional result vector
dy_dt[0] = sigma * (y - x)
dy_dt[1] = x * (rho - z) - y
dy_dt[2] = x * y - beta * z
return dy_dt
# import solver
from scipy.integrate import odeint
# select initial conditions
y0 = [.5, .6, .7] # define initial state
t_vector = linspace(0, 20, 1000) # define times where to compute the solution
params = [28, 10, 8./3.] # rho, sigma, beta
# run the integration
# a note to the parameters ("args"): We could define dy as follows:
# dy(state, t, p1, p2, ..., pn) and call odeint accordingly
# odeint(dy, y0, t_vector, args=(p1, p2, ..., pn). Here, we pass
# only a single(!) argument - which is a vector.
res = odeint(dy, y0, t_vector, args=(params,))
In [2]:
# visualize the result
figure()
plot(res[:,0], res[:,1], 'r-')
Out[2]:
The special thing here is that certain events like touchdown, takeoff, ... can interrupt the integration and switch to a different system. Here, we will run the SLIP system.
NOTE: We need a special solver that can handle these events. It is not supplied with standard python distributions, but freely available and distributable.
In [42]:
from libshai import integro
from copy import deepcopy
params = {'k' : 20000,
'g' : [0, -9.81,],
'l0' : 1.,
'alpha' : 68. * pi / 180.,
'm' : 80.,
'foot' : [-1, 0], #foot position - will be updated
}
IC = array([0, 1, 5, 0]) #x,y, vx, vy
In [43]:
def dy_stance(t, y, pars, return_force = False):
"""
:args:
t (float): simulation time
y (4x float): CoM state
pars (dict): parameters
return_force (bool, default: False): return forces instead of dy/dt.
"""
l = norm(array(y[:2]) - array(pars['foot'])) # leg length
F = -pars['k'] * (l - pars['l0']) # force magnitude
f_vec = (array(y[:2]) - array(pars['foot'])) / l * F # force vector: magnitude + direction
if return_force:
return f_vec
return hstack([y[2:], f_vec / pars['m'] + pars['g']])
def dy_flight(t, y, pars):
"""
a ballistic curve - just gravity acts
"""
return [y[2], y[3], pars['g'][0], pars['g'][1]]
In [44]:
def takeoff_event(t, states, traj, pars):
"""
triggers the take off of a leg.
:args:
t (2x float): list of time prior to and after event
states (2x array): list of states prior to and after event
traj (trajectory patch): a trajectory patch (ignored here)
pars (<any>): the leg functions parameters
:returns:
(bool) takeoff detected? (force has falling zero crossing)
"""
# NOTE: Here, the simulation state at two instances of time is compared!
# First instance of time: Y = states[0]
# Second instance of time: Y = states[1]
l0 = norm(array(states[0][:2]) - array(pars['foot']))
l1 = norm(array(states[1][:2]) - array(pars['foot']))
return l0 < pars['l0'] and l1 >= pars['l0']
def takeoff_event_refine(t, state, pars):
"""
refine the event - the zero-crossing of this function is the
ultimate event time.
:args:
t (float): list of time prior to and after event
state (array): list of states prior to and after event
pars (<any>): the leg functions parameters
:returns:
here, leg length minus leg rest length.
"""
l = norm(array(state[:2]) - array(pars['foot']))
return l - pars['l0']
def touchdown_event(t, states, traj, pars):
"""
triggers the touchdown of a leg.
:args:
t (2x float): list of time prior to and after event
states (2x array): list of states prior to and after event
traj (trajectory patch): a trajectory patch (ignored here)
pars (<any>): the leg functions parameters
:returns:
(bool) takeoff detected? (force has falling zero crossing)
"""
# what's the height of the leg when it's uncompressed?
yleg = pars['l0'] * sin(pars['alpha'])
tip0 = states[0][1] - yleg # tip height over ground
tip1 = states[1][1] - yleg # tip height over ground
return tip0 > 0 and tip1 <= 0
def touchdown_event_refine(t, state, pars):
"""
refine the event - the zero-crossing of this function is the
ultimate event time.
:args:
t (float): list of time prior to and after event
state (array): list of states prior to and after event
pars (<any>): the leg functions parameters
:returns:
here, leg length minus leg rest length.
"""
yleg = pars['l0'] * sin(pars['alpha'])
tip = state[1] - yleg # tip height over ground
return tip
In [45]:
# first, we copy the parameters because the will be updated (the "foot" position changes!)
p = deepcopy(params)
tmax = 2 # simulate at most 2 seconds (stop anyway if an event is detected)
y_all = [] # append simulation states
t_all = [] # append simulation times
# we need to adapt the call signature of the refine-function: only t and y are arguments!
td_refine = lambda t,y: touchdown_event_refine(t, y, p)
to_refine = lambda t,y: takeoff_event_refine(t, y, p)
# run a flight phase
ode_flight = integro.odeDP5(dy_flight, pars=p)
ode_flight.event = touchdown_event # activate event
t_flight, y_flight = ode_flight([IC,], 0, tmax, dt=.01) # run simulation
t_td, y_td = ode_flight.refine(td_refine) # refine event detection. NOTE: this also adapts t_flight, y_flight!
# store results
y_all.append(y_flight)
t_all.append(t_flight)
# adapt foot position
p['foot'] = [y_td[0] + cos(p['alpha']) * p['l0'], 0]
# run a stance phase
ode_stance = integro.odeDP5(dy_stance, pars=p)
ode_stance.event = takeoff_event # activate event
t_stance, y_stance = ode_stance(y_td, t_td, t_td + tmax, dt=.01) # run simulation
t_to, y_to = ode_stance.refine(to_refine) # refine event detection. NOTE: this also adapts t_flight, y_flight!
# store results
y_all.append(y_stance)
t_all.append(t_stance)
In [57]:
# run another flight phase
t_flight, y_flight = ode_flight(y_to, t_to, t_to + tmax, dt=.01) # run simulation
t_td, y_td = ode_flight.refine(td_refine) # refine event detection. NOTE: this also adapts t_flight, y_flight!
# store results
y_all.append(y_flight)
t_all.append(t_flight)
# adapt foot position
p['foot'] = [y_td[0] + cos(p['alpha']) * p['l0'], 0]
# run another stance phase
t_stance, y_stance = ode_stance(y_td, t_td, t_td + tmax, dt=.01) # run simulation
t_to, y_to = ode_stance.refine(to_refine) # refine event detection. NOTE: this also adapts t_flight, y_flight!
# store results
y_all.append(y_stance)
t_all.append(t_stance)
In [58]:
# note: we need to "hstack" the time because it's one-dimensional, and "vstack" the states because they are multidimensional (d=4)
y_sim = vstack(y_all)
t_sim = hstack(t_all)
In [59]:
figure(figsize=(12,7))
subplot(1,2,1)
plot(t_sim, y_sim[:,1],'rd-', label='vertical position')
xlabel('simulation time [s]')
ylabel('CoM height [m]')
legend()
subplot(1,2,2)
plot(t_sim, y_sim[:,2],'gd-', label='horizontal speed')
xlabel('simulation time [s]')
ylabel('horizontal speed [m s$^{-1}$]')
legend()
Out[59]:
In [41]: