"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 axis there are small oscillations but the robot remains stable. "
"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. "
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
...
...
%% 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:
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:
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.
### 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)$:
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
importmatplotlib.pyplotasplt
importnumpyasnp
fromnumpyimportnan
fromnumpy.linalgimportnormasnorm
importos
importtimeastmp
importsubprocess
# import the library TSID for the Whole-Body Controller
importtsid
# import the pinocchio library for the mathematical methods (Lie algebra) and multi-body dynamics computations.
importpinocchioaspin
importsys
sys.path.append('..')
# import graphical tools
importplot_utilsasplut
importgepetto.corbaserver
```
%% 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
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
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)
```
%%%% Output: execute_result
True
%% 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
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)
ax[i].plot(time,com_acc_des[i,:],'g--',label='CoM Acc Des '+str(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 axis there are small oscillations but the robot remains stable.
%% Cell type:code id: tags:
``` python
```
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.
