# Difference between revisions of "ICub coupled joints"

In iCub version 1 and 2 the shoulders and the torso degrees of freedom are actuated in a coupled way. This pages serves as a reference documentation for this coupling.

We will use the notation for Joints and Motors used in iCub joints.

## Motors

Before detailing the relationships between the rotations of the motors and those about the axes of the iCub's coupled joints, we define below the sign convention associated with the rotors' rotations.

Consider the above picture and let $m$ denote the (relative) angle between the stator and the rotor. Then, the positive rotations of the rotor, i.e.

$\dot{m} > 0$

can be defined as the rotations around the axis going either inward or outward the motor (rotations around an axis obey the right-hand rule). In some cases, it is impossible to define an inward (respc. outward) direction with respect to the motor, such as the motor actuating the robot's hip. Hence, the positive rotations are defined here as the rotations about the axis associated with the torque created with a positive PWM applied to the motor. In simpler words, assume no friction between the rotor and the stator, zero initial rotor's velocity, and zero external torque. Then, the application of a positive PWM results in a rotation of the rotor, which in turn defines an axis obeying the right-hand rule. This axis is chosen as the axis of positive rotations.

## Shoulder

The mechanism of the shoulder consists in three-coupled motors that generate three degrees of freedom labeled "shoulder_pitch", "shoulder_roll", and "shoulder_yaw" (see the Figure below).

The motors associated with the degrees of freedom are shown and defined in the next picture.

Consider the right shoulder. Define

$q :=$ (shoulder_pitch, shoulder_roll,shoulder_yaw) $\in \mathbb{R}^3$

as the vector of the joint angles, and, by abusing notation,

$m :=$ (2B0M0,2B0M1, 2B1M0) $\in \mathbb{R}^3$

as the vector of angles between the stator and the rotor of each motor. Then, a simple analysis leads to the following relationship between q_dot and m_dot:

$\dot{q} = T_R \dot{m}$

where

$T_R = \begin{bmatrix} 1 & 0 & 0 \\ 1 & t & 0 \\ 0 & -t & t \end{bmatrix},$

with $t = 0,625$.

In the case of the left shoulder, the vector m ∈ R^3 consists of the relative motor angles (1B0M0,1B0M1, 1B1M0), and the associated coupling matrix T_L that relates joints' and motors' angles, i.e.

$\dot{q} = T_L \dot{m}$

is given by

$T_L = \begin{bmatrix} -1 & 0 & 0 \\ -1 & -t & 0 \\ 0 & t & -t \end{bmatrix},$

with still $t = 0,625$.

## Torso

The mechanism of the torso consists in three-coupled motors that generate three-degrees of freedom.

The modification of the angles torso_yaw, torso_roll, torso_pitch depends upon the coupled rotation of the three motors 0B4M0, 0B5M0, 0B5M1. More precisely, define

$q$  := (torso_yaw, torso_roll, torso_pitch) $\in \mathbb{R}^3$

the vector of the joint angles, and, by abusing notation,

$m$ := (0B3M0, 0B3M1, 0B4M0) $\in \mathbb{R}^3$

as the angles between the stator and the rotor of each motor. Then, a simple analysis leads to the following relationship between q_dot and m_dot:

$\dot{q} = T \dot{m}$

where

$T = \begin{bmatrix} \frac{r}{2R} & \frac{r}{2R} & \frac{r}{R} \\ 0.5 & 0.5 & 0 \\ -0.5 & 0.5 & 0 \end{bmatrix}$

Where R = 0.04 meters and r = 0.022 meters. Observe that the relationship between the (pitch,roll) angles and the motors (0B3M0,0B3M1) constitutes the classical relation found in the differential mechanism.

## From kinematic coupling into dynamic coupling

Let $\tau_q$ denote the link torques, and $\tau_m$ the motor torques. By imposing the equality between link and motor powers one has:

$\dot{q}^\top \tau_q = \dot{m}^\top \tau_m \quad \forall \dot{q},\dot m \quad \quad \Rightarrow \quad \tau_q = T^{-\top} \tau_m .$

## Recap of all coupling matrices

### Torso

$T = \begin{bmatrix} \frac{r}{2R} & \frac{r}{2R} & \frac{r}{R} \\ 0.5 & 0.5 & 0 \\ -0.5 & 0.5 & 0 \end{bmatrix}$

$T^{-1} = \begin{bmatrix} 0 & 1 & -1 \\ 0 &1 & 1 \\ \tfrac{R}{r} & -1 & 0 \end{bmatrix},$

$T^{-\top} = \begin{bmatrix} 0 & 0 & \tfrac{R}{r} \\ 1 & 1 & -1 \\ -1 & 1 & 0 \end{bmatrix},$

where $R = 0.04 [m]$ and $r = 0.022$ $[m]$.

### Right shoulder

$T_R = \begin{bmatrix} 1 & 0 & 0 \\ 1 & t & 0 \\ 0 & -t & t \end{bmatrix},$

$T_R^{-1} := \begin{bmatrix} 1 & 0 & 0 \\ -\alpha & \alpha & 0 \\ -\alpha & \alpha & \alpha \end{bmatrix},$

$T_R^{-\top} := \begin{bmatrix} 1 & -\alpha & -\alpha \\ 0 & \alpha & \alpha \\ 0 & 0 & \alpha \end{bmatrix},$

$T_R^{\top} = \begin{bmatrix} 1 & 1 & 0 \\ 0 & t & -t \\ 0 & 0 & t \end{bmatrix},$

with

$t = 0,625$

$\alpha: = \tfrac{1}{t} = 1.6$

### Left shoulder

$T_L = \begin{bmatrix} -1 & 0 & 0 \\ -1 & -t & 0 \\ 0 & t & -t \end{bmatrix},$

$T_L^{-1} := \begin{bmatrix} -1 & 0 & 0 \\ \alpha & -\alpha & 0 \\ \alpha & -\alpha & -\alpha \end{bmatrix},$

$T_L^{-\top} := \begin{bmatrix} -1 & \alpha & \alpha \\ 0 & -\alpha & -\alpha \\ 0 & 0 & -\alpha \end{bmatrix},$

with

$t = 0,625$

$\alpha: = \tfrac{1}{t} = 1.6$