Commit 7d240c3a authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

[python] Stop using StdVec in python to make code more user friendly

parent 4ae77d5d
......@@ -22,9 +22,7 @@ PLOT_JOINT_ACC = 1
PLOT_TORQUES = 0
USE_VIEWER = 1
vector = pin.StdVec_StdString()
vector.extend(item for item in conf.path)
robot = tsid.RobotWrapper(conf.urdf, vector, False)
robot = tsid.RobotWrapper(conf.urdf, [conf.path], False)
model = robot.model()
formulation = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
......
%% Cell type:markdown id: tags:
# Joint-Space Inverse Dynamics
This notebook shows how to use the Python bindings of the C++ library TSID to control a manipulator in joint space.
%% Cell type:code id: tags:
``` python
import sys
sys.path.append('..')
import numpy as np
from numpy.linalg import norm
import matplotlib.pyplot as plt
import plot_utils as plut
import time
import pinocchio as pin
import tsid
#import gepetto.corbaserver
import subprocess
import os
import talos_arm_conf as conf
```
%% Cell type:markdown id: tags:
Create a `RobotWrapper` specifying the URDF file describing the robot.
%% Cell type:code id: tags:
``` python
vector = pin.StdVec_StdString()
vector.extend(item for item in conf.path)
robot = tsid.RobotWrapper(conf.urdf, vector, False)
robot = tsid.RobotWrapper(conf.urdf, [conf.path], False)
model = robot.model()
```
%% Cell type:markdown id: tags:
Create an `InverseDynamicsFormulationAccForce` object to collect all tasks together and transform them into a QP.
%% Cell type:code id: tags:
``` python
formulation = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
q0 = conf.q0
v0 = np.zeros(robot.nv)
formulation.computeProblemData(0.0, q0, v0)
```
%% Cell type:markdown id: tags:
Create a `TaskJointPosture` to control the joint configuration of the robot. Set the proportional and derivative gains $k_p$ and $k_d$. Add the task to the formulation object with a user-specified weight to the priority level 1 (1 corresponds to the cost function, 0 corresponds to the constraints).
%% Cell type:code id: tags:
``` python
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(conf.kp_posture * np.ones(robot.nv))
postureTask.setKd(2.0 * np.sqrt(conf.kp_posture) * np.ones(robot.nv))
priorityLevel = 1
transitionTime = 0.0
formulation.addMotionTask(postureTask, conf.w_posture, priorityLevel, transitionTime)
```
%% Cell type:markdown id: tags:
Create a constant trajectory in Euclidian space to use as reference for the joint posture task.
%% Cell type:code id: tags:
``` python
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q0)
postureTask.setReference(trajPosture.computeNext())
```
%% Cell type:markdown id: tags:
Create a TaskJointBounds to specify the joint velocity limits and add it to the formulation object with priority level 0 (which means it is added as a hard constraint in the QP).
%% Cell type:code id: tags:
``` python
v_max = conf.v_max_scaling * model.velocityLimit
v_min = -v_max
jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot, conf.dt)
jointBoundsTask.setVelocityBounds(v_min, v_max)
priorityLevel = 0
formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds, priorityLevel, transitionTime)
```
%% Cell type:markdown id: tags:
Create a QP solver for solving the TSID QP problem.
%% Cell type:code id: tags:
``` python
solver = tsid.SolverHQuadProgFast("qp solver")
solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)
```
%% Cell type:markdown id: tags:
Run the viewer if it's not already running.
%% Cell type:code id: tags:
``` python
robot_display = pin.RobotWrapper.BuildFromURDF(conf.urdf, [os.path.join(conf.path, '../..')])
#Viewer = pin.visualize.GepettoVisualizer
Viewer = pin.visualize.MeshcatVisualizer
viz = Viewer(robot_display.model, robot_display.collision_model, robot_display.visual_model)
viz.initViewer(loadModel=True)
viz.display(q0)
```
%% Cell type:code id: tags:
``` python
hasattr(viz.viewer, 'jupyter_cell') and viz.viewer.jupyter_cell()
```
%% Cell type:markdown id: tags:
Create empty arrays to store the simulation trajectories.
%% Cell type:code id: tags:
``` python
N = conf.N_SIMULATION
tau = np.full((robot.na, N), np.nan)
q = np.full((robot.nq, N + 1), np.nan)
v = np.full((robot.nv, N + 1), np.nan)
dv = np.full((robot.nv, N + 1), np.nan)
q_ref = np.full((robot.nq, N), np.nan)
v_ref = np.full((robot.nv, N), np.nan)
dv_ref = np.full((robot.nv, N), np.nan)
dv_des = np.full((robot.nv, N), np.nan)
samplePosture = trajPosture.computeNext()
```
%% Cell type:markdown id: tags:
Specify amplitude, phase and frequency of the sinusoidal joint trajectory to track.
%% Cell type:code id: tags:
``` python
amp = np.array([0.2, 0.3, 0.4, 0.0, 0.0, 0.0, 0.0]) # amplitude
phi = np.array([0.0, 0.5 * np.pi, 0.0, 0.0, 0.0, 0.0, 0.0]) # phase
two_pi_f = 2 * np.pi * np.array([1.0, 0.5, 0.3, 0.0, 0.0, 0.0, 0.0]) # frequency (time 2 PI)
two_pi_f_amp = np.multiply(two_pi_f, amp)
two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)
```
%% Cell type:markdown id: tags:
Start simulating the system. At each loop:
* compute the reference joint sinusoidal trajectory and set it to the posture task
* compute the QP problem data using formulation
* solve the QP and get the joint accelerations
* integrate the accelerations to update the robot state
%% Cell type:code id: tags:
``` python
t = 0.0
dt = conf.dt
q[:, 0], v[:, 0] = q0, v0
for i in range(N):
time_start = time.time()
# set reference trajectory
q_ref[:,i] = q0 + amp * np.sin(two_pi_f * t + phi)
v_ref[:,i] = two_pi_f_amp * np.cos(two_pi_f * t + phi)
dv_ref[:,i] = -two_pi_f_squared_amp * np.sin(two_pi_f * t + phi)
samplePosture.pos(q_ref[:, i])
samplePosture.vel(v_ref[:, i])
samplePosture.acc(dv_ref[:, i])
postureTask.setReference(samplePosture)
HQPData = formulation.computeProblemData(t, q[:,i], v[:,i])
sol = solver.solve(HQPData)
if sol.status != 0:
print("Time %.3f QP problem could not be solved! Error code:" % t, sol.status)
break
tau[:,i] = formulation.getActuatorForces(sol)
dv[:,i] = formulation.getAccelerations(sol)
dv_des[:,i] = postureTask.getDesiredAcceleration
if i % conf.PRINT_N == 0:
print("Time %.3f" % (t))
print("\ttracking err %s: %.3f" % (postureTask.name.ljust(20, '.'), norm(postureTask.position_error, 2)))
# numerical integration
v_mean = v[:,i] + 0.5*dt*dv[:,i]
v[:,i + 1] = v[:,i] + dt * dv[:,i]
q[:,i + 1] = pin.integrate(model, q[:,i], dt * v_mean)
t += conf.dt
if i % conf.DISPLAY_N == 0:
viz.display(q[:,i])
time_spent = time.time() - time_start
if time_spent < conf.dt:
time.sleep(conf.dt - time_spent)
# PLOT STUFF
time = np.arange(0.0, N * conf.dt, conf.dt)
```
%% Cell type:code id: tags:
``` python
(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))
for i in range(robot.nv):
ax[i].plot(time, q[i,:-1], label='q %i' % i)
ax[i].plot(time, q_ref[i,:], '--', label='q ref %i' % i)
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('q [rad]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
plt.show()
```
%% Cell type:code id: tags:
``` python
(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))
for i in range(robot.nv):
ax[i].plot(time, v[i,:-1], label='v %i ' % i)
ax[i].plot(time, v_ref[i,:], '--', label='v ref %i' % i)
ax[i].plot([time[0], time[-1]], 2 * [v_min[i]], ':')
ax[i].plot([time[0], time[-1]], 2 * [v_max[i]], ':')
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('v [rad/s]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
plt.show()
```
%% Cell type:code id: tags:
``` python
(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))
for i in range(robot.nv):
ax[i].plot(time, dv[i,:-1], label='dv '+str(i))
ax[i].plot(time, dv_ref[i,:], '--', label='dv ref %i' % i)
ax[i].plot(time, dv_des[i,:], ':', label='dv des %i' % i)
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('dv [rad/s^2]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
plt.show()
```
%% Cell type:code id: tags:
``` python
(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))
for i in range(robot.nv):
ax[i].plot(time, tau[i,:], label='tau %i' % i)
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('tau [Nm]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
plt.show()
```
......
%% Cell type:markdown id: tags:
# Task Space Inverse Dynamics
%% Cell type:markdown id: tags:
In this notebook is presented a simple use of the TSID framework on a humanoid robot, Talos.
### Notations and Definitions
The robot system has a state $x$ and we denote the control inputs as $u$. <br>
The choice of the robot model depends both on the __robot__ and the **task** to perform. For legged robots the typical model of the actuators is a torque source because we will need to control the contact forces exchanged between robot and environment. Therefore the control input $u$ are motor torques $\tau$. <br>
With the robot configuration $q$ and its velocity $\dot{q}$ we have the robot state $x$:
$$ x \triangleq (q, \dot{q}) $$
%% Cell type:markdown id: tags:
### Under-Actuated Systems
A legged robot is __under actuated__, its number of actuators is less than its number of degrees of freedom (DoFs). <br>
We denotes $n_{va}$ the number of actuators and $n_v$ the number of DoFs:
$$ n_{va} < n_v $$
Assume elements of $q$ are ordered, $q \triangleq (q_u, q_a)$, where:
- $q_u \in \mathbb{R}^{n_{qu}} $ are the passive (unactuated) joints
- $q_a \in \mathbb{R}^{n_{qa}} $ are the actuated joints
Similarly, $v \triangleq (v_u, v_a)$, where $v_u \in \mathbb{R}^{n_{vu}}$ and $v_a \in \mathbb{R}^{n_{va}}$.
$ S \triangleq [\ 0_{n_{va} \ \times \ n_{vu}} \ \ I_{n_{va}}] $ is a selection matrix associated to the actuated joints:
$$ v_a = S \ v $$
The dynamic of an under-actuated mechanical system is:
$$ M(q) \ \dot{v} \ + \ h(q, v) \ = \ S^T \tau \ + \ J(q)^T f $$
where $M(q) \in \mathbb{R}^{n_v × n_v}$ is the mass matrix, $h(q,v) \in \mathbb{R}^{n_v}$ are the bias forces, $\tau \in \mathbb{R}^{n_{va}}$ are the joint torques, $f \in \mathbb{R}^{n_f}$ are the contact forces, and $J(q) \in \mathbb{R}^{n_f×n_v}$ is the contact Jacobian.
This dynamic is often decomposed into unactuated and actuated parts:
\begin{array}{r c r c l}
M_u(q) \ \dot{v} & + & h_u(q, v) & = & J_u(q)^T f \\
M_a(q) \ \dot{v} & + & h_a(q, v) & = & \tau \ + \ J_a(q)^T f
\end{array}
Where
\begin{array}{r c l}
M & = & \begin{bmatrix}
M_u \\
M_a
\end{bmatrix} \\
h & = & \begin{bmatrix}
h_u \\
h_a
\end{bmatrix} \\
J & = & [J_u \ \ J_a]
\end{array}
%% Cell type:markdown id: tags:
### Generic Output Function
An output function $y$, can be an end-effector pose (such as a gripper), the robot center-of-mass, a visual feature position inside an image.
We assume that an output is a function from the robot configuration vector $q$ to a set called $Y$:
$$y = f_{task}(q)$$
It is also assumed that $f_{task}$ is $\mathcal{C}^1$ then:
$$J = \displaystyle \frac{\partial f_{task}}{\partial q}$$
When a desired output trajectory $y^*(t)$ is given, a task error is defined as:
$$
e: \mathcal{C} \times \mathbf{R} \rightarrow \mathbf{R}^m \qquad
e = y \ominus y^*
$$
where $\ominus$ is the difference operator in the output space (it is the standard difference if the output space is Euclidian).
We can now define the desired dynamics of the task.
Usually, when the robot is controlled in velocity, the desired dynamics is set to:
$$ \dot{e}^* = - K e $$
When the robot is controlled at the acceleration level (which is our case because we assume a torque controlled robot), the desired task dynamics is typically specified with a PID control law:
$$
\ddot{e}^* = K_P e + K_D \dot{e} + K_I \displaystyle \int_{j=0}^{j=k} e(j) dj
$$
with $K_P$ the error gain, $K_D$ the derivative of the error gain, and $K_I$ the integral gain.
We can now derive the real error dynamics as:
$$ \dot{e} = \dot{y} - \dot{y}^*
= J \dot{q} - \dot{y}^*$$
therefore:
$$\ddot{e} = J \ddot{q} + \dot{J} \dot{q} - \ddot{y}^* $$
%% Cell type:markdown id: tags:
### QP Optimisation problem with acceleration and torque
The transcription of the motion reference from the task space to the whole-body control is naturally written as a quadratic program (QP). A QP is composed of two layers, namely the constraints and the cost. It can be seen as a hierarchy of two levels, the constraint having priority over the cost. Inequalities can also be taken into account directly as constraints, or indirectly in the cost function.
Let us simplify the equations of motion based on the rigid body dynamics assuming there is no contact:
$$M \dot{v} + h = S^T \tau $$
Let us assume we have a task error $e$ regulating an output function $y$.
We can introduce a slack variable $s$ (an implicit optimization variable) to represent the difference between desired and real error dynamics:
$$\ddot{e} - \ddot{e}^* \triangleq s $$
A simple formulation of the QP problem can then be expressed as:
$$ \underset{\dot{v},\tau}{\min} \quad \| s \|^2 \\
\textrm{s.t.} \quad M \dot{v} + h = S^T \tau $$
If the system is in contact with environment, its dynamic must account for contact forces $f_{ext}$. If contacts are soft, measured/estimated contact forces $\hat{f}_{ext}$ can be easily included:
$$ \underset{\dot{v},\tau}{\min} \quad \| s \|^2 \\
\textrm{s.t.} \quad M \dot{v} + h = S^T \tau + J_c^T \hat{f}_{ext} $$
But if contacts are rigid, they constrain the motion. They are usually modelled as nonlinear functions, which are differentiated twice:
$$ c(q) = 0 \, \Leftrightarrow \text{Contact point do not move}$$
$$ J_c \, v = 0 \, \Leftrightarrow \text{Contact point velocities are null}$$
$$ J_c \, \dot{v} + \dot{J_c} \, v = 0 \, \Leftrightarrow \text{Contact point accelerations are null}$$
This leads to the following optimization problem:
$$ \underset{\dot{v},\tau}{\min} \quad \| s \|^2 \\
\textrm{s.t.} \quad \Bigg[
\begin{array}{lll}
J_c & 0 & 0 \\
M & -J_c^T & -S^T
\end{array}
\Bigg] \, \Bigg[\begin{array}{l}
\dot{v} \\
f \\
\tau
\end{array}
\Bigg] \, = \, \Bigg[\begin{array}{l}
- \dot{J_c} \, v \\
-h
\end{array}
\Bigg]$$
The main benefit of QP solvers is that they can handle inequality constraints. Inequalities are mainly used to defined boundaries of the system, such as joint torque, velocity or position limits, or friction cones for the contact forces.
%% Cell type:markdown id: tags:
### Weighted Sum
Complex robots are typically redundant with respect to the main task they must perform, this redundancy can be used to execute secondary tasks. This multi-objective optimization can be achieved by setting respective weights between the tasks (strategy used in TSID), or by imposing a strict hierarchy between them (cascade of QP or HQP).
Assume robot must perform N tasks, each defined by a task function $g_i(\cdot)$:
$$ g_i = \| A_i \Bigg[ \begin{array}{l} \dot{v} \\ f \\ \tau \end{array}\Bigg] - a_i \|^2 $$
The easiest strategy is to sum all functions using user-defined weights $w_i$:
$$ \underset{\dot{v},f,\tau}{\min} \quad \displaystyle \sum_{i=0}^N{w_i \, g_i} \\
\textrm{s.t.} \quad \Bigg[
\begin{array}{lll}
J_c & 0 & 0 \\
M & -J_c^T & -S^T
\end{array}
\Bigg] \, \Bigg[\begin{array}{l}
\dot{v} \\
f \\
\tau
\end{array}
\Bigg] \, = \, \Bigg[\begin{array}{l}
- \dot{J_c} \, v \\
-h
\end{array}
\Bigg]$$
This problem remains standard computationally-efficient (compared to Least Square Programming). However, finding proper weights can be hard because too extreme weights lead to numerical issues.
%% Cell type:markdown id: tags:
# An example: CoM Sinusoidal trajectory on the robot TALOS
The goal of this exercise is to create a set of tasks allowing the tracking of a sinusoidal trajectory with the CoM of the biped robot Talos.
In the following we describe the process to create, initialise and solve the QP problem defined by the tasks.
%% Cell type:code id: tags:
``` python
# Python import needed by the exercise
import matplotlib.pyplot as plt
import numpy as np
from numpy.linalg import norm
import os
import time as tmp
# import the library TSID for the Whole-Body Controller
import tsid
# import the pinocchio library for the mathematical methods (Lie algebra) and multi-body dynamics computations.
import pinocchio as pin
# import example_example_robot_data to get the current path for the robot models
from example_robot_data.path import EXAMPLE_ROBOT_DATA_MODEL_DIR
import sys
sys.path.append('..')
# import graphical tools
import plot_utils as plut
```
%% Cell type:code id: tags:
``` python
# Definition of the tasks gains, weights and the foot geometry (for contact task)
lxp = 0.1 # foot length in positive x direction
lxn = 0.11 # foot length in negative x direction
lyp = 0.069 # foot length in positive y direction
lyn = 0.069 # foot length in negative y direction
lz = 0.107 # foot sole height with respect to ankle joint
mu = 0.3 # friction coefficient
fMin = 1.0 # minimum normal force
fMax = 1000.0 # maximum normal force
rf_frame_name = "leg_right_6_joint" # right foot joint name
lf_frame_name = "leg_left_6_joint" # left foot joint name
contactNormal = np.array([0., 0., 1.]) # direction of the normal to the contact surface
w_com = 1.0 # weight of center of mass task
w_posture = 0.1 # weight of joint posture task
w_forceRef = 1e-3 # weight of force regularization task
w_waist = 1.0 # weight of waist task
kp_contact = 30.0 # proportional gain of contact constraint
kp_com = 20.0 # proportional gain of center of mass task
kp_waist = 500.0 # proportional gain of waist task
kp_posture = np.array( # proportional gain of joint posture task
[10., 5., 5., 1., 1., 10., # lleg, low gain on axis along y and knee
10., 5., 5., 1., 1., 10., # rleg
500., 500., # chest
50., 10., 10., 10., 10., 10., 10., 10., # larm
50., 10., 10., 10., 10., 10., 10., 10., # rarm
100., 100.] # head
)
dt = 0.001 # controller time step
PRINT_N = 500 # print every PRINT_N time steps
DISPLAY_N = 20 # update robot configuration in viwewer every DISPLAY_N time steps
N_SIMULATION = 10000 # number of time steps simulated
```
%% Cell type:code id: tags:
``` python
# Set the path where the urdf file of the robot is registered
path = EXAMPLE_ROBOT_DATA_MODEL_DIR
urdf = path + '/talos_data/robots/talos_reduced.urdf'
vector = pin.StdVec_StdString()
vector.extend(item for item in path)
# Create the robot wrapper from the urdf, it will give the model of the robot and its data
robot = tsid.RobotWrapper(urdf, vector, pin.JointModelFreeFlyer(), False)
robot = tsid.RobotWrapper(urdf, [path], pin.JointModelFreeFlyer(), False)
srdf = path + '/talos_data/srdf/talos.srdf'
```
%% Cell type:code id: tags:
``` python
# Creation of the robot wrapper for gepetto viewer (graphical interface)
robot_display = pin.RobotWrapper.BuildFromURDF(urdf, [os.path.join(path, '../..')], pin.JointModelFreeFlyer())
#Viewer = pin.visualize.GepettoVisualizer
Viewer = pin.visualize.MeshcatVisualizer
viz = Viewer(robot_display.model, robot_display.collision_model, robot_display.visual_model)
viz.initViewer(loadModel=True)
```
%% Cell type:code id: tags:
``` python
# Take the model of the robot and load its reference configurations
model = robot.model()
pin.loadReferenceConfigurations(model, srdf, False)
# Set the current configuration q to the robot configuration half_sitting
q = model.referenceConfigurations['half_sitting']
# Set the current velocity to zero
v = np.zeros(robot.nv)
```
%% Cell type:code id: tags:
``` python
# Display the robot in Gepetto Viewer in the configuration q = halfSitting
viz.display(q)
```
%% Cell type:code id: tags: