ICub coupled joints

From Wiki for iCub and Friends
Revision as of 16:45, 7 January 2015 by Randaz (talk | contribs) (Right shoulder)
Jump to: navigation, search

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.

MotorSignConvMHQ.png

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).

ShoulderWithDoF.png

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

Motor&BoardPlacementHQ.png

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.

MainGeometry.png

DefinitionDoFs.png


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.

CouplingNatureTorso.png

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