# Five-Link biped walking loop problem: interactive demonstration

Hello and welcome. This is a Jupyter Notebook, a kind of document that can alternate between static content, like text and images, and executable cells of code.

This document ilustrates the Five-link biped walking loop test case of the paper: "Collocation Methods for Second Order Systems", submitted to RSS 2022.

In order to run the cells of code, you can select the cell and clic on the small "play" button in the bar above or press shift+enter. Alternatively, you can select the option "run" -> "run all cells" in order to run all the code in order. Beware that some cells can take several minutes!

All of the code used in this example is open-source and free to use.

[SymPy](https://www.sympy.org/en/index.html) is used for Symbolic formulation and manipulation of the problem.

[Numpy](https://numpy.org/) is used for numerical arrays and operations.

[CasADI](https://web.casadi.org/) is used for optimization.

[Optibot](https://github.com/AunSiro/optibot) is the name of the package where we are compiling our code. We aim to produce a toolbox for Optimal Control Problems, focused on robotics, including a high level, readable and clean interface between the prior three packages.

## Package imports


```python
import numpy as np
import matplotlib.pyplot as plt
```


```python
from sympy import (symbols, simplify)
from sympy.physics.mechanics import dynamicsymbols, init_vprinting
from sympy.physics.mechanics import Lagrangian, ReferenceFrame, Point, Particle,inertia, RigidBody, angular_momentum
```


```python
from optibot.symbolic import lagrange, diff_to_symb, SimpLagrangesMethod
from optibot.numpy import unpack
```


```python
#SymPy vector-like latex rendering inizialization:

init_vprinting()
```

## Symbolic Problem Modelling

The first step is to model our problem taking advantage of the high level object syntax of the mechanics module in SymPy


```python
# Creating symbols and dynamic symbols

m0, m1, m2, m3, m4, l0, l1, l2, l3, l4, t, g = symbols('m_0:5 l_0:5 t g')
I0, I1, I2, I3, I4, d0, d1, d2, d3, d4 = symbols('I_0:5 d_0:5')
q0, q1, q2, q3, q4 = dynamicsymbols('q_0:5')
m0, m1, m2, m3, m4, l0, l1, l2, l3, l4, t, g, I0, I1, I2, I3, I4, d0, d1, d2, d3, d4, q0, q1, q2, q3, q4
```


```python
# Definition of the physics system

N_in = ReferenceFrame('N')
P0 = Point('P0')
P0.set_vel(N_in, 0)

N0 = N_in.orientnew('N0', 'Axis', [q0, N_in.z])
P1 = P0.locatenew('P1', l0 * N0.y)
P1.set_vel(N_in, P1.pos_from(P0).dt(N_in))
CM0 = P0.locatenew('CM0', (l0-d0) * N0.y)
CM0.set_vel(N_in, CM0.pos_from(P0).dt(N_in))
I_0 = inertia(N0, 0, 0, I0)
body0 = RigidBody('Stance_Tibia', CM0, N0, m0, (I_0,CM0))
body0.potential_energy = m0 * g * CM0.pos_from(P0).dot(N_in.y)


N1 = N_in.orientnew('N1', 'Axis', [q1, N_in.z])
P2 = P1.locatenew('P2', l1 * N1.y)
P2.set_vel(N_in, P2.pos_from(P0).dt(N_in))
CM1 = P1.locatenew('CM1', (l1-d1) * N1.y)
CM1.set_vel(N_in, CM1.pos_from(P0).dt(N_in))
I_1 = inertia(N1, 0, 0, I1)
body1 = RigidBody('Stance_Femur', CM1, N1, m1, (I_1,CM1))
body1.potential_energy = m1 * g * CM1.pos_from(P0).dot(N_in.y)


N2 = N_in.orientnew('N2', 'Axis', [q2, N_in.z])
P3 = P2.locatenew('P3', l2 * N2.y)
P3.set_vel(N_in, P3.pos_from(P0).dt(N_in))
CM2 = P2.locatenew('CM2', d2 * N2.y)
CM2.set_vel(N_in, CM2.pos_from(P0).dt(N_in))
I_2 = inertia(N2, 0, 0, I2)
body2 = RigidBody('Torso', CM2, N2, m2, (I_2,CM2))
body2.potential_energy = m2 * g * CM2.pos_from(P0).dot(N_in.y)


N3 = N_in.orientnew('N3', 'Axis', [q3, N_in.z])
P4 = P2.locatenew('P4', -l3 * N3.y)
P4.set_vel(N_in, P4.pos_from(P0).dt(N_in))
CM3 = P2.locatenew('CM3', -d3 * N3.y)
CM3.set_vel(N_in, CM3.pos_from(P0).dt(N_in))
I_3 = inertia(N3, 0, 0, I3)
body3 = RigidBody('Swing_Femur', CM3, N3, m3, (I_3,CM3))
body3.potential_energy = m3 * g * CM3.pos_from(P0).dot(N_in.y)


N4 = N_in.orientnew('N4', 'Axis', [q4, N_in.z])
P5 = P4.locatenew('P5', -l4 * N4.y)
P5.set_vel(N_in, P5.pos_from(P0).dt(N_in))
CM4 = P4.locatenew('CM4', -d4 * N4.y)
CM4.set_vel(N_in, CM4.pos_from(P0).dt(N_in))
I_4 = inertia(N4, 0, 0, I4)
body4 = RigidBody('Swing_Tibia', CM4, N4, m4, (I_4,CM4))
body4.potential_energy = m4 * g * CM4.pos_from(P0).dot(N_in.y)
```


```python
#Computing the Lagrangian

Lag_simp = Lagrangian(N_in, body0, body1, body2, body3, body4)
Lag_simp
```


```python
from optibot.symbolic import ImplicitLagrangesMethod
```


```python
# Defining the control forces and external actions, and applying them to our system

u0, u1, u2, u3, u4 = symbols('u_:5')
FL = [
    (N0, (u0-u1) * N_in.z),
    (N1, (u1-u2) * N_in.z),
    (N2, (u2-u3) * N_in.z),
    (N3, (u3-u4) * N_in.z),
    (N4, u4 * N_in.z)
]
LM_small = ImplicitLagrangesMethod(Lag_simp, [q0, q1, q2, q3, q4], forcelist=FL, frame=N_in)
```


```python
# Generating the dynamic equations

LM_small.form_lagranges_equations()
```


```python
impl_x = LM_small.implicit_dynamics_x
```


```python
impl_q = LM_small.implicit_dynamics_q
```

### Generating auxiliar functions

Later in the problem we need some expressions derived from the problem. Here we will generate them as symbolic expressions, and then convert them to numerical functions.


```python
import casadi as cas
from sympy import lambdify
from optibot.casadi import implicit_dynamic_q_to_casadi_function, implicit_dynamic_x_to_casadi_function, sympy2casadi
from optibot.symbolic import find_arguments, diff_to_symb_expr
from sympy.physics.mechanics import kinetic_energy, potential_energy
```


```python
imp_dyn_x_f_cas = implicit_dynamic_x_to_casadi_function(impl_x, list(dynamicsymbols('x_0:10')), verbose=True)
```


```python
imp_dyn_q_f_cas = implicit_dynamic_q_to_casadi_function(impl_q, list(LM_small.q), verbose=True)
```


```python
imp_dyn_x_f_cas
```


```python
feet_x = P5.pos_from(P0).dot(N_in.x)
feet_x = diff_to_symb_expr(feet_x)
feet_x
```


```python
feet_y = P5.pos_from(P0).dot(N_in.y)
feet_y = diff_to_symb_expr(feet_y)
feet_y
```


```python
feet_y_vel = P5.vel(N_in).dot(N_in.y)   #pos_from(P0).dot(N_in.y)
feet_y_vel = diff_to_symb_expr(feet_y_vel)
feet_y_vel
```


```python
cm_pos = m0*CM0.pos_from(P0)
cm_pos += m1*CM1.pos_from(P0)
cm_pos += m2*CM2.pos_from(P0)
cm_pos += m3*CM3.pos_from(P0)
cm_pos += m4*CM4.pos_from(P0)
cm_pos = cm_pos/(m0+m1+m2+m3+m4)

sys_CM = P0.locatenew('Sys_CM', cm_pos)
sys_CM_x = simplify(sys_CM.pos_from(P0).dot(N_in.x))
sys_CM_y = simplify(sys_CM.pos_from(P0).dot(N_in.y))
```


```python
sym_x = dynamicsymbols('q_0:5')
sym_x = sym_x + [ii.diff() for ii in sym_x]
sym_x = [diff_to_symb(ii) for ii in sym_x]
sym_params = list(symbols('I_0:5 d_0:5 g l_0:2 l_3 m_0:5'))
sym_add_params = [symbols('l_4'),]
sym_vars = sym_x + sym_params + sym_add_params

print(len(sym_vars), sym_vars)
```


```python
cas_x_args = cas.MX.sym("x", len(sym_x))
cas_params = cas.MX.sym("p", len(sym_params))
cas_add_params = cas.MX.sym("p_add", len(sym_add_params))
cas_all_vars = [cas_x_args[ii] for ii in range(len(sym_x))]
cas_all_vars += [cas_params[ii] for ii in range(len(sym_params))]
cas_all_vars += [cas_add_params[ii] for ii in range(len(sym_add_params))]
print(len(cas_all_vars), cas_all_vars)
```


```python
_cas_expr_temp_x = sympy2casadi(feet_x, sym_vars, cas_all_vars)
feet_x_cas = cas.Function(
        "Feet_x",
        [cas_x_args, cas_params, cas_add_params],
        [_cas_expr_temp_x,],
        ["x", "params", "additional_params"],
        ["feet_x_position"],
    )
```


```python
_cas_expr_temp_y = sympy2casadi(feet_y, sym_vars, cas_all_vars)
feet_y_cas = cas.Function(
        "Feet_y",
        [cas_x_args, cas_params, cas_add_params],
        [_cas_expr_temp_y,],
        ["x", "params", "additional_params"],
        ["feet_y_position"],
    )
```


```python
_cas_expr_temp_y_vel = sympy2casadi(feet_y_vel, sym_vars, cas_all_vars)
feet_y_vel_cas = cas.Function(
        "Feet_y_vel",
        [cas_x_args, cas_params, cas_add_params],
        [_cas_expr_temp_y_vel,],
        ["x", "params", "additional_params"],
        ["feet_y_speed"],
    )
```


```python
def simetric_cond_casadi(n = 5):
    x1 = cas.MX.sym('x_1', 2*n)
    x2 = cas.MX.sym('x_2', 2*n)
    cond = [x1[ii] - x2[n-1-ii] for ii in range(n)]
    cas_funcs = cas.horzcat(*cond)
    return cas.Function(
        "Sim_cond",
        [x1, x2],
        [cas_funcs,],
        ["x_1", "x2"],
        ["residue"],
    )
```


```python
simetric_5_links = simetric_cond_casadi(5)
```

Creating and simplifying symbolically the expressions of the heel impact may require some time, but it alows for a faster problem formulation later on.


```python
bodies = [body0, body1, body2, body3, body4]
points_right = [P0, P1, P2, P2, P4]
points_left = [P5, P4, P2, P2, P1]
subs_key = list(zip(dynamicsymbols('q_0:5'),dynamicsymbols('q_p_0:5')))

impact_eqs = []
for ii in range(5):
    print('calculating eq', ii)
    print('\tleft side')
    left_side = angular_momentum(points_left[ii], N_in, *bodies[:5-ii]).dot(N_in.z)
    left_side = simplify(left_side)
    print('\tright side')
    right_side = angular_momentum(points_right[ii], N_in, *bodies[ii:]).dot(N_in.z)
    right_side = simplify(right_side).subs(subs_key)
    impact_eqs.append(left_side-right_side)
#impact_eqs
```


```python
def impact_cond_casadi(eqs, x1_sym, x2_sym, sym_params, sym_add_params):
    x1_sym = [diff_to_symb(ii) for ii in x1_sym]
    x2_sym = [diff_to_symb(ii) for ii in x2_sym]
    eqs = [diff_to_symb_expr(ii) for ii in eqs]

    all_vars = x1_sym + x2_sym + sym_params + sym_add_params
    n = len(x1_sym)
    cas_x1 = cas.MX.sym('x_1', n)
    cas_x2 = cas.MX.sym('x_2', n)
    cas_params = cas.MX.sym("p", len(sym_params))
    cas_add_params = cas.MX.sym("p_add", len(sym_add_params))
    cas_all_vars = [cas_x1[ii] for ii in range(n)]
    cas_all_vars += [cas_x2[ii] for ii in range(n)]
    cas_all_vars += [cas_params[ii] for ii in range(len(sym_params))]
    cas_all_vars += [cas_add_params[ii] for ii in range(len(sym_add_params))]
    
    cas_funcs = []
    for function in eqs:
        cas_funcs.append(sympy2casadi(function, all_vars, cas_all_vars))
    cas_funcs = cas.horzcat(*cas_funcs)
    return cas.Function(
        "Sim_cond",
        [cas_x1, cas_x2, cas_params, cas_add_params],
        [cas_funcs,],
        ["x_1", "x2", 'params', 'additional_params'],
        ["residue"],
    )
```


```python
sym_x = dynamicsymbols('q_0:5')
sym_x = sym_x + [ii.diff() for ii in sym_x]
subs_key = list(zip(dynamicsymbols('q_0:5'),dynamicsymbols('q_p_0:5')))
sym_x_2 = [ii.subs(subs_key) for ii in sym_x]
impact_cond_cas_f = impact_cond_casadi(impact_eqs, sym_x,  sym_x_2, sym_params, sym_add_params)
```


```python
sys_cm_np = lambdify([sym_x, sym_params], [sys_CM_x, sys_CM_y],'numpy')
```


```python
ang_mom_p0 = angular_momentum(P0, N_in, *bodies).dot(N_in.z)
ang_mom_p0_np = lambdify([sym_x, sym_params], ang_mom_p0,'numpy')
```


```python
ang_mom_p5 = angular_momentum(P5, N_in, *bodies).dot(N_in.z)
ang_mom_p5_np = lambdify([sym_x, sym_params, sym_add_params], ang_mom_p5,'numpy')
```


```python
P5_static = P5.locatenew('P5_static', 0 * N_in.y)
P5_static.set_vel(N_in, 0 * N_in.y)
```


```python
ang_mom_p5_static = angular_momentum(P5_static, N_in, *bodies).dot(N_in.z)
ang_mom_p5_static_np = lambdify([sym_x, sym_params, sym_add_params], ang_mom_p5_static,'numpy')
```


```python
angular_momentum(P0, N_in, bodies[0]).dot(N_in.z)
```


```python
system_energy = potential_energy(*bodies) + kinetic_energy(N_in, *bodies)
```


```python
system_energy_np = lambdify([sym_x, sym_params], system_energy,'numpy')
```


```python
mass_matrix_np = lambdify([sym_x, sym_params], LM_small.mass_matrix,'numpy')
```


```python
sym_u = symbols('u_:5')
F_impl_np = lambdify([sym_x, sym_u, sym_params], LM_small.forcing,'numpy')
```

### Scheme definitions

Each scheme is defined here as a function that must be equal to zero at each interval.
Note that functions that contain "mod" in the name are those we define as "second order",
and use separate conditions for q and v.

Note that we will operate this problem wihout combining the scheme equations $F(q_k, q_{k+1}, q'_k, q'_{k+1} q''_k, q''_{k+1}, SchemeParams) = 0$ and the dynamics equations $H(q, q', q'', u, params) = 0$ imposed at the collocation points. This approach allows us to solve this problem without inverting the mass matrix.

If you wish to define your own schemes, do it here.

Be careful to respect the function structure:

    restriction(x, x_n, a, a_n, dt, scheme_params) = 0


```python
from optibot.schemes import index_div
from copy import copy

def euler_accel_restr(x, x_n, a, a_n, dt, scheme_params):
    first_ind, last_ind = index_div(x)
    x_d = copy(x)
    x_d[first_ind] = x[last_ind]
    x_d[last_ind] = a
    return x_n - (x + dt * x_d)


def trapz_accel_restr(x, x_n, a, a_n, dt, scheme_params):
    first_ind, last_ind = index_div(x)
    x_d = copy(x)
    x_d[first_ind] = x[last_ind]
    x_d[last_ind] = a
    x_d_n = copy(x)
    x_d_n[first_ind] = x_n[last_ind]
    x_d_n[last_ind] = a_n
    return x_n - (x + dt / 2 * (x_d + x_d_n))


def trapz_mod_accel_restr(x, x_n, a, a_n, dt, scheme_params):
    res = copy(x)
    first_ind, last_ind = index_div(x)
    res[last_ind] = x[last_ind] + dt / 2 * (a + a_n)
    res[first_ind] = x[first_ind] + dt * x[last_ind] + dt ** 2 / 6 * (a_n + 2 * a)
    return x_n - res


def hs_half_x(x, x_n, x_d, x_d_n, dt):
    x_c = (x + x_n) / 2 + dt / 8 * (x_d - x_d_n)
    return x_c


def hs_accel_restr(x, x_n, a, a_n, dt, scheme_params):
    a_c = scheme_params
    first_ind, last_ind = index_div(x)
    x_d = copy(x)
    x_d[first_ind] = x[last_ind]
    x_d[last_ind] = a

    x_d_n = copy(x)
    x_d_n[first_ind] = x_n[last_ind]
    x_d_n[last_ind] = a_n

    x_c = hs_half_x(x, x_n, x_d, x_d_n, dt)
    x_d_c = copy(x)
    x_d_c[first_ind] = x_c[last_ind]
    x_d_c[last_ind] = a_c
    return x + dt / 6 * (x_d + 4 * x_d_c + x_d_n) - x_n


def hs_mod_half_x(x, x_n, a, a_n, dt):
    x_c = copy(x)
    first_ind, last_ind = index_div(x)
    q = x[first_ind]
    v = x[last_ind]
    q_n = x_n[first_ind]
    v_n = x_n[last_ind]
    q_c = q + dt / 32 * (13 * v + 3 * v_n) + dt**2 / 192 * (11 * a - 5 * a_n)
    v_c = (v + v_n) / 2 + dt / 8 * (a - a_n)
    x_c[first_ind] = q_c
    x_c[last_ind] = v_c
    return x_c


def hs_mod_accel_restr(x, x_n, a, a_n, dt, scheme_params):
    a_c = scheme_params.T
    res = copy(x)
    first_ind, last_ind = index_div(x)
    q = x[first_ind]
    v = x[last_ind]
    res[last_ind] = v + dt / 6 * (a + 4 * a_c + a_n)
    res[first_ind] = q + dt * v + dt ** 2 / 6 * (a + 2 * a_c)
    return x_n - res
```

### Casadi optimization

We have generated the system equations symbolicaly. Now, we translate them to CasADi objects in order to perform the optimization.


```python
from optibot.casadi import accelrestriction2casadi
#from optibot.schemes import (euler_accel_restr, trapz_accel_restr, trapz_mod_accel_restr,
#                             hs_mod_accel_restr, hs_accel_restr, hs_half_x)
```


```python
#Numerical values of the paramenters

I_0_n, I_1_n, I_2_n, I_3_n, I_4_n = 0.93, 1.08, 2.22, 1.08, 0.93
d_0_n, d_1_n, d_2_n, d_3_n, d_4_n = 0.128, 0.163, 0.2, 0.163, 0.128
g_n = 9.81
l_0_n, l_1_n, l_2_n, l_3_n, l_4_n = 0.4, 0.4, 0.625, 0.4, 0.4
m_0_n, m_1_n, m_2_n, m_3_n, m_4_n = 3.2, 6.8, 20, 6.8, 3.2
params = [
    I_0_n, I_1_n, I_2_n, I_3_n, I_4_n,
    d_0_n, d_1_n, d_2_n, d_3_n, d_4_n,
    g_n,
    l_0_n, l_1_n, l_3_n,
    m_0_n, m_1_n, m_2_n, m_3_n, m_4_n
]
additional_params = [l_4_n,]
```


```python
opti = cas.Opti()
p_opts = {}#{"expand":True,'ipopt.print_level':0, 'print_time':0}
s_opts = {}#{"max_iter": 10000, 'tol': 1e-26}#, 'linear_solver' : "MA27"}
opti.solver("ipopt",p_opts,
                    s_opts)
```


```python
N = 25
X = opti.variable(N+1,10)
X_dot = opti.variable(N+1,10)
U = opti.variable(N+1,5)
U_c = opti.variable(N,5)
X_c = opti.variable(N,10)
X_dot_c = opti.variable(N,10)
```


```python
T = opti.parameter()
u_m = opti.parameter()
Params_opti = opti.parameter(len(params))
Add_params_opti = opti.parameter(len(additional_params))
D = opti.parameter()
```


```python
# Definition of the cost function

#cost = cas.sum2((cas.sum1(U[:,:]**2)+cas.sum1(U[1:-1,:]**2))/N)
cost = cas.sum2((4*cas.sum1(U_c[:,:]**2) + cas.sum1(U[:,:]**2)+cas.sum1(U[1:-1,:]**2))/(3*N))
#cost = cas.sum2(cas.sum1(U**2))
opti.minimize(cost)
```


```python
#Periodic gait constraint:
opti.subject_to(simetric_5_links(X[0,:], X[-1,:]) == 0)
opti.subject_to(impact_cond_cas_f(X[-1,:], X[0,:], Params_opti, Add_params_opti) == 0)
```


```python
#Step size constraint:
opti.subject_to(feet_x_cas(X[-1,:], Params_opti, Add_params_opti) == D)
opti.subject_to(feet_y_cas(X[-1,:], Params_opti, Add_params_opti) == 0)
```


```python
#Small Feet Conditions:
opti.subject_to(U[:,0] == 0)
opti.subject_to(U_c[:,0] == 0)
opti.subject_to(feet_y_vel_cas(X[0,:], Params_opti, Add_params_opti)>0)
opti.subject_to(feet_y_vel_cas(X[-1,:], Params_opti, Add_params_opti)<0)
```


```python
#Feet over ground Restrictions:
for ii in range(1,N):
    opti.subject_to(feet_y_cas(X[ii,:], Params_opti, Add_params_opti) > 0)
```


```python
#Dynamics Constraints:
for ii in range(N+1):
    opti.subject_to(imp_dyn_x_f_cas(X[ii,:], X_dot[ii,:], U[ii,:], [], Params_opti) == 0)
for ii in range(N):
    opti.subject_to(X_c[ii,:] == hs_mod_half_x(X[ii,:], X[ii+1,:], X_dot[ii,5:], X_dot[ii+1,5:], T/N))
    opti.subject_to(imp_dyn_x_f_cas(X_c[ii,:], X_dot_c[ii,:], U_c[ii,:], [], Params_opti) == 0)
```


```python
#Scheme Constraints
#cas_accel_restr = accelrestriction2casadi(trapz_mod_accel_restr, 5)
cas_accel_restr = accelrestriction2casadi(hs_mod_accel_restr, 5, 5)
for ii in range(N):
    opti.subject_to(cas_accel_restr(X[ii,:], X[ii+1,:], X_dot[ii, 5:], X_dot[ii+1, 5:],T/N, X_dot_c[ii,5:]) == 0)
```


```python
opti.set_value(T, 0.7)#0.7
opti.set_value(D, 0.5)
```


```python
opti.set_value(Params_opti, params)
opti.set_value(Add_params_opti, additional_params)
```


```python
q_0_guess = np.array([-0.3, 0.7, 0, -0.5, -0.6])
q_1_guess = q_0_guess[::-1]
s_arr = np.linspace(0, 1, N+1)
q_guess = np.expand_dims(q_0_guess,0)+ np.expand_dims(s_arr,1)*np.expand_dims((q_1_guess - q_0_guess),0)

q_dot_guess = (q_1_guess - q_0_guess) * np.ones([N+1,1])/opti.value(T)
```


```python
opti.set_initial(X[:,:5], q_guess)
opti.set_initial(X[:,5:], q_dot_guess)

opti.set_initial(X_c[:,:5], (q_guess[:-1,:]+q_guess[1:,:])/2)
opti.set_initial(X_c[:,5:], q_dot_guess[:-1,:])

opti.set_initial(X_dot[:,:5], q_dot_guess)
opti.set_initial(X_dot[:,5:], 0)

opti.set_initial(X_dot_c[:,:5], q_dot_guess[:-1,:])
opti.set_initial(X_dot_c[:,5:], 0)

opti.set_initial(U, 0)
opti.set_initial(U_c, 0)
```


```python
sol = opti.solve()
```


```python
U_sol = sol.value(U)
U_c_sol = sol.value(U_c)
X_sol = sol.value(X)
X_c_sol = sol.value(X_c)
X_dot_sol = sol.value(X_dot)
X_dot_c_sol = sol.value(X_dot_c)
T_sol = sol.value(T)
T_sol_arr = np.linspace(0, T_sol, N+1)
T_c_arr = (T_sol_arr[:-1]+T_sol_arr[1:])/2
```


```python
plt.figure(figsize=[14,10])
labels= ['stance anckle', 'stance knee', 'stance hip', 'swing hip', 'swing knee']
for ii in range(5):
    plt.plot(T_sol_arr,U_sol[:,ii], marker = 'o', label = labels[ii] + ' u_k')
    plt.plot(T_c_arr,U_c_sol[:,ii], 'o', label = labels[ii] + ' u_c')
plt.grid()
plt.legend()
plt.title('u(t)')
```


```python
plt.figure(figsize=[14,10])
labels= ['stance tibia', 'stance femur', 'torso', 'swing femur', 'swing tibia']
for ii in range(5):
    plt.plot(T_sol_arr, X_sol[:,ii], marker = 'o', label = labels[ii] + ' q_k')
    plt.plot(T_c_arr,X_c_sol[:,ii], 'o', label = labels[ii] + ' q_c')
plt.grid()
plt.legend()
plt.title('q(t)')
```


```python
def chain_to_draw(x,params):
    [
    I_0_n, I_1_n, I_2_n, I_3_n, I_4_n,
    d_0_n, d_1_n, d_2_n, d_3_n, d_4_n,
    g_n,
    l_0_n, l_1_n, l_3_n,
    m_0_n, m_1_n, m_2_n, m_3_n, m_4_n
    ] = params
    points_x = [0, ]
    points_y = [0, ]
    points_x.append(points_x[-1] - l_0_n*np.sin(x[0]))
    points_x.append(points_x[-1] - l_1_n*np.sin(x[1]))
    points_x.append(points_x[-1] - l_2_n*np.sin(x[2]))
    points_x.append(points_x[-2])
    points_x.append(points_x[-1] + l_3_n*np.sin(x[3]))
    points_x.append(points_x[-1] + l_4_n*np.sin(x[4]))
    
    
    points_y.append(points_y[-1] + l_0_n*np.cos(x[0]))
    points_y.append(points_y[-1] + l_1_n*np.cos(x[1]))
    points_y.append(points_y[-1] + l_2_n*np.cos(x[2]))
    points_y.append(points_y[-2])
    points_y.append(points_y[-1] - l_3_n*np.cos(x[3]))
    points_y.append(points_y[-1] - l_4_n*np.cos(x[4]))
    
    return points_x, points_y
```


```python
points_x, points_y = chain_to_draw(X_sol[0], params)
plt.figure(figsize=[15,15])
plt.grid()
for ii in range(0, N, 1):
    points_x, points_y = chain_to_draw(X_sol[ii], params)
    plt.plot(points_x, points_y, lw=1,  color = plt.cm.viridis(ii/N))
    

plt.gca().set_aspect('equal')
```

total_mass = m_0_n + m_1_n + m_2_n + m_3_n + m_4_n
ang_mom_arr = [ang_mom_p0_np(X_sol[ii,:],params) for ii in range(N+1)]
ang_mom_swing_foot_arr = [ang_mom_p5_np(X_sol[ii,:],params, additional_params) for ii in range(N+1)]
ang_mom_swing_foot_static_arr = [ang_mom_p5_static_np(X_sol[ii,:],params, additional_params) for ii in range(N+1)]
cm_torque_arr = [total_mass * -g_n * sys_cm_np(X_sol[ii,:], params)[0] for ii in range(N+1)]
ang_mom_arr_deriv = np.gradient(ang_mom_arr, T_sol_arr)


```python
from optibot.schemes import interpolated_array, interpolated_array_derivative
from optibot.analysis import dynamic_error_implicit
```

## Sistematic comparative of schemes for different values of N

Now let's solve the problem with different methods.

### Caution!

Executing the next cell may require some time!


```python
def q_init(N):
    q_0_guess = np.array([-0.3, 0.7, 0, -0.5, -0.6])
    q_1_guess = q_0_guess[::-1]
    s_arr = np.linspace(0, 1, N+1)
    q_guess = np.expand_dims(q_0_guess,0)+ np.expand_dims(s_arr,1)*np.expand_dims((q_1_guess - q_0_guess),0)
    q_dot_guess = (q_1_guess - q_0_guess) * np.ones([N+1,1])/opti.value(T)
    return q_guess, q_dot_guess
```


```python
import time
def chrono_solve(opti, solve_repetitions):
    cput0 = time.time()
    for ii in range(solve_repetitions):
        sol = opti.solve()
    cput1 = time.time()
    cpudt = (cput1-cput0)/solve_repetitions
    return sol, cpudt
```


```python
def casadi_biped(N = 25, scheme = "trapz", solve_repetitions = 1, t_end = 0.7, step_length = 0.5):
    opti = cas.Opti()
    p_opts = {"expand":True,'ipopt.print_level':0, 'print_time':0}
    s_opts = {"max_iter": 10000, 'tol': 1e-26}#, 'linear_solver' : "MA27"}
    opti.solver("ipopt",p_opts,
                        s_opts)
    
    restr_schemes = {
        'trapz': trapz_accel_restr,
        'trapz_mod' : trapz_mod_accel_restr,
        'hs': hs_accel_restr,
        'hs_mod': hs_mod_accel_restr,
        'hs_parab': hs_accel_restr,
        'hs_mod_parab': hs_mod_accel_restr
    }
    
    f_restr = restr_schemes[scheme]

    X = opti.variable(N+1,10)
    X_dot = opti.variable(N+1,10)
    U = opti.variable(N+1,5)
    if 'hs' in scheme:
        U_c = opti.variable(N,5)
        X_c = opti.variable(N,10)
        X_dot_c = opti.variable(N,10)

    T = opti.parameter()
    u_m = opti.parameter()
    Params_opti = opti.parameter(len(params))
    Add_params_opti = opti.parameter(len(additional_params))
    D = opti.parameter()

    # Cost
    if 'parab' in scheme:
        cost = cas.sum2((4*cas.sum1(U_c[:,:]**2) + cas.sum1(U[:,:]**2)+cas.sum1(U[1:-1,:]**2))/(3*N))
    else:
        cost = cas.sum2((cas.sum1(U[:,:]**2)+cas.sum1(U[1:-1,:]**2))/N)
    #cost = cas.sum2(cas.sum1(U**2))
    opti.minimize(cost)

    #Periodic gait constraint:
    opti.subject_to(simetric_5_links(X[0,:], X[-1,:]) == 0)
    opti.subject_to(impact_cond_cas_f(X[-1,:], X[0,:], Params_opti, Add_params_opti) == 0)

    #Step size constraint:
    opti.subject_to(feet_x_cas(X[-1,:], Params_opti, Add_params_opti) == D)
    opti.subject_to(feet_y_cas(X[-1,:], Params_opti, Add_params_opti) == 0)

    #Small Feet Conditions:
    opti.subject_to(U[:,0] == 0)
    opti.subject_to(feet_y_vel_cas(X[0,:], Params_opti, Add_params_opti)>0)
    opti.subject_to(feet_y_vel_cas(X[-1,:], Params_opti, Add_params_opti)<0)
    if 'hs' in scheme:
        opti.subject_to(U_c[:,0] == 0)

    #Feet over ground Restrictions:
    for ii in range(1,N):
        opti.subject_to(feet_y_cas(X[ii,:], Params_opti, Add_params_opti) > 0)

    #Dynamics Constraints:
    for ii in range(N+1):
        opti.subject_to(imp_dyn_x_f_cas(X[ii,:], X_dot[ii,:], U[ii,:], [], Params_opti) == 0)
    if 'hs' in scheme:
        for ii in range(N):
            opti.subject_to(X_c[ii,:] == hs_half_x(X[ii,:], X[ii+1,:], X_dot[ii,:], X_dot[ii+1,:], T/N))
            opti.subject_to(imp_dyn_x_f_cas(X_c[ii,:], X_dot_c[ii,:], U_c[ii,:], [], Params_opti) == 0)
        if 'parab' not in scheme:
            for ii in range(N):
                opti.subject_to(U_c[ii,:] == (U[ii,:]+U[ii+1,:])/2)

    #Scheme Constraints
    if 'hs' in scheme:
        cas_accel_restr = accelrestriction2casadi(f_restr, 5, 5)
        for ii in range(N):
            opti.subject_to(cas_accel_restr(X[ii,:], X[ii+1,:], X_dot[ii, 5:], X_dot[ii+1, 5:],T/N, X_dot_c[ii,5:]) == 0)
    else:
        cas_accel_restr = accelrestriction2casadi(f_restr, 5)
        for ii in range(N):
            opti.subject_to(cas_accel_restr(X[ii,:], X[ii+1,:], X_dot[ii, 5:], X_dot[ii+1, 5:],T/N, []) == 0)
    

    opti.set_value(T, t_end)#0.7
    opti.set_value(D, step_length)#0.5

    opti.set_value(Params_opti, params)
    opti.set_value(Add_params_opti, additional_params)
    
    q_guess, q_dot_guess = q_init(N)
    opti.set_initial(X[:,:5], q_guess)
    opti.set_initial(X[:,5:], q_dot_guess)
    
    opti.set_initial(X_dot[:,:5], q_dot_guess)
    opti.set_initial(X_dot[:,5:], 0)
    opti.set_initial(U, 0)
    
    if 'hs' in scheme:
        opti.set_initial(X_c[:,:5], (q_guess[:-1,:]+q_guess[1:,:])/2)
        opti.set_initial(X_c[:,5:], q_dot_guess[:-1,:])

        opti.set_initial(X_dot_c[:,:5], q_dot_guess[:-1,:])
        opti.set_initial(X_dot_c[:,5:], 0)
        opti.set_initial(U_c, 0)

    sol, cpudt = chrono_solve(opti, solve_repetitions)

    U_sol = sol.value(U)
    X_sol = sol.value(X)
    X_dot_sol = sol.value(X_dot)
    T_sol = sol.value(T)
    T_sol_arr = np.linspace(0, T_sol, N+1)
    T_c_arr = (T_sol_arr[:-1]+T_sol_arr[1:])/2
    cost_sol = sol.value(cost)
    if 'hs' in scheme:
        U_c_sol = sol.value(U_c)
        X_c_sol = sol.value(X_c)
        X_dot_c_sol = sol.value(X_dot_c)
    else:
        U_c_sol = None
        X_c_sol = None
        X_dot_c_sol = None
        
    return{
        'u':U_sol,
        'x':X_sol,
        'x_dot':X_dot_sol,
        't':T_sol,
        't_array':T_sol_arr,
        't_c_array': T_c_arr,
        'cpudt':cpudt,
        'u_c':U_c_sol,
        'x_c':X_c_sol,
        'x_dot_c':X_dot_c_sol,
        'cost':cost_sol
    }
```


```python
schemes = ['hs_parab', 'hs_mod_parab','trapz', 'trapz_mod']
solve_repetitions = 3
N_arr = [20, 25, 30, 40, 50, 60]
results = {}

for scheme in schemes:
    key = scheme
    print('Problem:', key)
    results[key] = {'N_arr':N_arr}
    for N in N_arr:
        print(f'\tN = {N}')
        results[key][N] = casadi_biped(
            N = N,
            scheme = scheme,
            solve_repetitions = solve_repetitions,
            t_end = 0.7,
            step_length = 0.5)
```

### Calculating dynamic errors for each case

Caution! May take several seconds to run!


```python
schemes = ['hs_parab', 'hs_mod_parab','trapz', 'trapz_mod']

n_graph = 2000 # A higher number here will provide more exact results but take longer to run
t_arr = np.linspace(0,0.7,n_graph)
for scheme in schemes:
    key = scheme
    if 'parab' in scheme:
        u_scheme = 'parab'
    else:
        u_scheme = 'lin'
    print('Problem:', key)
    N_arr = results[key]['N_arr']
    for N in N_arr:
        print(f'\tN = {N}')
        dyn_err_q, dyn_err_v, _, dyn_err_2 = dynamic_error_implicit(
            x_arr=results[key][N]['x'],
            u_arr=results[key][N]['u'],
            t_end=results[key][N]['t'],
            params = params,
            F_impl = F_impl_np,
            M = mass_matrix_np,
            scheme = scheme, 
            u_scheme = u_scheme,
            scheme_params={'u_c':results[key][N]['u_c'],
                          'x_dot_c': results[key][N]['x_dot_c'],
                          'x_c': results[key][N]['x_c']},
            n_interp= n_graph)
        results[key][N]['dyn_err_q'] = dyn_err_q
        results[key][N]['dyn_err_v'] = dyn_err_v
        results[key][N]['dyn_err_2'] = dyn_err_2
```


```python
# Plot settings
plt.rcParams.update({'font.size': 15})
oct_fig_size = [10,6]
```


```python
schemes = ['hs_parab','hs_mod_parab', 'trapz', 'trapz_mod']
titles = ['Hermite Simpson','2nd order Hermite Simpson', 'Trapezoidal', '2nd order Trapezoidal']
colors = ['b', 'orange', 'g', 'r', 'purple']
n_int = len(t_arr)
N = 25
interv_n = (N * t_arr)/results[scheme][N]['t']
for kk in range(len(schemes)):
    scheme = schemes[kk]
    plt.figure(figsize=[14,8])
    for ii in range(5):
        cut_p = 0
        for ll in range(1,N+1):
            jj = np.searchsorted(interv_n, ll)
            plt.plot(t_arr[cut_p:jj],results[scheme][N]['dyn_err_q'][cut_p:jj,ii], '-', c = colors[ii], label = f'$q_{ii+1}$' if cut_p == 0 else None)
            cut_p = jj
    plt.plot(np.linspace(0,results[scheme][N]['t'],N+1), np.zeros(N+1), 'ok')
    plt.legend()
    plt.grid()
    if kk == 1:
        plt.ylim([-0.00001, 0.00001])
    elif kk == 3:
        plt.ylim([-0.001, 0.001])
    plt.title(r'First order dynamic error $\varepsilon^{[1]}_{q_i}$,'+f' {titles[kk]} scheme')
    plt.xlabel('Time(s)')
    plt.ylabel('Dynamic error $(rad/s)$')
    plt.tight_layout(pad = 0.0)
    sch_type = titles[kk].replace(' ','_')
    
    # If you are running the notebook locally and want to save the plots,
    # uncomment the next line
    #plt.savefig(f'5_link_First_Order_Dynamic_Error_{sch_type}_scheme.eps', format='eps')

```


```python
schemes = ['hs_parab','hs_mod_parab', 'trapz', 'trapz_mod']
titles = ['Hermite Simpson','2nd order Hermite Simpson', 'Trapezoidal', '2nd order Trapezoidal']
colors = ['b', 'orange', 'g', 'r', 'purple']
n_int = len(t_arr)
N = 25
interv_n = (N * t_arr)/results[scheme][N]['t']
for kk in range(len(schemes)):
    scheme = schemes[kk]
    plt.figure(figsize=[14,8])
    for ii in range(5):
        cut_p = 0
        for ll in range(1,N+1):
            jj = np.searchsorted(interv_n, ll)
            plt.plot(t_arr[cut_p:jj],results[scheme][N]['dyn_err_2'][cut_p:jj,ii], '-', c = colors[ii], label = f'$q_{ii+1}$' if cut_p == 0 else None)
            cut_p = jj
    plt.plot(results[scheme][N]['t_array'], np.zeros(N+1), 'ok', label = 'knot & collocation points')
    if 'hs' in scheme:
        plt.plot(results[scheme][N]['t_c_array'], np.zeros(N), 'ow', markeredgecolor='b', label = 'collocation points')
        plt.ylim([-0.08, 0.08])
    else:
        plt.ylim([-1.75, 1.75])
    plt.legend()
    plt.grid()
    plt.title(r'Second order dynamic error $\varepsilon^{{[2]}}_{{q_i}}$,'+f' {titles[kk]} scheme')
    plt.xlabel('Time(s)')
    plt.ylabel('Dynamic error $(rad/s^2)$')
    plt.tight_layout(pad = 0.0)
    sch_type = titles[kk].replace(' ','_')
    
    # If you are running the notebook locally and want to save the plots,
    # uncomment the next line
    #plt.savefig(f'5_link_Second_Order_Dynamic_Error_{sch_type}_scheme.eps', format='eps')

```


```python
def arr_mod(x):
    x_1 = np.sum(x*x, axis=1)
    return np.sqrt(x_1)
def arr_sum(x):
    return np.sum(np.abs(x), axis = 1)
def arr_max(x):
    return np.max(np.abs(x), axis = 1)
```


```python
schemes = ['hs_mod_parab','hs_parab']#, 'trapz', 'trapz_mod']
titles = ['2nd order Hermite Simpson','Hermite Simpson']#, 'Trapezoidal', 'Modified Trapezoidal']
colors = ['b', 'orange', 'g', 'r', 'purple']
funcs = [arr_sum,]#arr_mod,  arr_max
#func_tittles = ['Module of', 'Sum of absolute', 'Maximum of absolute']
y_max_list = [0.12, 0.2, 0.09]
n_int = len(t_arr)
N = 25
interv_n = (N * t_arr)/results[scheme][N]['t']
for ii in range(1):
    plt.figure(figsize=oct_fig_size)
    for kk in [1,0]:
        scheme = schemes[kk]
        cut_p = 0
        for ll in range(1,N+1):
            jj = np.searchsorted(interv_n, ll)
            y_plot = funcs[ii](results[scheme][N]['dyn_err_2'])
            plt.plot(t_arr[cut_p:jj],y_plot[cut_p:jj], '-', c = f'C{kk}', label = titles[kk] if cut_p == 0 else None)
            cut_p = jj
    plt.plot(results[scheme][N]['t_array'], np.zeros(N+1), 'ok', label = 'knot & collocation points')
    plt.plot(results[scheme][N]['t_c_array'], np.zeros(N), 'ow', markeredgecolor='k', label = 'collocation points')
    plt.legend()
    plt.grid()
    plt.ylim([-0.01,y_max_list[ii]])
    plt.title(r'Second order dynamic error $\varepsilon^{[2]}$,'+f' N = {N}')
    plt.xlabel('Time(s)')
    plt.ylabel('Dynamic error $(rad/s^2)$')
    plt.tight_layout(pad = 0.0)
    
    # If you are running the notebook locally and want to save the plots,
    # uncomment the next line
    #plt.savefig(f'5_link_HS_N{N}_second_order_dynamic_error.eps', format='eps')
    

```


```python
schemes = ['trapz', 'trapz_mod']
titles = ['Trapezoidal', '2nd order Trapezoidal']
funcs = [arr_sum,]#arr_mod,  arr_max
y_max_list = [0.12, 0.2, 0.09]
n_int = len(t_arr)
N = 50
interv_n = (N * t_arr)/results[scheme][N]['t']
for ii in range(1):
    plt.figure(figsize=oct_fig_size)
    for kk in range(2):
        scheme = schemes[kk]
        cut_p = 0
        for ll in range(1,N+1):
            jj = np.searchsorted(interv_n, ll)
            y_plot = funcs[ii](results[scheme][N]['dyn_err_2'])
            plt.plot(t_arr[cut_p:jj],y_plot[cut_p:jj], '-', c = f'C{kk+2}', label = titles[kk] if cut_p == 0 else None)
            cut_p = jj
    plt.plot(results[scheme][N]['t_array'], np.zeros(N+1), 'ok', label = 'knot & collocation points')
    plt.legend()
    plt.grid()
    plt.title(r'Second order dynamic error $\varepsilon^{[2]}$,'+f' N = {N}')
    plt.xlabel('Time(s)')
    plt.ylabel('Dynamic error $(rad/s^2)$')
    plt.tight_layout(pad = 0.0)
    
    # If you are running the notebook locally and want to save the plots,
    # uncomment the next line
    #plt.savefig(f'5_link_Trapezoidal_N{N}_second_order_dynamic_error.eps', format='eps')
    

```


```python
def total_state_error(t_arr, dyn_err):
    errors = np.trapz(np.abs(dyn_err), t_arr, axis=0)
    return errors
```


```python
schemes = ['hs_parab', 'hs_mod_parab','trapz', 'trapz_mod']
N_arr = [10,15,20,25,30,40,50,75,100,150]

t_arr = np.linspace(0,0.7,n_graph)
for scheme in schemes:
    key = scheme
    print('Problem:', key)
    N_arr = results[key]['N_arr']
    for N in N_arr:
        print(f'\tN = {N}')
        for letter in 'qv2':
            results[key][N][f'integ_dyn_err_{letter}']= total_state_error(t_arr, results[scheme][N][f'dyn_err_{letter}'])
            results[key][N][f'module_dyn_err_{letter}']= np.sqrt(np.dot(results[key][N][f'integ_dyn_err_{letter}'], results[key][N][f'integ_dyn_err_{letter}']))
            results[key][N][f'sum_dyn_err_{letter}'] = np.sum(results[key][N][f'integ_dyn_err_{letter}'])
        
```


```python
for scheme in schemes:
    key = scheme
    print('Problem:', key)
    N_arr = results[key]['N_arr']
    
    for letter in 'qv2':
        list_mod = []
        list_sum = []
        for N in N_arr:
            #print(f'\tN = {N}')
            list_mod.append(results[key][N][f'module_dyn_err_{letter}'])
            list_sum.append(results[key][N][f'sum_dyn_err_{letter}'])
        results[key][f'module_dyn_err_{letter}_array'] = np.array(list_mod)
        results[key][f'sum_dyn_err_{letter}_array'] = np.array(list_sum)
```


```python
# For each scheme, the number of collocation points can be obtained
for scheme in results.keys():
    if 'hs' in scheme:
        n_coll = np.array(results[scheme]['N_arr'])*2-1
        results[scheme]['N_coll_arr'] = n_coll
    else:
        results[scheme]['N_coll_arr'] = results[scheme]['N_arr']
```


```python
schemes = ['hs_mod_parab','hs_parab', 'trapz', 'trapz_mod']
titles = ['2nd order Hermite Simpson', 'Hermite Simpson','Trapezoidal', '2nd order Trapezoidal']
plt.figure(figsize=oct_fig_size)
for ii in [2,3,1,0]:
    key = schemes[ii]
    plt.plot(results[key]['N_arr'], results[key][f'sum_dyn_err_2_array'], marker = 'o', c = f'C{ii}',label = titles[ii])
plt.grid()
plt.legend()
plt.yscale('log')
plt.title('Second order dynamic error $E^{[2]}$')
plt.xlabel('Number of intervals')
plt.ylabel('Dynamic error ($rad/s$)')
plt.tight_layout(pad = 0.0)

# If you are running the notebook locally and want to save the plots,
# uncomment the next line
#plt.savefig(f'5_link_Sum_second_order_dynamic_error_vs_interval_number.eps', format='eps')
```


```python
for scheme in schemes:
    key = scheme
    print('Problem:', key)
    N_arr = results[key]['N_arr']
    list_cpudt = []
    for N in N_arr:
        #print(f'\tN = {N}')
        list_cpudt.append(results[key][N]['cpudt'])
    results[key]['cpudt_array'] = np.array(list_cpudt)
```


```python
schemes = ['hs_mod_parab','hs_parab', 'trapz', 'trapz_mod']
titles = ['2nd order Hermite Simpson', 'Hermite Simpson','Trapezoidal', '2nd order Trapezoidal']
plt.figure(figsize=oct_fig_size)
for ii in [2,3,1,0]:
    key = schemes[ii]
    plt.plot(results[key]['N_arr'], results[key][f'cpudt_array'], marker = 'o', c = f'C{ii}', label = titles[ii])
plt.grid()
plt.legend()
#plt.yscale('log')
plt.title('Optimization time')
plt.xlabel('Number of intervals')
plt.ylabel('Time (s)')
plt.tight_layout(pad = 0.0)

# If you are running the notebook locally and want to save the plots,
# uncomment the next line
#plt.savefig(f'5_link_optimization_vs_interval_number.eps', format='eps')
```


```python
# Here we print the data shown in Table III of the paper

for scheme in ['hs_mod_parab', 'hs_parab', 'trapz', 'trapz_mod']:
    key = scheme
    for N in [25,50]:#results[key]['N_arr']:
        print('scheme:', scheme, 'N:', N,'\n\ttime:', results[key][N][f'cpudt'],
              '\n\tErr 1:', results[key][N]['sum_dyn_err_q'], '\n\tErr 2:', results[key][N]['sum_dyn_err_2'])
```

## Animation


```python
from matplotlib import animation, rc
import matplotlib.patches as patches
from matplotlib.transforms import Affine2D
from IPython.display import HTML
import matplotlib
```


```python
matplotlib.rcParams['animation.embed_limit'] = 200
```


```python
def body_tray(X, params):
    res = []
    for ii in range(X.shape[0]):
        res.append(list(chain_to_draw(X[ii,:], params)))
    return np.array(res)
```


```python
def loop_body_tray(X, params):
    point_tray = body_tray(X, params)
    point_tray_loop = np.append(
        point_tray,
        np.expand_dims(
            np.array(list(chain_to_draw(X[0,[4,3,2,1,0,5,6,7,8,9]],params)))
            ,0),
        0)
    return point_tray_loop
```


```python
def mod_sum(iterable, start):
    for element in iterable:
        start += element
    return start
```


```python
def create_anim(X, U, params, n_loops = 1):
    [
    I_0_n, I_1_n, I_2_n, I_3_n, I_4_n,
    d_0_n, d_1_n, d_2_n, d_3_n, d_4_n,
    g_n,
    l_0_n, l_1_n, l_3_n,
    m_0_n, m_1_n, m_2_n, m_3_n, m_4_n
    ] = params
    
    N = X.shape[0]
    fig, ax = plt.subplots()
    draw_width = 14
    draw_height = 14
    
    fig.set_dpi(72)
    fig.set_size_inches([draw_width,draw_height])
    ax.set_xlim(( -1, 1))
    ax.set_ylim(( -0.2, 1.8))
    
    body, = ax.plot([], [], lw=4, ms = 12, marker = 'o')
    trail, = ax.plot([], [], lw=1, color = 'k')
    old_trail, = ax.plot([], [], lw=1, color = 'k')
    next_trail, = ax.plot([], [], lw=1, color = 'k')
    
    
    point_tray = body_tray(X, params)
    point_tray_loop = loop_body_tray(X, params)
    #sys_cm_point, = ax.plot([], [], 'go', ms=12)
    #line_sys_cm, = ax.plot([], [], 'k:', lw=1)
    
    print_vars = [X[:,ii] for ii in range(5)]+[np.linspace(0, N-1, N, dtype=int)]
    print_var_names = [f'q_{ii}' for ii in range(5)]+['step']
    texts = []
    ii = 0.8
    for arr in print_vars:
        texts.append(ax.text(-0.8, ii, "", fontsize = 12))
        ii -= 0.2
    
    ax.grid()
    
    def init():
        body.set_data([], [])
        trail.set_data(point_tray_loop[0,0,-1], point_tray_loop[0,1,-1])
        old_trail.set_data(point_tray_loop[:,0,-1]-0.5, point_tray_loop[:,1,-1])
        #next_trail.set_data(point_tray_loop[:,0,-1]+0.5, point_tray_loop[:,1,-1])
        #sys_cm_point.set_data([], [])
        #line_sys_cm.set_data([], [])
        return (body,)
    
    def animate(i):
        margin_x = -0.25 + i * 0.5/N
        trail.set_data(point_tray_loop[0:i+1,0,-1], point_tray_loop[0:i+1,1,-1])
        #sys_cm_coords = sys_cm_np(X[i,:], params)
        #sys_cm_point.set_data(sys_cm_coords)
        #line_sys_cm.set_data([0, sys_cm_coords[0]], [0, sys_cm_coords[1]])
        
        ax.set_xlim(( -1+ margin_x, 1+ margin_x))
        points_x, points_y = point_tray[i,:,:]
        body.set_data(points_x, points_y) 
        
        for ii in range(len(texts)):
            text = texts[ii]
            name = print_var_names[ii]
            arr = print_vars[ii]
            text.set_position((-0.9 + margin_x, 1.7 - 0.05*ii))
            if name == 'step':
                text.set_text("$step$ = " + str(arr[i]))
            else:
                text.set_text("$" + name + "$ = %.3f" % arr[i])
        return (body,)
    iterable_frames = mod_sum([[jj for jj in range(N)]for kk in range(n_loops)], start = [])
    anim = animation.FuncAnimation(fig, animate, init_func=init,
                               frames=iterable_frames, interval=20, 
                               blit=True)
    return anim




```


```python
anim = create_anim(results['hs_mod_parab'][25]['x'][:-1,:],results['hs_mod_parab'][25]['u'], params, 4)
```


```python
HTML(anim.to_jshtml())
```


```python
f = r"biped_animation.mp4" 
writervideo = animation.FFMpegWriter(fps=25//0.7) 
# If you are running the notebook locally and want to save the animation,
# uncomment the next line
#anim.save(f, writer=writervideo)
```


```python

```

a1d91564870d9ee35478d3d68f1b4e154dc9d2e5
ipynb
Jupyter Notebook
Five-Link-Biped-demo.ipynb
AunSiro/Second-Order-Schemes
ef7ac9a6755e166d81b83f584f82055d38265087
MIT
Five-Link-Biped-demo.ipynb
AunSiro/Second-Order-Schemes
ef7ac9a6755e166d81b83f584f82055d38265087
MIT
Five-Link-Biped-demo.ipynb
AunSiro/Second-Order-Schemes
ef7ac9a6755e166d81b83f584f82055d38265087
MIT
Qwen/Qwen-72B
1. YES
2. YES