===== Joint Command Limiter =====

The main purpose of the //Joint Command Limiter// is to avoid the robot hitting its joint limits and to avoid the hand hitting its own arm. Basically, once //position//, //velocity// and //acceleration// ranges are specified for any joint, the idea is to keep the robot within these ranges all the time. Imagine for example, that certain joint is going with a high speed against its mechanical limit, then the limiter's job is to know how far the joint can continue with this speed and when to start breaking so that neither the acceleration limit nor the position limit (mechanical limit) are violated.

On the other side, the limiter should interfere as least as possible with the main controller acting only when the ranges are actually going to be trespassed.

{{ :projects:velocity-position.png?nolink&400 |}}

As the previous image shows, ideally the joint limiter should allow a joint to move at its maximum velocity until a certain position **d** is reached where the joint should start braking with the maximum allowed deceleration. That way, the joint velocity should be zero by the time it reaches the **joint limit**.

==== Ranges ====

A range is an interval. It has a min and a max value (or as in RobotCheck.hh a start and end). Each joint has 3 ranges which describe allowed (good) regions in terms of acceleration, velocity and position. That is, each joint has 

<code>
RangeAcc: [acc_min, acc_max]
RangeVel: [vel_min, vel_max]
RangePos: [pos_min, pos_max]
</code>

within it should stay. pos_min and pos_max are the mechanical joint limits.

==== Hand Limits ====

For the last two joints (5 and 6 starting from 0) the Joint Limiter not only checks that the individual joint ranges are respected. Since the hand (attached to the 6th joint) could collide with its own arm before the normal joint limits are reached (such a collision depends also on the position of joint 5), the Joint Limiter needs to take care about this as well. 

{{ :projects:j6-j5.png?nolink&270 |}}

The previous image helps us to visualize this problem. It represents positions of joint 6 vs. positions of joint 5. Keeping the positions of these joints within the region delimited by the blue line will prevent self collisions. Note, that not only the positions of these joints should be kept inside ranges, but also the velocities and accelerations. The next figure illustrates the idea.

{{ :projects:hand_limits.png?nolink&300 |}}

Given a certain position in the j5-j6 plane and assuming certain velocities, an intersection point of the current trajectory and the border can be calculated. This point then determines **new** position limits for j6 and j5. These limits will change as the velocity vector in the j5-j6 plane changes.

Finally the idea is to transform acceleration and velocity ranges to future positions and check for range intersections and to determine strategies in order to limit the velocity commands. As an example, questions like "What to do if there's no range intersection?", "In which cases could that happen?", "What does mean that there is no intersection?", etc. arise when analyzing this problem.

{{ :projects:ranges_intersection.png?nolink&300 |}}

===== Intersection of allowed Joint Ranges =====

Based on the current joint state, the four
ranges position (**pos**), velocity (**vel**), acceleration (**acc**) and
braking-velocity (**brk**) are converted into the position (-increment) domain.
Thus, the range **vel** depends on the current joint position, **acc** depends
on the current joint position and -velocity and **brk** depends on the current
joint position (and sampling rate).

The range **vel** is symmetric around the current position, **acc** is
symmetric around the current position plus the current velocity. The computed
range **brk** should always contain the current position.

After these conversions, the ranges are intersected. This may lead to the
case, that one of these intersections is empty. The following cases might
occur:

|         | **pos** | **vel** | **acc** | **brk** |
| **pos** |    -    |  //1//  |  //2//  |  //3//  |
| **vel** |         |    -    |  //4//  |  //5//  |
| **acc** |         |         |    -    |  //6//  |
| **brk** |         |         |         |    -    |

  * //1// (**pos** and **vel**): Current joint position is outside and can't move inside in the next step (//Reasons: missed cycle?//)
  * //2// (**pos** and **acc**): Joint can't brake in time to stay within position limits. Due to acceleration limits, it's 'about to crash'. (//Reasons: Miscalculation of **brk**, missed cycled, we may be numerically close...//)
  * //3// (**pos** and **brk**): Current joint position is outside and braking velocity doesn't allow to move inside in the next time step. (//This should not happen! Reasons: pathological joint position limits, error in the computation of **brk**//)
  * //4// (**vel** and **acc**): The current joint velocity is so high that we can't brake into the allowed velocity range in one step. (//Reasons ???//)
  * //5// (**vel** and **brk**): The computed braking velocity is invalid: It doesn't even contain zero velocity (//This should not happen. Reason: Error in the computation of **brk**//)
  * //6// (**acc** and **brk**): The computed braking velocity falls too quickly, leading to too strong a deceleration. (//Reasons: Numerical, we might be close or we have a gross error in the computation of **brk**//)

==== Remedies ====

None of the above situations should happen 'if we did everything right', but
we can't afford this limiter to crash then. We need to complete this
specification and give one of each ranges priority.

We have two suggestions:

  - Reduce the position limits slightly, so that we have room for small numerical errors. Half a degree should be enough. This means, that we must distinguish between the real (hard) limits and the slightly smaller soft limits.
  - Modify the intersection operation to give priority to one of the ranges (either the first or the second).

{{ :projects:intersec_robotcheck.png?nolink&200 |}}

The goals are:
  - Keep the robot safe. Avoid crashing into its hard limits. Let it use it's brakes if it must!
  - Keep the controllers happy by not violating the limits.
  - Still only interfere as much as we must.


==== Evaluation of conflicts ====

  * //1//: pos_hard > vel > pos_soft: The hard position limits protect the robot and should have highest priority. But if we can bend the soft limits a little to avoid the controller stopping us, it should be fine!
  * //2//: pos_hard > acc > pos_soft: See //1//.
  * //3//: pos_hard > brk > pos_soft: In addition to the above points, the computed range **brk** might be off by an epsilon or two. This is the main motivation for having soft position limits.
  * //4//: vel > acc: There is no strong reason to prefer one ordering over the other. The KUKA controllers would have stopped us before already. Ingo prefers velocity, since computing the actual velocity is more stable (only the first derivative of position, not second).
  * //5//: vel > brk: This is catching errors in the computation of **brk**. Since **vel** is easier to compute (it is just a shifted range), it takes priority in this case.
  * //6//: acc > brk: See //5//.

==== Our priorities ====

These results lead to the following priorities:

  - pos_hard
  - vel
  - acc
  - brk
  - pos_soft

The braking velocity is computed using the soft limits.

==== TODO ====

  * check how the computation of **brk** behaves, when the joint is outside the position limits. Does it break?
  * What is the behaviour when the robot is outside the hand position limits? We see two options:
    - Move back in the direction where we left (definitely stable, but requires extra state. If the current desired position can be pushed around, the it may lead to undesired behaviour)
    - Move to the closest limit (might become unstable with pathological hand limits, but doesn't require extra state)