Commit e4b399d8 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

TP6 final version.

parent e53ac0ec
%% Cell type:markdown id: tags:
# Dynamics: simulation and control
This notebook focuses on the robot dynamics, for both simulating the behavior of the robot when forces are applied to it, and computing dynamic-aware control laws like computed torque.
%% Cell type:code id: tags:
``` python
import magic_donotload
```
%% Output
NB: as for all the tutorials, a magic command %do_not_load is introduced to hide the solutions to some questions. Change it for %load if you want to see (and execute) the solution.
%% Cell type:markdown id: tags:
## Set up
We will use Pinocchio, the viewer and the QP solver quadprog. We use an ad-hoc model of a robot hand with simple geometry to make the contact simulation problem simple.
%% Cell type:code id: tags:
``` python
import math
import time
import pinocchio as pin
import numpy as np
from numpy.random import rand
from numpy.linalg import inv,pinv,norm,eig,svd
import matplotlib.pylab as plt; plt.ion()
import quadprog
from tp6.robot_hand import RobotHand
from tp6.meshcat_viewer_wrapper import MeshcatVisualizer
```
%% Cell type:markdown id: tags:
## QuadProg solver
QuadProg is a simple QP solver that is very convenient to import in Python and very simple to use.
It is package in robotpkg apt repository (alternatively in pip), install it with
"sudo apt install robotpkg-py35-quadprog" or alternatively "pip install --user quadprog".
A simple example using QuadProg is as follow. First randomly define a QP problem min 1/2 x'Ax - b'x s.t. Cx>=0
$$\min_x \frac{1}{2} x^T A x - b^T x$$
Under the constraints:
$$C x \ge d$$
%% Cell type:code id: tags:
``` python
# %load -r 10-16 tp4/qp.py
A = np.random.rand(5,5)*2-1
A = A @ A.T ### Make it positive symmetric
b = np.random.rand(5)
C = np.random.rand(10,5)
d = np.random.rand(10)
```
%% Cell type:markdown id: tags:
The quadratic program can be solved by calling:
%% Cell type:code id: tags:
``` python
# %load -r 19 tp4/qp.py
[x,cost,_,niter,lag,iact] = quadprog.solve_qp(A,b,C.T,d) # Notice that C.T is passed instead of C
```
%% Cell type:markdown id: tags:
where x is the solution, cost is the value of the cost function at x, niter is the number of iterations needed to obtain the optimum, lag is the dual optimum (lagrange multipliers) and iact are the active constraints (numbered from 1 to N, i.e. lag[iact-1] is not null). See the doc for more details.
%% Cell type:code id: tags:
``` python
# help(quadprog.solve_qp)
```
%% Cell type:markdown id: tags:
## A robot hand for our test
For this notebook, we need the additional package Fast-Collision-Library "sudo apt install robotpkg-py36-hpp-fcl".
We are going to use a 4-finger hand, whose model is defined in Python (no urdf model) using capsule volumes.
The hand is defined in the python code robot_hand.py. You can load and
display it like this (don't forget to turn gepetto-viewer on with starting gepetto-gui).
%% Cell type:code id: tags:
``` python
robot=RobotHand()
viz = MeshcatVisualizer(robot)
viz.display(robot.q0)
viz.viewer.jupyter_cell()
```
%% Output
You can open the visualizer by visiting the following URL:
http://127.0.0.1:7005/static/
<IPython.core.display.HTML object>
%% Cell type:markdown id: tags:
Take care that the hand is small: zoom in to see it in the window.
%% Cell type:code id: tags:
``` python
q0 = robot.q0.copy()
from ipywidgets import interact
@interact(q=(0., 2., 0.1))
def move_fingers1(q=0.3):
q0[1:11] = q
viz.display(q0)
@interact(q=(-.5, 1.5, 0.01))
def move_thumb(q=0.7):
q0[11:] = q
viz.display(q0)
```
%% Output
%% Cell type:markdown id: tags:
Remember that the model rendered in the viewer is just a display, not a simulation (yet). You can pretty much achieve any movements you want in this display, because we are not enforcing (yet) any physical law. For example, if you want to move the fingers following any trajectories you like, you can do:
%% Cell type:code id: tags:
``` python
q = robot.q0.copy()
for i in range(500): # Put 1000 or 5000 if you want a longer move.
for iq in range(3,robot.model.nq):
q[iq] = -1+np.cos(i*1e-2*(1+iq/5))
viz.display(q)
time.sleep(2e-3)
```
%% Cell type:markdown id: tags:
## Forward unconstrained dynamics
### Computing the acceleration
%% Cell type:markdown id: tags:
In a first time, let's play with the dynamics without constraints (hence without quadprog).
Choosing an arbitrary joint torque $\tau_q$, pinocchio can compute the robot acceleration and integrate it.
The dynamic equation of the robot is $M a_q + b = \tau_q$, with $M$ the mass, $a_q$ the joint acceleration and $b$ the drift.
The mass matrix can be computed using *CRB* algorithm (function of q). The drift is computed using *RNE* algorithm (function of $q$, $v_q$ and $a_q$ with $a_q=0$).
%% Cell type:code id: tags:
``` python
q = rand(robot.model.nq)
vq = rand(robot.model.nv)
aq0 = np.zeros(robot.model.nv)
b = pin.rnea(robot.model,robot.data,q,vq,aq0) # compute dynamic drift -- Coriolis, centrifugal, gravity
M = pin.crba(robot.model,robot.data,q) # compute mass matrix M
print (norm(M-M.T))
```
%% Output
0.0
%% Cell type:markdown id: tags:
These terms correspond to the inverse dynamics. They can be numerically inverted to compute the direct dynamics.
%% Cell type:code id: tags:
``` python
tauq = np.zeros(robot.model.nv)
```
%% Cell type:markdown id: tags:
Using $M$ and $b$ computed by the above algorithms, and knowing a given set of joint torques $\tau_q$, how would you compute $a_q$ so that $M a_q + b = \tau_q$?
%% Cell type:code id: tags:
``` python
%do_not_load -r 32 tp6/solution_pd.py
```
%% Cell type:markdown id: tags:
The inverse-dynamics algorithm indeed compute the needed torques to achieve a given acceleration. We can use this function to double-check our result:
%% Cell type:code id: tags:
``` python
print(norm(pin.rnea(robot.model,robot.data,q,vq,aq)-tauq))
```
%% Output
4.9145198079073e-16