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:
``` python
# Check that the frames of the feet exist.
assert model.existFrame(rf_frame_name)
assert model.existFrame(lf_frame_name)
```
%% Cell type:code id: tags:
``` python
t = 0.0 # time
# InverseDynamicsFormulationAccForce is the class in charge of
# creating the inverse dynamics HQP problem using
# the robot accelerations (base + joints) and the contact forces as decision variables
invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
invdyn.computeProblemData(t, q, v)
# Get the data -> initial data
data = invdyn.data()
```
%% Cell type:markdown id: tags:
## Tasks Definitions
A **Task** is a control objective for the robot, which is used at each control cycle to generate a **Constraint**.
Note that constraints are purely mathematical objects that are independent of the concept of robot, while Tasks are instead robot-related objects.
A **Constraint** is a linear equality or inequality.
In TSID the HQP is defined as a collection of constraints with different priority levels and weights.
There are three kinds of constraints defined in TSID:
- Equalities, represented by matrix $A$ and vector $a$:
$$ Ax = a $$
- Inequalities, represented by matrix $A$ and vectors $lb$ and $ub$:
$$ lb ≤ Ax ≤ ub $$
- Bounds, represented by vectors $lb$ and $ub$:
$$ lb ≤ x ≤ ub $$
There are three kinds of Task in TSID:
- **TaskMotion**: computes a constraint that is a linear function of the robot accelerations
- **TaskContactForce**: computes a constraint that is a linear function of the contact forces
- **TaskActuation**: computes a constraint that is a linear function of the joint torques
Tasks can compute either equality constraints, or bounds, or inequality constraints.
Examples of **TaskMotion**:
- **TaskComEquality**: computes an equality constraint to specify a desired acceleration of the center of mass (CoM) of the robot.
- **TaskJointPosture**: computes an equality constraint to specify the desired joint accelerations.
- **TaskSE3Equality**: computes an equality constraint to specify the desired acceleration for a frame attached to one of the links of the robot.
- **TaskJointBounds**: computes a bounds constraint to specify the joint acceleration bounds in order to satisfy the joint position/velocity/acceleration limits.
%% Cell type:markdown id: tags:
In this exercise, for the sinusoidal movement of the CoM, we need 3 tasks of class TaskMotion:
- **TaskComEquality** as **constraint of the control** (priority level = 0) to make the CoM follow a sinusoidal trajectory. It is the most important task so it is introduced as a constraint.
- **TaskSE3Equality** in the **cost function** (priority level = 1) for the waist of the robot, to maintain its orientation (with a reference trajectory). It is an important task so it has a weight of 1.
- **TaskJointPosture** in the **cost function** (priority level = 1) for the posture of the robot, to maintain it to half-sitting as much as possible (with a reference trajectory). It is the least important task so it has a weight of 0.1.
%% Cell type:code id: tags:
``` python
# COM Task
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(kp_com * np.ones(3)) # Proportional gain defined before = 20
comTask.setKd(2.0 * np.sqrt(kp_com) * np.ones(3)) # Derivative gain = 2 * sqrt(20)
# Add the task to the HQP with weight = 1.0, priority level = 0 (as constraint) and a transition duration = 0.0
invdyn.addMotionTask(comTask, w_com, 0, 0.0)
# WAIST Task
waistTask = tsid.TaskSE3Equality("task-waist", robot, 'root_joint') # waist -> root_joint
waistTask.setKp(kp_waist * np.ones(6)) # Proportional gain defined before = 500
waistTask.setKd(2.0 * np.sqrt(kp_waist) * np.ones(6)) # Derivative gain = 2 * sqrt(500)
# Add a Mask to the task which will select the vector dimensions on which the task will act.
# In this case the waist configuration is a vector 6d (position and orientation -> SE3)
# Here we set a mask = [0 0 0 1 1 1] so the task on the waist will act on the orientation of the robot
mask = np.ones(6)
mask[:3] = 0.
waistTask.setMask(mask)
# Add the task to the HQP with weight = 1.0, priority level = 1 (in the cost function) and a transition duration = 0.0
invdyn.addMotionTask(waistTask, w_waist, 1, 0.0)
# POSTURE Task
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(kp_posture) # Proportional gain defined before (different for each joints)
postureTask.setKd(2.0 * kp_posture) # Derivative gain = 2 * kp
# Add the task with weight = 0.1, priority level = 1 (in cost function) and a transition duration = 0.0
invdyn.addMotionTask(postureTask, w_posture, 1, 0.0)
```
%% Cell type:markdown id: tags:
## Rigid Contacts
A **Rigid Contact** is a description of a rigid contact between a body of the robot and the environment.
The main difference between a task and a rigid contact is that a rigid contact is associated to reaction forces, while a task is not.
This class allows to use different representations for the motion space and the force space (more details on this below), and it is characterize by the following four elements.
- Motion Task
- Represents the motion constraint (equality) associated to the rigid contact
- $ J_c \, \dot{v} \, = \, - \dot{J_c} \, v \, - \, K_p \ e - \, K_d \, \dot{e}$
- Force Task
- Computes the inequality constraints acting on the contact forces
- e.g., friction cone constraints : $A \, f \,\, a$
- Force Regularization Task
- Computes a regularization cost on the contact forces
- e.g., keep them close to friction cone center
- Force-Generator matrix $T$
- Maps force variables to motion constraint representation
- Dynamic: $ M \ \dot{v} \ + \ h \ = \ S^T \tau \ + \ J_c^T T f $
- Motion constraint : $ J_c \, \dot{v} \, = \, - \dot{J_c} \, v$
- Friction cones: $A \, f \,\, a$
%% Cell type:markdown id: tags:
### Contact 6d
In case of a unilateral plane contact (with polygonal shape), the motion constraint is 6d, because the body in contact cannot move in any direction.
##### PROBLEM
Minimal force representation would be 6d (3d force + 3d moment):
- It is hard to write friction constraints with 6d force representation (especially for non-rectangular contact shapes)
- A solution would be to represent the reaction force as collection of 3d forces applied at vertices of contact surface (writting friction constraints is then easy)
- But it leads to redundant representation, e.g., 4-vertex surface → 12 variables
- The redundancy is an issue for motion constraints if the HQP solver does not handle redundant constraints (as eiQuadProg).
##### SOLUTION
- Use 6d representation for motion constraint $ J_c \, \dot{v} \, = \, - \dot{J_c} \, v \, \in \mathbf{R}^6$
- But use 12d representation for force variable $f \in \mathbf{R}^{12}$
- A force-generator matrix $T \in \mathbf{R}^{6 \times 12}$ defines the mapping between the two representations:
$\tau_{c} \, = J_c^T \, T \, f$
%% Cell type:markdown id: tags:
In this exercise, we need two Rigid Contacts, one for each foot as **Contact6d**.
The Rigid Contacts are always defined as **constraints** (priority level = 0) to maintain the robot in contact.
%% Cell type:code id: tags:
``` python
# CONTACTS 6D
# Definition of the foot geometry with respect to the ankle joints (which are the ones controlled)
contact_Point = np.ones((3, 4)) * lz
contact_Point[0, :] = [-lxn, -lxn, lxp, lxp]
contact_Point[1, :] = [-lyn, lyp, -lyn, lyp]
# The feet are the only bodies in contact in this experiment and their geometry defines the plane of contact
# between the robot and the environement -> it is a Contact6D
# To define a contact6D :
# We need the surface of contact (contact_point), the normal vector of contact (contactNormal along the z-axis)
# the friction parameter with the ground (mu = 0.3), the normal force bounds (fMin =1.0, fMax=1000.0)
# Right Foot
contactRF = tsid.Contact6d("contact_rfoot", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
contactRF.setKp(kp_contact * np.ones(6)) # Proportional gain defined before = 30
contactRF.setKd(2.0 * np.sqrt(kp_contact) * np.ones(6)) # Derivative gain = 2 * sqrt(30)
# Reference position of the right ankle -> initial position
H_rf_ref = robot.position(data, model.getJointId(rf_frame_name))
contactRF.setReference(H_rf_ref)
# Add the contact to the HQP with weight = 0.1 for the force regularization task,
# and priority level = 0 (as real constraint) for the motion constraint
invdyn.addRigidContact(contactRF, w_forceRef)
# Left Foot
contactLF = tsid.Contact6d("contact_lfoot", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
contactLF.setKp(kp_contact * np.ones(6)) # Proportional gain defined before = 30
contactLF.setKd(2.0 * np.sqrt(kp_contact) * np.ones(6)) # Derivative gain = 2 * sqrt(30)
# Reference position of the left ankle -> initial position
H_lf_ref = robot.position(data, model.getJointId(lf_frame_name))
contactLF.setReference(H_lf_ref)
# Add the contact to the HQP with weight = 0.1 for the force regularization task,
# and priority level = 0 (as real constraint) for the motion constraint
invdyn.addRigidContact(contactLF, w_forceRef)
```
%% Cell type:markdown id: tags:
## TSID Trajectory
A **Trajectory** is a multi-dimensional function of time describing the motion of an object and its time derivatives.
For standard use in control, the method *compute_next* is provided, which computes the value of the trajectory at the next time step.
In the example, we need to set 3 trajectories, one for each task.
These trajectories will give at each time step the desired position, velocity and acceleration of the different tasks (CoM, posture and waist).
In our case, the posture and the waist will be constant, equal to their initial values.
For the CoM however, we will update the trajectory with a sinusoidal signal at each time step.
%% Cell type:code id: tags:
``` python
# Set the reference trajectory of the tasks
com_ref = data.com[0] # Initial value of the CoM
trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
sampleCom = trajCom.computeNext() # Compute the first step of the trajectory from the initial value
q_ref = q[7:] # Initial value of the joints of the robot (in halfSitting position without the freeFlyer (6 first values))
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
waist_ref = robot.position(data, model.getJointId('root_joint')) # Initial value of the waist (root_joint)
# Here the waist is defined as a 6d vector (position + orientation) so it is in the SE3 group (Lie group)
# Thus, the trajectory is not Euclidian but remains in the SE3 domain -> TrajectorySE3Constant
trajWaist = tsid.TrajectorySE3Constant("traj_waist", waist_ref)
```
%% Cell type:code id: tags:
``` python
# Initialisation of the Solver
# Use EiquadprogFast: dynamic matrix sizes (memory allocation performed only when resizing)
solver = tsid.SolverHQuadProgFast("qp solver")
# Resize the solver to fit the number of variables, equality and inequality constraints
solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
```
%% Cell type:code id: tags:
``` python
# Initialisation of the plot variables which will be updated during the simulation loop
# These variables describe the behavior of the CoM of the robot (reference and real position, velocity and acceleration)
com_pos = np.full((3, N_SIMULATION), np.nan)
com_vel = np.full((3, N_SIMULATION), np.nan)
com_acc = np.full((3, N_SIMULATION), np.nan)
com_pos_ref = np.full((3, N_SIMULATION), np.nan)
com_vel_ref = np.full((3, N_SIMULATION), np.nan)
com_acc_ref = np.full((3, N_SIMULATION), np.nan)
com_acc_des = np.full((3, N_SIMULATION), np.nan)
```
%% Cell type:code id: tags:
``` python
# Parametes of the CoM sinusoid
offset = robot.com(data) # offset of the measured CoM
amp = np.array([0.0, 0.05, 0.0]) # amplitude function of 0.05 along the y axis
two_pi_f = 2 * np.pi * np.array([0.0, 0.5, 0.0]) # 2π function along the y axis with 0.5 amplitude
two_pi_f_amp = two_pi_f * amp # 2π function times amplitude function
two_pi_f_squared_amp = two_pi_f * two_pi_f_amp # 2π function times squared amplitude function
```
%% Cell type:code id: tags:
``` python
# Simulation loop
# At each time step compute the next desired positions of the tasks
# Set them as new references for each tasks
# The CoM trajectory is set with the sinusoid parameters:
# a sine for the position, a cosine (derivative of sine) for the velocity
# and a -sine (derivative of cosine) for the acceleration
# Compute the new problem data (HQP problem update)
# Solve the problem with the solver
# Get the forces and the accelerations computed by the solver
# Update the plot variables of the CoM
# Print the forces applied at each feet
# Print the tracking error of the CoM task and the norm of the velocity and acceleration needed to follow the
# reference trajectory
# Integrate the acceleration computed by the QP
# One simple Euler integration from acceleration to velocity
# One integration (velocity to position) with pinocchio to have the freeFlyer updated
# Display the result with gepetto viewer
for i in range(N_SIMULATION):
time_start = tmp.time()
sampleCom.pos(offset + amp * np.sin(two_pi_f * t))
sampleCom.vel(two_pi_f_amp * np.cos(two_pi_f * t))
sampleCom.acc(-two_pi_f_squared_amp * np.sin(two_pi_f * t))
comTask.setReference(sampleCom)
sampleWaist = trajWaist.computeNext()
waistTask.setReference(sampleWaist)
samplePosture = trajPosture.computeNext()
postureTask.setReference(samplePosture)
HQPData = invdyn.computeProblemData(t, q, v)
# if i == 0:
# HQPData.print_all()
sol = solver.solve(HQPData)
if sol.status != 0:
print("QP problem could not be solved! Error code:", sol.status)
break
tau = invdyn.getActuatorForces(sol)
dv = invdyn.getAccelerations(sol)
com_pos[:,i] = robot.com(invdyn.data())
com_vel[:,i] = robot.com_vel(invdyn.data())
com_acc[:,i] = comTask.getAcceleration(dv)
com_pos_ref[:,i] = sampleCom.pos()
com_vel_ref[:,i] = sampleCom.vel()
com_acc_ref[:,i] = sampleCom.acc()
com_acc_des[:,i] = comTask.getDesiredAcceleration
if i % PRINT_N == 0:
print("Time %.3f" % t)
if invdyn.checkContact(contactRF.name, sol):
f = invdyn.getContactForce(contactRF.name, sol)
print("\tnormal force %s: %.1f" % (contactRF.name.ljust(20, '.'), contactRF.getNormalForce(f)))
if invdyn.checkContact(contactLF.name, sol):
f = invdyn.getContactForce(contactLF.name, sol)
print("\tnormal force %s: %.1f" % (contactLF.name.ljust(20, '.'), contactLF.getNormalForce(f)))
print("\ttracking err %s: %.3f" % (comTask.name.ljust(20, '.'), norm(comTask.position_error, 2)))
print("\t||v||: %.3f\t ||dv||: %.3f" % (norm(v, 2), norm(dv)))
v_mean = v + 0.5 * dt * dv
v += dt * dv
q = pin.integrate(model, q, dt * v_mean)
t += dt
if i % DISPLAY_N == 0:
viz.display(q)
time_spent = tmp.time() - time_start
if time_spent < dt:
tmp.sleep(dt - time_spent)
```
%% Cell type:code id: tags:
``` python
# PLOT the result
time = np.arange(0.0, N_SIMULATION * dt, dt)
```
%% Cell type:code id: tags:
``` python
# Position tracking of the CoM along the x,y,z axis
(f, ax) = plut.create_empty_figure(3, 1, figsize=(10, 10))
for i in range(3):
ax[i].plot(time, com_pos[i,:], label='CoM %i' % i)
ax[i].plot(time, com_pos_ref[i,:], 'r:', label='CoM Ref %i' % i)
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('CoM [m]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
plt.show()
```
%% Cell type:code id: tags:
``` python
# Velocity tracking of the CoM along the x,y,z axis
(f, ax) = plut.create_empty_figure(3, 1, figsize=(10, 10))
for i in range(3):
ax[i].plot(time, com_vel[i,:], label='CoM Vel %i' % i)
ax[i].plot(time, com_vel_ref[i,:], 'r:', label='CoM Vel Ref %i' % i)
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('CoM Vel [m/s]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
plt.show()
```
%% Cell type:code id: tags:
``` python
# Acceleration tracking of the CoM along the x,y,z axis
(f, ax) = plut.create_empty_figure(3, 1, figsize=(10, 10))
for i in range(3):
ax[i].plot(time, com_acc[i,:], label='CoM Acc %i' % i)
ax[i].plot(time, com_acc_ref[i,:], 'r:', label='CoM Acc Ref %i' % i)
ax[i].plot(time, com_acc_des[i,:], 'g--', label='CoM Acc Des %i' % i)
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('CoM Acc [m/s^2]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
plt.show()
```
%% Cell type:markdown id: tags:
As expected the robot's CoM follows the sinusoidal trajectory along the y axis (the CoM task defines a sinusoidal movement only on this axis). On the other axes there are small oscillations but the robot remains stable.
......
......@@ -216,7 +216,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.6.9"
"version": "3.5.2"
}
},
"nbformat": 4,
......
......@@ -17,9 +17,7 @@ class TsidBiped:
def __init__(self, conf, viewer=pin.visualize.MeshcatVisualizer):
self.conf = conf
vector = pin.StdVec_StdString()
vector.extend(item for item in conf.path)
self.robot = tsid.RobotWrapper(conf.urdf, vector, pin.JointModelFreeFlyer(), False)
self.robot = tsid.RobotWrapper(conf.urdf, [conf.path], pin.JointModelFreeFlyer(), False)
robot = self.robot
self.model = robot.model()
pin.loadReferenceConfigurations(self.model, conf.srdf, False)
......
......@@ -18,9 +18,7 @@ class TsidManipulator:
def __init__(self, conf, viewer=True):
self.conf = conf
vector = se3.StdVec_StdString()
vector.extend(item for item in conf.path)
self.robot = tsid.RobotWrapper(conf.urdf, vector, False)
self.robot = tsid.RobotWrapper(conf.urdf, [conf.path], False)
robot = self.robot
self.model = model = robot.model()
try:
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment