# Difference between revisions of "Shoulder range of movements"

Some iCub users have been complaining about the shoulder limited range of movements when reaching for objects above the torso level. The range of movement is primarily limited by the shoulder tendons length. Unfortunately, constraints imposed by tendons cannot be written in the form [theta_min, theta_max] (the constraints currently implemented in the firmware) and therefore the current solution is over-restrictive (the range of motion has been reduced in order to be sure that we do not exceed the tendon length). 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.

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
```

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

```    [  b, -b,  0  |           [ 404.0 ]
| -1,  1,  0  |           |  54.3 |
| -b,  b,  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 |
|  b, -b, -b  |           | 431.0 |
| -1,  1,  1  |           | 101.7 |
| -b,  b,  b  |           | 109.0 |
|  1, -1, -1  |           | 258.3 |
|  0,  1,  1  |           |  71.7 |
|  0,  0, -1  |           | 120.0 |
|  0, -1, -1  |           | 228.3 |
|  0,  0,  1  ]           | 120.0 ]
```

Clearly these inquality constraints 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 we have:

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