create an example of single shooting - parametric optim

[mickaelbegon]

idea: Implement single shooting parametric approach of Yeadon as an example that use no control and

nshooting = 1
integrator RK4 with several steps
control = none
params = transition time and value for quintic splines (idem with quartic spline of Bessonnet.)

e.g. 3 phases for 1 DoF
params = [t1, t2, t3, Q1, Q2, Q3]

warning: in case of yeadon's quintic function qdot=0 at start and end

params of quintic function give the kinematic q, qdot and qddot of joints.
root integrated according to free-fall acceleration driven equations:

$$\begin{aligned} \begin{pmatrix} \qddot_B \\ \qddot_J \end{pmatrix} = f_{\text{full,fd}}(\states,\controls) = & M(\q)^{-1} \left(\begin{pmatrix} \bm{0} \\ \btau_J \end{pmatrix} - N(\q,\qdot)\right) \\\ = & \begin{bmatrix} M_{BB}(\q) & M_{BJ}(\q)\\ M_{JB}(\q) & M_{JJ}(\q) \end{bmatrix}^{-1} \begin{pmatrix} \bm{0}-N_B(\q,\qdot) \\ \btau_J - N_J(\q,\qdot)\end{pmatrix}, \end{aligned}$$

where $M(\q)$ is the mass matrix, $N(\q,\qdot)$ is the nonlinear effects vector and gravity effects, and $\btau_J$ are the joint torques.
With this equation, states are $\states=[\q_B ; \q_J ; \qdot_B ; \qdot_J]^\top$ and controls are $\controls=\btau_J$.
The reduced equation of motion, namely FFBD is:

$$\begin{equation} \qddot_B = f_{\text{base,fd}}(\states,\controls) = -M_{BB}(\q)^{-1} \; \left( M_{BJ}(\q) \; \qddot_J + N_B(\q, \qdot)\right), \end{equation}$$

with $\states=[\q_B ; \q_J ; \qdot_B ; \qdot_J]^\top$ and controls $\controls=\qddot_J$.

Find a way to avoid if loop to generate the kinematics: trajectory is the sum of n quintic functions but all but one are zeros according to t.

T = [0] + param[:nb_phases]
for in range(nb_phases):
add = t > T[i] && t < T[i+1]
...
qd[joint] += add* (Qi + tp3 * tp2)

#quintic polynomial of Yeadon.

def trajectory(t):

qd = np.zeros(ws['NDDL'])
qdotd = np.zeros(ws['NDDL'])
qddotd = np.zeros(ws['NDDL'])

for joint in ws['q2']: # TQ was 3D ... change for a vector or do a function to reshape the params
    T = TQ[0, :, qi]
    Q = TQ[1, :, qi]
    
    i = 1
    n = len(T)
    while i < n and t > T[i]:
        i += 1
    
    Ti = T[i - 1]
    Tj = T[i]
    Qi = Q[i - 1]
    Qj = Q[i]
    
    # function quintic
    if t < Ti:
        t = Ti
    elif t > Tj:
        t = Tj
    
    tp0 = Tj - Ti
    tp1 = (t - Ti) / tp0
    tp2 = tp1**3 * (6 * tp1**2 - 15 * tp1 + 10)
    tp3 = Qj - Qi
    tp4 = t - Ti
    tp5 = Tj - t
    
    qd[qi] = Qi + tp3 * tp2
    qdotd[qi] = 30 * tp3 * tp4**2 * tp5**2 / tp0**5
    qddotd[qi] = 60 * tp3 * tp4 * tp5 * (Tj + Ti - 2 * t) / tp0**5

return qd, qdotd, qddotd

[Ipuch]

I was thinking about this problem and the coming refactor on PhaseParameters @pariterre. With a discussion we have, it would be relevant to have:

• VariableContainer, would refer to time series, could be renamed TimeSeriesContainer
• PhaseParameterContainer, would refer to parameters only relevant per phase
• GlobalParameterContainer, (or ModelParameterContainer) ? It would refer to decision variables that could change over multiple phases.

And VariableContainer outputs may be extended to be expressed as a function of PhaseParameters, to get parametric time series.