Joint limits

Joint limits task map assigns a cost for violating joint limits. The joint limits are loaded from the robot model. The mapping is calculated as:

$$\begin{split}\Phi_\text{Bound}(x) = \begin{cases} x - x_\text{min} - \epsilon, & \text{if } x < x_\text{min}+\epsilon \\ x - x_\text{max} + \epsilon, & \text{if } x > x_\text{max}-\epsilon \\ 0, & \text{otherwise} \end{cases},\end{split}$$

where $x_\text{min}$ and $x_\text{max}$ are lower and upper joint limits respectively, and $\epsilon\geq0$ is a safety margin. The Jacobian and Jacobian derivative are identity matrices.