# Difference between revisions of "Shoulder range of movements"

The range of movements of the arm is constrained by the length of the tendons. Unfortunately, these constraints cannot be written in the form [theta_min, theta_max] (the constraints currently implemented in the firmware), therefore care must be taken when controlling the arm in joint space to avoid reaching configurations that may break the tendons.

Note that the Cartesian controller for this reason includes safety constraints that restrict movement when reaching for objects above the torso level (as reported by many iCub users).

In summary the current situation can be improved in two aspects:

1. Modifying the firmware so as to handle tendon length constraints;
2. Studying the problem of optimizing tendon lengths in order to increase the manipulation workspace.

We provide below the constraints imposed by the tendons (and that are enforced by the Cartesian Controller) and the ones currently imposed by the firmware.

## Correct limits

Each tendon imposes the following constraint on the joint positions:

```[a1 a2 a3] .* [theta0 theta1 theta2] > c
```

where theta0, theta1 and theta2 correspond to the positions of the three shoulder joints (joint0, joint1 and joint2 of the left_arm and right_arm). All the inequalities imposed by the tendons can be grouped in the following matrix inequality:

``` A * q + b > 0
```

where q = [theta0, theta1, theta2] the matrix A and the vector b which have the following numerical values:

```    |  c, -c,  0  |           | 404.0 |
| -1,  1,  0  |           |  54.3 |
| -c,  c,  0  |           |  46.0 |
|  1, -1,  0  |           | 305.7 |
|  0, -1,  0  |           | 215.7 |
|  0,  1,  0  |           | 150.0 |
|  0,  1,  0  |           |  54.3 |
A = |  0, -1,  0  |      b =  | 210.0 |
|  c, -c, -c  |           | 431.0 |
| -1,  1,  1  |           | 101.7 |
| -c,  c,  c  |           | 109.0 |
|  1, -1, -1  |           | 258.3 |
|  0,  1,  1  |           |  71.7 |
|  0, -1, -1  |           | 228.3 |
```

with c = 1.7105.

## Current firmware limits

Clearly the inquality described above cannot be expressed in the form:

```theta_min < theta < theta_max
```

which is the current description for the joint limits implemented in the firmware.

At the moment the firmware constraints the joints to remain within:

``` -95.5 < theta0 < 10.0
0.0 < theta1 < 160.8
-37.0 < theta2 < 80.0
```

which are *not* guaranteed to avoid the cable length limits (represented by A and b). The iKin based modules (along with the Yarp cartesian interface) and the cableLengthGuard module however can be used to prevent the robot from reaching the cable lenght limits.