interact(plotPD, g=(-2.0, 6.0, 0.1), y_start=(-1.0, 2.0, 0.1), yd_start=(-50.0, 50.0, 5.0))     

"""
Explanation: Introduction to Discrete Dynamic Movement Primitives
Arne Böckmann
What are DMPs
<table>
  <tr>
    <td>
    <ul style="list-style-type:disc">
        <li>Dynamical systems</li>
        <li>Represent goal directed movements in joint or task space </li>
        <li>Guaranteed to converge to the goal</li>
        <li>Scale spatial and temporal</li>
        <li>Robust against pertubations</li>
        <li>Adaptable online</li>
        <li>Extendable</li>
        <li>Learnable from demonstration</li>
    </ul>
    </td>
    <td><img src="dmp.png"></td>
  </tr>
</table>

Basic Idea

Use well understood stable dynamical system with convenient properties
Add nonlinear terms to achieve the desired movement behavior

Damped Spring Model
\begin{align}
\dot{z} &= \alpha_z(\beta_z(g-y)-z)\
\dot{y} &= z
\end{align}
$y$ - position, $g$ - goal position
$\alpha_z$, $\beta_z$ - Dampening constants. Use $\beta_z = \alpha_z/4$ for critical dampening.
Damped Spring Model
Difference Equations
\begin{align}
z_t &= z_{t-1} + \alpha_z(\beta_z(g-y_{t-1})-z_{t-1}) * \Delta t\
y_t &= y_{t-1} + z_{t-1} * \Delta t
\end{align}
Damped Spring Model (Demo)
End of explanation
"""


interact(plotPDT, g=(-2.0, 6.0, 0.1), T=(0.01, 1.5, 0.1))     

"""
Explanation: Adding temporal scaling

Add temporal scaling factor $\color{red}\tau$
\begin{align}
\color{red}\tau\dot{z} &= \alpha_z(\beta_z(g-y)-z)\
\color{red}\tau\dot{y} &= z\\
z_t &= z_{t-1} + \frac{\alpha_z(\beta_z(g-y_{t-1})-z_{t-1}) * \Delta t}{\color{red}\tau}\
y_t &= y_{t-1} + \frac{z_{t-1} * \Delta t}{\color{red}\tau}
\end{align}

Adding temporal scaling (Demo)
End of explanation
"""


interact(plotSin, K=(0.1, 1000.0, 0.1))

"""
Explanation: Shaping the Trajectory (The Transformation System)
Introduce the forcing term $\color{red}f$
\begin{align}
z_t &= z_{t-1} + \frac{\alpha_z(\beta_z(g-y_{t-1})-z_{t-1} + \color{red}f) * \Delta t}{\tau}\
\end{align}
* 'forces' the system into a certain shape.
* could be a function of time: $f(t)$
Shaping the Trajectory (Demo)

set $f(t) = K * sin(t)$
End of explanation
"""


interact(plotCS, T=(0.1, 2.0))

"""
Explanation: Restoring Goal Convergence

Influence of $f$ should diminish over time

Introducing the phase $\color{red}s$
\begin{align}
z_t &= z_{t-1} + \frac{\alpha_z(\beta_z(g-y_{t-1})-z_{t-1} + \color{red}sf) * \Delta t}{\tau}\
\end{align}


$\color{red}s$ starts at 1 and exponentially decays to 0


Restoring Goal Convergence (The Canonical System)
$s$ can be generated by the following system:
\begin{align}
\tau \dot{s} = -\alpha_s s\\
s_t = s_{t-1} + \frac{-\alpha_s \Delta t}{T}
\end{align}
* $\alpha_s$ defines how fast the system decays.
Canonical System (Demo)
End of explanation
"""


interact(plotSinCS, sin_scale=(-1000, 1000, 1.0), T=(0.1, 1.5, 0.1), g=(0.0, 6.0, 0.1))

"""
Explanation: Sin Forcing Term and Canonical System (Demo)
End of explanation
"""


interact(plotSinCS2, sin_scale=(-1000, 1000, 1.0), T=(0.1, 1.5, 0.1), g=(0.0, 6.0, 0.1))

"""
Explanation: Restoring Temporal Scaling

Because $f$ is time dependent
Solution: Replace the time dependency with a phase dependency
$f = f(s)$

Restoring Temporal Scaling (Demo)
End of explanation
"""


plt.plot(np.linspace(start=0.0, stop=0.01 * len(values), num = len(values)), values); plt.xlabel("Time"); plt.ylabel("X Position")

"""
Explanation: Imitating arbitrary Trajectories

Set $f(s) = \frac{\sum_{i=1}^N \psi_i(s)w_i}{\sum_{i=1}^N \psi_i(s)}$
$\psi_i$ are fixed basis functions (e.g. radial basis functions)
$N$ is the number of basis functions/weights
$f$ can be shaped by adjusting the $w_i$
$f$ can imitate any given trajectory using locally weighted regression (not part of this talk)

Imitating arbitrary Trajectories
This is a part of B-Human's current kick trajectory
End of explanation
"""


interact(plotImitateDmp, g=(-10, 200, 1), T=(0.1, 1.5, 0.1), y0=(-30, 30, 1))

"""
Explanation: Imitating arbitrary Trajectories (Demo)
End of explanation
"""


interact(plotImitateScaleDmp, g=(-10, 200, 1), T=(0.1, 1.5, 0.1), y0=(-30, 30, 1))

"""
Explanation: Adding Spatial Scaling

$w_i$ are learned for one specific goal position
Are too strong/weak for other goal positions

Solution: Scale the forcing term according to the difference $\color{red}d$ between the original trajectory and the modified one
\begin{align}
\color{red}d &= (y_e - y_0)/(y_{de} - y_{d0})\
z_t &= z_{t-1} + \frac{\alpha_z(\beta_z(g-y_{t-1})-z_{t-1} + \color{red}dsf) * \Delta t}{\tau}\
\end{align}


$y_{de}$ and $y_{d0}$ - End and start position of the demonstrated trajectory


Spatial Scaling (Demo)
End of explanation
"""


interact(plotPerturbateDmp, p_loc=(0.01, 0.5, 0.01), p_strength=(-200, 200, 1))

"""
Explanation: Perturbation robustness
End of explanation
"""


from IPython.display import YouTubeVideo
YouTubeVideo('Ge0GduY1rtE')

from IPython.display import YouTubeVideo
YouTubeVideo('SH3bADiB7uQ')  

"""
Explanation: Extension to multiple dimensions

As long as the dimensions are independent
Use one Transformation System per dimension
Use the same Canonical System for all dimensions

Extensions

Oscillating movements
Enforcing a goal velocity
Obstacle avoidance
Closed-loop perception-action control
Movement synchroization
Rotation Dmps (quaternion based and rotation matrix based)
...

Recap

Dmps are a toolbox to imitate movements and modify them on the fly
Robust against perturbations
Always converge to the goal in time
Can be executed and modified in real-time
Can be extended to multiple dimensions
Can be extended using arbitrary coupling terms to achieve any desired behavior

Recap
Parameters

$\alpha_z$ and $\beta_z$ are constant $\rightarrow$ can be ignored
$\alpha_s$ can be calculated automatically
Good values for the parameters of $f$ can be found automatically
The weights of $f$ can be learned from demonstration using locally weighted regression

Examples
End of explanation
"""


