Let us consider a robot where motions are restrained to a specific workspace. Its limits are included between $\boldsymbol{X}^{min}$ and $\boldsymbol{X}^{max} \in \mathbb{R}^3$. We ommit the rotations which are a bit more complex to handle. A constraint on the robot Cartesian position is expressed as
$$ \boldsymbol{X}^{min} \le \boldsymbol{X}_{k+1}(\boldsymbol{\tau}) \le \boldsymbol{X}^{max}. $$
With $\boldsymbol{X}_{k+1} \in \mathbb{R}^3$, the end-effector position in the operational space at the next control time step. This constraint can expressed as a function of the robot joint torques using the following Taylor expansion:
$$ \boldsymbol{X}_{k+1}(\boldsymbol{\tau}) = \boldsymbol{X}_{k} + \boldsymbol{v}_{k} dt + \frac{1}{2}\boldsymbol{\dot{v}}_{k}(\boldsymbol{\tau}) {dt}^2. $$
Using the equation of motion of the robot and the derivative of $\boldsymbol{v} = J\boldsymbol{\dot{q}}$, it is possible to link the robot Cartesian position with the joint torques and express the constraint as
$$ \boldsymbol{X}^{min} \le \boldsymbol{X}_k + J_k\boldsymbol{\dot{q}}_{k}dt + \frac{dt^2}{2}\left(\dot{J_k}\boldsymbol{\dot{q}}_k+J_kM_k^{-1}\left(\boldsymbol{\tau} -\boldsymbol{g}_k - \boldsymbol{n}_k\right)\right) \le \boldsymbol{X}^{max} $$.
Finally, this constraint can be put in the form $ \boldsymbol{lb} \le A \boldsymbol{\tau}_{k+1} \le \boldsymbol{ub}$ and put inside a constrained QP :
$$ \begin{matrix} \boldsymbol{\tau}^{opt} = & \displaystyle \arg\min_{\boldsymbol{\tau}} & || \dot{v}^* - \dot{v}(\tau) ||^2_2 + \boldsymbol{R}(\boldsymbol{\tau}) \\\ & \textrm{s.t.} & A \boldsymbol{\tau} \le \boldsymbol{b} \end{matrix} $$
The following video show a simple example where a robot is required to move outisde the table, but a virtual wall prevent it from doing it. The trajectory is displayed by the green line and the moving frame. The virtual wall is in red.
Thanks to the QP formulation, the robot follows the trajectory at best during the whole experiment. When the robot reaches the virtual wall, it stops on it while the trajectory keeps on playing. When the trajectory goes down, the robot follows since the QP formulation minizes the trajectory tracking error. When the trajectory goes back inside the allowed workspace the robot accuratly tracks it again.
