reacting to dynamic events implied in the considered collaborative scenarios. Instead one needs to consider online re-planning at the task level.

MPC is capable of combining both online re-planning and prediction of the control action over a horizon. It offers a convenient framework to optimally and interactively exploit allowed motion constraints while respecting the robot capacities and safety constraints.

As a consequence, this chapters implements a hierarchical cascade of closed-loops controllers in a similar way to [Lober et al., 2020], combining both online trajectory re-planning over a receding horizon and task space-control in two stages: a high-level safety-aware operational space MPC and a high-frequency lower-level task-to-joint-space controller.

An architecture schematic is shown at the bottom of Figure 3.5. A third block is in charge of processing the environment information and translating this into safety-aware constraints and tasks that can be used during re-planning. The task-space re-planning is performed by an MPC described in Section 3.3.5 and the high-frequency joint-space controller (an inverse velocity kinematics solver) is described in Section 3.2.

More specifically, the focus of this section lies in the constrained online re-planning: continuously finding the optimal pose and twist trajectory that will drive a system from an initial pose to some target pose, subject to velocity and acceleration limits that account for safety and the real robot capacities. While this is not a fully novel problem, predictive algorithms in the literature:

* often use non-linear solvers [Kleff et al., 2021]; [Sathya et al., 2020] which may not be appropriate for frequent re-planning;

* are formulated at the joint-space level [Nicolis et al., 2020]; [Incremona et al., 2017] which is not ideal when considering task-space specification as a central feature;

* mostly disregard orientation.

By leveraging the relation between the pose space (the Lie Group $SE(3)$) and its tangents spaces (in the Lie algebras $sc(3)$) [Saccon et al., 2013]; [Torres Alberto et al., 2022a]; [Forster et al., 2015], this work proposes a Linear MPC capable of estimating the evolution of the position and orientation with fast Quadratic Program (QP) solvers.

Figure 3.6 shows an example horizon computed by the MPC towards one of the target poses. It exemplifies that the MPC only plans over a limited time horizon (starting from the left). As the robot moves towards the target pose, the MPC computes a new trajectory for each horizon according to the new robot state (middle image), until it reaches the target pose (on the right).

### 3.4.2 Numerical Simulation ↑

To showcase the result of a task-space MPC employed for online re-planning, a numerical simulation was constructed for a simple Cartesian motion along one of the
<figure><img src="image_26.png" /><figcaption>Figure 3.6: The planned horizon in time. The online MPC progressively “discovers” a path from the initial pose (on the left) towards the target pose (on the right). Given the MPC optimizes over a receding time window, the optimizing horizon does not cover the complete path until it is close enough to the target pose.</figcaption></figure>

axis of the reference frame attached to the robot base (x in this case). While the experience might seem trivial, it emulates the operation of the MPC by re-planning at a constant rate. It was executed using the full implementation of the algorithm even though only the axis in question actually performs a motion (as intended by the design of the experiment).

The example output of the MPC control loop is shown in Figure 3.7. The prediction horizon is 75ms composed of 15 time steps of 5ms each. The update rate of the MPC is 50Hz thus implying an interpolation of the first computed time-step as the Cartesian space state is updated at 1kHz. For an intuitive comparison, the resulting trajectory is compared to a time optimal trapezoidal acceleration profile computed once using Ruckig [Berscheid et al., 2021b].

Unlike the globally planned trajectory which includes jerk bounds, the current formulation of this MPC does not embed jerk limitations, yielding some large acceleration variations. Yet, the resulting profile is very similar and demonstrates the ability of MPC to obtain quasi-time-optimal trajectories while conferring online adaptation capabilities to the overall architecture. Moreover, while Ruckig is in practice a decoupled multi-dofs planner, the proposed approach truly accounts for 3D motions in both position and orientation.

### 3.4.3 Experimental Scenario & Safety Constraints ↑

The setup was constructed with a collaborative scenario in mind (shown in Figure 3.8): the robot is placed on a table and requested to cycle over a series of poses on repetition. Meanwhile, to depict a safety concern, the operator approaches the robot and the robot operating velocity is modulated according to the distance. The objective of the following experiment is to show the controller ability to

1. find a trajectory towards arbitrary target poses while respecting constraints;

2. adapt to changing constraints online.

To do so, a 7-DoF Panda robot from Franka Emika is requested to move to four different targets. These targets are not known in advance by the MPC but rather given on-the-fly.
<figure><img src="image_27.png" /><figcaption>Figure 3.7: Numerical simulation of the replanning controller for 1-Degree of Freedom (DoF). This example shows the result of employing the MPC to replanify at 50Hz and apply the resulting twists for a Cartesian motion along one of the axis (x from the frame attached to the robot base). To compare optimality, the output is compared to the jerk-bounded algorithm in [Berscheid et al., 2021b].</figcaption></figure>

For this experiment, a cascade controller as shown in Figure 3.5 is implemented. This schema also shows the working frequency of each controller and the feedback flow. The MPC implemented uses OSQP [Stellato et al., 2020] as a QP solver. The joint velocity controller used for this experiments is available online<fnref n="10" /> and uses qpOASES [Ferreau et al., 2014]. The robot model is computed using the Pinocchio library [Carpentier et al., 2019]. The robot control architecture is implemented using the Robot Operating System (ROS) framework and ran in real-time at a frequency of 1kHz using the frankaRos library.

<figure><img src="image_28.png" /></figure>

video/link

<figure><img src="image_29.png" /><figcaption>Figure 3.8: Velocity modulation experimental setup. The photo shows the setup. The robot is placed on a table to perform a task provided on-the-fly. The distance to the operator *d*<sub>h</sub> used for safety-aware velocity modulation is measured via the laser sensor.</figcaption></figure>

<footnote n="10">¹⁰The code used for the controller is hosted at gitlab.inria.fr/auctus-team/components/robots/panda/panda_qp_control. That said, future versions of the controller will be integrated in the qontrol library at gitlab.inria.fr/auctus-team/components/control/qontrol.</footnote>
When solving the MPC problem for a new target pose, computation times vary a lot depending on the amount of steps and constraints in the horizon. Moreover, the first solving step (cold start) always takes more time. For a typical horizon of $h = 10$, $\Delta t_h = 50$ms OSQP solves the problem in under 10ms. The solution of the previous QP is used as a first guess for the next one (hot start). By assuming that the optimal solution between two resolutions closely resembles the previous one, the resolution process can be accelerated. This leads to computation times under 1ms with a warm start.

## Safety constraints

To showcase the application of the MPC with safety in mind, it was employed for velocity modulation. The velocity constraints are adapted according to the distance between the operator and robot and provided to the MPC online. Notice that this implies that the constraints in (3.49) are changing over time.

The constraint modulation in this scenario is chosen for its simplicity but it shows how to potentially integrate arbitrarily complex safety behaviours as velocity modulation functions, such as, for example, dynamic risk assessment strategies based on probabilistic scenarios [Z. Liu et al., 2020]; [Mayyas et al., 2020], the robot stopping distance [Joseph et al., 2020] or damage-based maximum velocities [Mansfeld et al., 2017].

The distance between the end-effector and the human is calculated as $d_h = ||\mathbf{p}_{ee} - \mathbf{p}_h||^2$ (where $\mathbf{p}_{ee}, \mathbf{p}_h$ respectively refer to the end-effector and the human position). A simple piece-wise function is employed for each component of $\mathbf{u}_M$ designated $u_{M,i}$ for $i = 1, \dots, 6$ (to modulate both the linear and angular velocities):

$$ u_{M,i}(d_h) = \begin{cases} V_m & d_h \le d_m \\ \frac{d_h-d_m}{d_M-d_m} V_M & d_m < d_h < d_M \\ V_M & d_h \ge d_M \end{cases} \qquad (3.52) $$

that has user defined extreme values for the minimal and maximal distances $d_m, d_M$ and velocities $V_m, V_M$; all greater than zero.

This function ensures a linear relation between maximum velocity and the distance inside the distance interval; outside of this range $\mathbf{u}_M$ is assigned either the minimal or the maximum velocity. The minimum velocity $V_m$ is chosen to be close to zero to completely stop the robot motion when the distance is below the threshold $d_m$.

The robot Cartesian velocity is constrained using (3.52). The distance between the operator and the robot is determined by a Hokuyo Laser range finder placed at the robot base, as shown in Figures 3.4 and 3.8 (in the real setup); and Figure 3.6 (in the simulation). The following values are used to modulate the robot velocity according to (3.52):

$$ \begin{aligned} V_{m,\text{lin}} &= 0.01m.s^{-1} & V_{m,\text{ang}} &= 0.01s^{-1} & d_m &= 0.2m \\ V_{M,\text{lin}} &= 1m.s^{-1} & V_{M,\text{ang}} &= 1.5s^{-1} & d_M &= 1m \end{aligned} \qquad (3.53) $$

In an industrial scenario, these values would be determined by the type of human-robot collaboration and technical specification used, such as the ISO TS 15066.
### 3.4.4 Results ↑

This section shows the results from periodically requesting the robot to move to a different pose. Figure 3.9 presents the experiments performed on the Panda robot, including

1. the error between the current robot pose and the target pose;

2. the body twist used during the trajectory, as well as the maximum available velocities.

Result 1) shows the error between the current position $p_k \in \mathbb{R}^3$ and orientation of the robot $R_k \in \mathcal{SO}(3)$ and the target ones. There is a spike every time a new pose is requested. It is important to highlight that this is not the resulting tracking error of following the trajectory planned with the MPC but rather the “distance” between the current pose and the objective.

The following formula was used to compute this error:

$$e = p_k - p_t \qquad \phi = \log(R_k^{-1} R_t) \qquad (3.54)$$

where $p_t, R_t$ denote the target position and orientation, respectively; $e \in \mathbb{R}^3$ reflects the position distance and $\phi \in \mathcal{SO}(3)$ is the orientation error.

For result 2), the velocity curves show the body twist $v = [v^T, \omega^T]^T$ as well as the available velocity (in a red shade) that is being scaled by the distance between the robot end-effector and the human.

The main highlight of these results is that the MPC is able to modulate velocities according to constraints changing in real-time achieving the two main objectives of this work:

1. when the human is far, the algorithm is able to saturate the velocity, maximizing the robot movement performance;

2. while the human approaches, the robot movement is handicapped until it no longer moves.

A subsidiary result of this MPC formulation is that it allows changing the robot target position on the fly, overriding the current target. The resulting robot motion keeps being smooth since it respects an acceleration-bounded profile as shown in Figure 3.9. More details can be observed in the attached video (see Figure 3.10).

## 3.5 Conclusion ↑

This Chapter proposes an efficient linear Model Predictive Control (MPC) approach that can deal with the online planning of SE(3) motion in task space. The main features of the proposed control approach lie in its ability to generate optimal Cartesian motion which dynamically accounts both for targets updated on-the-fly and evolving motion constraints. This receding horizon approach endows the robot with the ability to interactively adapt to its environment. More particularly the safety of a human operator sharing its workspace with the robot can be accounted for through the sensor-based adaptation of maximum velocity constraints.
<figure><img src="image_30.png" /><figcaption>Figure 3.9: Shows the result of requesting the robot to move between 4 preset poses. While the robot moves, a laser sensor is used to capture the minimal distance to a human and modulate the maximum velocity (shown in red shade). The error curves show the distance between the current position and orientation ($p_k, R_k$) and the preset target poses ($p_t, R_t$). The linear error is $e = p_t - p_k$, while the angular error is computed as $\phi = \log(R_k^{-1}R_t)$. The MPC (T=300ms, $h = 10$, $\Delta_h = 30$ms, $f_{MPC} = 50$Hz) modulates the body twist $v = [v^T, \omega^T]^T$ respecting the safe limits imposed even when they become so low that the robot stops moving.</figcaption></figure>
The chapter proposes a velocity-based MPC formulation for acceleration-bounded trajectory planning. This allows formulating constraints up to the acceleration level but presents two big limitations: the resulting trajectories is not smooth on the acceleration level; the MPC cost function is very sensitive to the regularisation task.

The next chapters of this work will focus on an acceleration-based formulation that
allows incorporating jerk bounds to obtain smoother trajectories. At the same time,
they focus on formulating the linear constraints in the MPC for the Cartesian pose
state, to limit the effective position and orientation space.

Furthermore, the current experiment was implemented with explicit equality con-
straints in the *Quadratic Program (QP)* solver. This adds a computation complexity
to the solver that results in slower convergence. The following chapters show an im-
proved version that employs a compressed matricial, speeding up resolution times.

<figure><img src="image_31.png" /><figcaption>Figure 3.10: QR code for the video of the velocity modulation experiment. On PDF file readers, it is also a clickable hyperlink.</figcaption></figure>
# Chapter 4

## Linear Model Predictive Control on SE(3): Acceleration-based

Robots require the ability to autonomously, continuously and smoothly react to unexpected online changes in the task definition and in the environment, especially those cohabited with humans. To react to these changes, the task, from the current state up to the finish, must instantly be re-considered. This implies a prohibitive re-computation cost.

This Chapter proposes a modular control architecture based on Model Predictive Control, that offers a good compromise between optimally achieving the task and the required computation time, by only re-considering the near future. This framework offers a generic way to formulate task-related objectives and constraints that dissociates the planning from the execution, which depends mainly on the robot dynamics. The proposal exploits a linear formalization of the Model Predictive Control (MPC) in SE(3) to implement this architecture in a high-frequency closed-loop controller, achieving task re-planning at the control rate.

The pertinence of the proposed control architecture is demonstrated using experiments with the Franka Emika robots in scenarios where the task to be achieved is modified on the fly.
# Contents

<table><tr><td>4.1</td><td>Introduction</td><td>77</td></tr><tr><td>4.1.1</td><td>Robots in Changing Environments for Dynamic Tasks</td><td>78</td></tr><tr><td>4.1.2</td><td>Control Architecture</td><td>79</td></tr><tr><td>4.2</td><td>Acceleration-based Task-space Tracking</td><td>81</td></tr><tr><td>4.2.1</td><td>Dynamic Robot Model</td><td>81</td></tr><tr><td>4.2.2</td><td>Inverse Dynamics Controller as a Quadratic Program</td><td>82</td></tr><tr><td>4.2.3</td><td>Inverse Dynamics Constraints</td><td>83</td></tr><tr><td>4.3</td><td>MPC-based motion re-planning over a receding horizon</td><td>84</td></tr><tr><td>4.3.1</td><td>Reduced Formulation</td><td>87</td></tr><tr><td>4.3.2</td><td>Dynamics-based MPC in the Tangent Space</td><td>88</td></tr><tr><td>4.3.3</td><td>Interpolating the Horizon Trajectory</td><td>91</td></tr><tr><td>4.4</td><td>Experimental Validation: Online Re-planning at the Control Rate</td><td>92</td></tr><tr><td>4.4.1</td><td>Experimental Procedure &amp; Setup</td><td>92</td></tr><tr><td>4.4.2</td><td>Analysis</td><td>95</td></tr><tr><td>4.5</td><td>Discussion</td><td>99</td></tr><tr><td>4.6</td><td>Conclusion</td><td>100</td></tr></table>
## 4.1 Introduction

The following sections present the rationale behind the task adaptation in interactive and collaborative scenarios from a dynamics modeling point of view. The proposal is to make a slight adjustment to the control architecture presented in Chapter 3, exploiting a more efficient mathematical formulation of the constrained motion re-planning problem.

Section 4.2 focuses on the modeling and control assumptions of the robot. In order to start considering motion dynamics for collaborative tasks, these aspects must also be embedded in the robot model. With this objective in mind, the kinematics-based approach presented before is extended to control a robot through the joint torques. This model is then used for task-space tracking via a Quadratic Program (QP).

Furthermore, Section 4.3 focuses on the mathematical formalisms that allow continuous point to point pose trajectory re-planning. So far, Chapter 2 presented the algebraic formulation that allows for pose integration in the manifold; afterwards, Chapter 3 employed these principles to obtain a linear form that can be employed in QP optimization in a Model Predictive Control (MPC) control scheme. These allows constrained point to point motion generation over a gliding time horizon. This chapter extends this to higher order motions to further improve the motion smoothness, achieve higher frequency re-planning and extend the strategy to be able to include the dynamics of collaborative tasks.

Finally, Section 4.4 presents an experimental validation of the control scheme. It introduces a real-world implementation of the architecture along with the experimental setup. The experiment focuses on achieving smooth task transitions to showcase the online adaptation in unplanned interactive tasks.

Collaborative robotics has increasingly gained interest over the last decades. From an application point of view, it crystallises early expectations [Makinson, 1971]; [J. Edward Colgate et al., 2003] around the improvement of human working conditions [Maurice et al., 2017]; [Schoose et al., 2022]. From a research stand point, it raises several essential questions, for example regarding the prediction of human motion [Mainprice et al., 2015] or the underlying cognitive principles behind the concept of collaboration [Chen et al., 2021].

Among these research questions, the more general one of controlling robots in dynamic environments<fnref n="1" /> is a recurring yet unsolved problem. A large amount of literature has been dedicated to this problem but the most recent works in the domain of collaborative robotics have focused on human avoidance [Eckhoff et al., 2022]; [Merkkaert et al., 2022]. Another essential feature when considering dynamic environments is the general ability to autonomously, continuously and smoothly adapt to unplanned task updates. While Chapter 3 focuses on a control approach for continuous task adaptation, this Chapter proposes an extension that more comprehensively tackles the problem to achieve smoother reactive behaviors from the robot.

<footnote n="1">¹as opposed to static ones where robots are physically separated from anything that could provoke a change in either the task or the surrounding objects during execution</footnote>
4.1.1 Robots in Changing Environments for Dynamic Tasks ↑

Given the existence of mechanisms for updating the robot current state in relation to
its environment and task objective (i.e. perception) in real-time, the most straightfor-
ward approach to grant robots with the ability to adapt is through real-time adjust-
ments in their movement planning to reach the desired goal [Palleschi et al., 2021].

Actually, while it is essential to find a viable path towards the target and continu-
ously update it in order to have any possibility of accomplishing the task, the re-
consideration of the temporal aspects of the motion from the beginning to the end is
debatable.

Indeed, it is unlikely for the entire pre-computed trajectory to be successfully ex-
ecuted in the presence of unforeseen changes to the task. Additionally, despite ad-
vancements in motion planning [Coleman et al., 2014] and optimal trajectory compu-
tation [Pham et al., 2018], the online adjustment of both the path and the time profile
throughout the entire motion remains an extremely challenging problem [Palleschi
et al., 2021]. This difficulty becomes even more pronounced when the planning ap-
proach involves computing a trajectory for control inputs.

Re-planning over a receding horizon presents a viable alternative [Ghazaei Ardakani et al., 2019] to ensure online computational feasibility of the control problem while striving for optimal task performance. This approach represents a compromise between methods that compute an optimal trajectory once offline and those that adapt to environmental changes without fully revising the whole trajectory [Joseph et al., 2020].

Another consequence of robots operating in dynamic environments is the inability
to verify in advance whether all constraints will be satisfied, including those assos-
ciated with the task and inherent robot capabilities. In contrast, on-line re-planning
provides some level of guarantee that the generated motion will not lead constraint
violations although it is unlikely to help in the event of unforeseen events occurring
during motion execution.

As a consequence, situations may arise where emergency stops become the sole re-
course to prevent accidents. To circumvent such control exceptions, controllers need
to be designed as optimization problems that explicitly incorporate constraints in
the resolution [M. Liu et al., 2015].

MPC appears as a good candidate due to its capability to perform optimal motion re-planning based on the current system state (i.e. in closed-loop fashion) while taking into account some constraints. In fact reasoning over a receding horizon not only achieves local optimality but also implicitly ensures compatibility among the set of constraints. This approach is more likely to succeed compared to methods that attempt to address constraint compatibility offline [Rubrecht et al., 2010]; [Meguenani, 2017]; [Prete, 2018].

Assuming that MPC can keep up with the robot control rate [D. Kouzoupis et al., 2015]; [Dimitris Kouzoupis et al., 2018], one can rely on a control architecture where the joint velocity or torque control input horizon is re-computed at each control time step and fed to the robot, based on a given operational space target.
<figure><img src="image_32.png" /><figcaption>Figure 4.1: The proposed modular control architecture is based on a cascade loop composed of: 1) a task-space Model Predictive Controller (MPC) for online re-planning; 2) a constrained Task Space Inverse Dynamics solver to follow the planned trajectory. The whole control loop runs synchronously at the control rate.</figcaption></figure>

revisited ↑ ↓

While Chapter 3 proposed a kinematic approach to the online re-planning problem-
atic, the interactive nature of collaborative tasks enforce the need for forces to be
considered in the architecture. Yet, to do so, this approach must be extended to
account for the dynamics.

This is proposed in [Kleff et al., 2021] where MPC is performed through Differential Dynamic Programming which can be solved efficiently [Mastalli et al., 2020] at each control step over a receding horizon.

This approach couples two underlying sub-problems and solves them together: 1) the computation of an operational space trajectory towards the target and 2) the resolution of the operational to joint space mapping. Even though constraints are implemented in the cost function and cannot be formally guaranteed, this approach presents the advantage of considering joint and actuation constraints directly at the MPC problem level. As a consequence, except for the joint torque servoing performed at the actuation level, no inner control loop is required.

Yet, alternative approaches can be considered where an MPC is solved at the oper-
ational space level to incrementally generate a feasible task-space trajectory given
some task-related constraints [Faroni et al., 2019]; [Eckhoff et al., 2022]. The output
of the MPC is then fed to a joint level controller which can, for example, solve for the
joint torques at each control instant using a constrained optimization approach [M.
Liu et al., 2015]. This approach does not allow to account for the joint level limits
of the robot at the MPC level. However, it provides a modular architecture closing
the loop on the current operational state of the robot, independently from the nature
of the controller provided with a given robot. The later approach is retained in this
work.

4.1.2 Control Architecture ↑

The scenario presented above builds upon the one introduced in Chapter 3. Namely, it considers the same collaborative work cells wherein robots and humans share the workspace and the events to which the robot must react. It also shares the same task-centric design philosophy that allows decoupling task planning from task execution, achieving higher modularity from the controller point of view as well as from the components choice: a workspace planner can be utilized with multiple robots. Moreover, re-purposing a robot does not require re-designing a task planner. In contrast, joint-space planning is coupled to the robot architecture and, in the same
way, task planning in the joint space can hardly be straightforwardly reused when
changing robot architecture.

Beyond that, the architecture must also consider the interactive nature of collabora-
tive tasks. In fact, these kind of tasks often involve physical interactions with the
environment and human operators. In such case, the robot must not only re-actively
adapt to its surroundings but it must also properly act upon it in a measured and
coherent way.

In fact, the inter-activeness of these scenarios imply that the robot is capable of per-
forming tasks that consider the acting forces and torques to achieve an objective.
Two main challenges arise from this scenario that cannot be properly addressed by
the architecture proposed in Chapter 3:

* For one, the robot must consider the dynamics of the interactions

* Secondly, the closed feedback loop needs to be highly reactive

Indeed, the first challenge accounts for the fact that in order to properly consider the acting forces between a robot and its environment (and / or humans), the control models must be extended for higher order motions, i.e. dynamics. This offers a way to better account for inertial effects while also relating the motion to the actual acting quantities.

Furthermore, the asynchronous design choice of the proposed architecture in Chap-
ter 3 is a workaround to the heavy computing requirements of motion planning.
Nevertheless, when controlling a motion at the dynamics level, a faster and more
reactive controller is desired.

This directly connects to the second challenge: the computational demands of con-
sidering more complex models that account for dynamics. In fact, the faster the
control architecture the better: the closed feedback loop is able to better adapt to
what is happening at a higher frequency.

The control architecture drafted here and summarily described on Figure 4.1 consists
of two main components:

* The first one is a linear *MPC*, formulated at the operational space level. It generates an optimal trajectory in $SE(3)$ over a receding horizon, given a reference target and constraints on the admissible motion, both provided on the fly. These constraints can be both related to the real joint space motion capabilities of the robot and to restrictions relative to the performed task.

* The second one computes the robot control input leading to the execution of the planned trajectory while respecting the joint-level constraints using an inverse dynamics solver.

These two components are described hereafter. In all, the architecture enables the
robot to re-plan a trajectory at the control frequency of the robot while accounting
for pose constraints on the end-effector up to the jerk order.
## 4.2 Acceleration-based Task-space Tracking ↑

The previous chapter presented an inverse kinematics solver as a Quadratic Program (QP) to provide task-space tracking at the kinematic level. This section extends the previous strategy to formulate task-space tracking with an inverse dynamics solver.

In order to control the robot at the acceleration level, one must first extend the kinematic model introduced in Section 3.2.1 to consider the joint accelerations and torques. This is done in Section 4.2.1. Then, a proper torque-based QP cost function for tracking is presented in Section 4.2.2. Finally, the constraints are formulated in Section 4.2.3.

Given the interpolated first step of the trajectory computed by the Model Predictive Control (MPC), task space inverse dynamics has to be performed to compute the input joint torque for the robot.

Feeding the inverse dynamics solver with the desired acceleration computed by the MPC may sound tempting. Nevertheless, the feedback provided by the closed-loop MPC is, partly due to the interpolated output, not efficient enough to properly reject tracking errors related to the inaccuracies in the model of the robot.

These inaccuracies are mostly related to dry friction at the joint level, imperfectly rejected by the lower-level torque control loop in most robots. As a consequence, a Proportional Derivative (PD) controller including a feed-forward term in acceleration is used to compute a corrected desired acceleration.

### 4.2.1 Dynamic Robot Model ↑

The dynamical model of a manipulator is one that describes the interaction between the joint torques and external contact forces to produce a joint acceleration $\ddot{q} \in \mathbb{R}^n$ (see the Forward Dynamics Section 8.5 of [Lynch et al., 2017] for the joint-space formulation and Section 3.3 of [Siciliano et al., 2008], for the task-space one):

$$ \ddot{q} = M(q)^{-1}(\tau - J^T(q)f - B(q, \dot{q})) \qquad B(q, \dot{q}) = C(q, \dot{q}) + G(q) \quad (4.1) $$

where $\tau \in \mathbb{R}^n$ denotes the torques generated by each joint; $J^T(q)f$ corresponds to the joint torques resulting of applying an external wrench $f \in \mathbb{R}^6$; $M(q) \in \mathbb{R}^{n \times n}$ corresponds to the joint space inertia matrix; and the $B(q, \dot{q}) \in \mathbb{R}^n$ term is referred to as the *bias forces*, comprised of the effects of the Coriolis and centrifugal forces in $C(q, \dot{q})$ and the gravity in $G(q)$.

A dynamics-based task-space control formulation requires linking the task-space acceleration (see Section 8.5.1 of [Siciliano et al., 2008]) with the joint accelerations from (4.1). The link between both is visible once the time derivative of the twist $\dot{v} \in \mathfrak{SE}(3)$ in (3.6) is considered:

$$ w \nu = w J(q) \dot{q} \qquad (4.2) $$

revisited ↑ ↓

$$ w \dot{v} = w J(q) \ddot{q} + w \dot{j}(q, \dot{q}) \dot{q} \qquad (4.3) $$
and now $\ddot{q}$ can be replaced with (4.1). The kinematic model assumes actuation limits on the joint configuration bounds and joint velocities:

$$ 
\begin{aligned}
Q &= \{q \in \mathbb{R}^n \mid q_m \le q \le q_M\} \\
\dot{Q} &= \{\dot{q} \in \mathbb{R}^n \mid \dot{q}_m \le \dot{q} \le \dot{q}_M\}
\end{aligned}
\quad (4.4) \quad \text{revisited } \uparrow \downarrow
$$

These need to be extended to account for the joint torques:

$$ \mathcal{T} = \{ \tau \in \mathbb{R}^n \mid \tau_m \le \tau \le \tau_M \} \quad (4.5) $$

which can be even further extended in case one needs to consider the torque variation limits:

$$ \dot{\mathcal{T}} = \{ \dot{\tau} \in \mathbb{R}^n \mid \dot{\tau}_m \le \dot{\tau} \le \dot{\tau}_M \} \quad (4.6) $$

This would effectively impose varying limits on the joint jerks; as can be seen by taking the time derivative of (4.1).

### 4.2.2 Inverse Dynamics Controller as a Quadratic Program ↑

Task-space tracking focuses on the problem of computing the optimal control input to achieve a certain end-effector motion. The kinematics-based strategy solves for the joint velocities whereas a dynamics-based solver calculates the joint accelerations. The first approach focuses on the geometric view of the problem, ignoring the required forces or torques. In contrast, the inverse dynamics strategy can formulate the accelerations as a function of the joint torques and external forces acting on the end-effector. This accounts for the system dynamic properties such as the mass, inertia, external forces, etc.

The desired motion for an end-effector is parameterized with reference pose trajectory $\mathcal{X}^r(t) \in SE(3)$. Once discretized, this yields a reference pose, twist and twist derivative (also referred to as spatial acceleration) to be achieved at the current time step k: $\mathcal{X}_k^r = \mathcal{X}^r(t_k)$, $v_k^r = v^r(t_k)$, $\dot{v}_k^r = \dot{v}^r(t_k) \in se(3)$ (with $t_k = k\Delta t$ and $k \in \mathbb{N}$).

The controller presented here is similar to the one in [Joseph et al., 2018b]; [Prete, 2018]. The objective is to devise a strategy to control manipulators based on joint torque. For this, one needs to link the desired Cartesian spatial acceleration from the reference trajectory with the joint torques. First, the spatial acceleration can be expressed as a function of the joint accelerations with (4.3) using the Jacobian and its derivative. Then, by exploiting the robot dynamics model (see Section 4.2.1), one can formulate the joint accelerations as a function of the joint torques. This enables a QP formulation (as in (3.2)) of the task-space tracking problem, just like the kinematic formulation in (3.8):

$$ \tau^{\text{opt}} = \arg \min_{\tau} \quad \left\| {}^{B}\dot{v}(\tau) - {}^{B}\dot{v}_{k}^{*} \right\|^{2} + f_{\text{reg}}(\tau) \quad (4.7) $$

$$ {}^{B}\dot{v}(\tau) = {}^{B}\mathcal{J}(q_k)\ddot{q}(\tau) + {}^{B}\dot{\mathcal{J}}(q_k, \dot{q}_k)\dot{q}_k \quad (4.8) $$

where the sub-index k indicates measured states of the robot (for instance, the Jacobian J is computed at the measured robot configuration $q_k$); and $f_{\text{reg}}$ defines a regularisation task used to optimize over the redundant Degree of Freedom (DoF)s
of the robot and $^B\dot{v}^*$ denotes the desired twist derivative expressed in the body frame (spatial acceleration).

Additionally, the term $J(q_k)\dot{q}_k$ can be computed from the current measured state and be treated as a constant bias in the optimization. Thus, the spatial acceleration $\dot{v}(\tau)$ is a linear function of the optimization variable (the joint torque $\tau$).

If the model of the robot was perfectly known (i.e. $\dot{v}(\tau)$ is calculated without error), then the spatial acceleration would be the one corresponding to the reference trajectory $\dot{v}^* = \dot{v}^r$. However, there are always unmodeled dynamics and a proper strategy to deal with those is to use a PD servoing by defining a desired spatial acceleration:

$$ ^B\dot{v}^* = K_p^B e + K_d(v_k^r - v_k) + ^B\dot{v}^r \quad (4.9) $$

$$ ^B e = \log(\chi_k^{-1} \chi_k^r) \quad (4.10) $$

where the reference spatial acceleration $^B\dot{v}^r$ acts as a feed forward term.

Finally, the regularisation task $f_{reg}$ is defined in such a way that it uniquely defines an optimal solution even on redundant robots:

$$ f_{reg}(\tau) = w_{reg}||M^{-1}(\tau_{reg} - \tau)||_2^2 \quad (4.11) $$

with $w_{reg} \ll 1$, ensuring it will not affect the main tracking task; and $\tau_{reg}$ is defined depending on the desired use of the redundant DoFs. The inclusion of the inertia matrix $M$ helps modulate the importance of each joint according to the configuration.

For instance, if it was used to keep the robot configuration as far as possible (i.e. without affecting the tracking performance) from the joint limits while damping the joint velocity. It is defined as:

$$ \tau_{reg} = K_{\tau}(q_{reg} - q) - K_{\dot{q}}\dot{q} \quad (4.12) $$

$$ q_{reg} = \frac{(q_M - q_m)}{2} \quad (4.13) $$

where $K_{\tau}, K_{\dot{q}}$ are positive proportional terms and $q_M, q_m$ respectively denote the upper and lower joint bounds.

### 4.2.3 Inverse Dynamics Constraints ↑

This section presents a way to integrate the dynamics-level actuation limits into the linear constraints of a QP, for a robot controlled through its joint torques.

First of all, the intrinsic joint torque constraints from (4.3) can simply be embedded as:

$$ \tau_m \le I_n \tau \le \tau_M \quad (4.14) $$

where $I_n$ denotes an n-sized identity matrix.

The kinematic constraints (3.7) need to be reformulated as a function of the control
input. To achieve this, one can perform a taylor expansion of future joint configuration and velocities (assuming the current ones can be measured, i.e. $q_k, \dot{q}_k \in \mathbb{R}^n$):

$$q_{k+1} = q_k + \Delta t \dot{q}_k + \frac{\Delta t^2}{2} \ddot{q}(\tau) \quad (4.15)$$

$$\dot{q}_{k+1} = \dot{q}_k + \Delta t \ddot{q}(\tau) \quad (4.16)$$

where $\ddot{q}$ is explicitly dependent on the decision variable, the joint torques $\tau$, as seen in (4.1). Then, knowing that $q_{k+1} \in \mathcal{Q}$ and $\dot{q}_{k+1} \in \dot{\mathcal{Q}}$, one can formulate the kinematic constraints as:

$$\begin{aligned} q_M &\ge q_k + \Delta t \dot{q}_k + \frac{\Delta t^2}{2} \ddot{q}(\tau) & \dot{q}_M &\ge \dot{q}_k + \Delta t \ddot{q}(\tau) \\ q_m &\le q_k + \Delta t \dot{q}_k + \frac{\Delta t^2}{2} \ddot{q}(\tau) & \dot{q}_m &\le \dot{q}_k + \Delta t \ddot{q}(\tau) \end{aligned} \quad (4.17)$$

where $\Delta t$ is chosen large enough to cover a few control periods, as a safe margin to avoid entering the limits [Rubrecht et al., 2010]. These equations allow formulating the constraints as a function of the joint accelerations:

$$C_{\tau|q} = \left\{ \ddot{q}(\tau) \in \mathbb{R}^n \mid \ddot{q}(\tau) \in \left[ 2 \frac{(q_m - q_k - \dot{q}_k \Delta t)}{\Delta t^2}, 2 \frac{(q_M - q_k - \dot{q}_k \Delta t)}{\Delta t^2} \right] \right\} \quad (4.18)$$

$$C_{\tau|\dot{q}} = \left\{ \ddot{q}(\tau) \in \mathbb{R}^n \mid \ddot{q}(\tau) \in \left[ \frac{(\dot{q}_m - \dot{q}_k)}{\Delta t}, \frac{(\dot{q}_M - \dot{q}_k)}{\Delta t} \right] \right\} \quad (4.19)$$

which is possible thanks to the definition of $\ddot{q}$ in (4.1), under the assumption that there are no external forces applied ($f = 0$).

Finally, the torque rate constraints may be integrated as (4.6):

$$C_{\tau|\dot{\tau}} = \{ \tau \in \mathbb{R}^n \mid \tau \in [\tau_k + \Delta t \dot{\tau}_m, \tau_k + \Delta t \dot{\tau}_M] \} \quad (4.20)$$

## 4.3 MPC-based motion re-planning over a receding horizon

The focus of this section is to propose an *Model Predictive Control (MPC)* implementation on the SE(3) manifold, allowing pose control. In contrast to the kinematics-based approach of Chapter 3, this one looks to extend the definitions to a dynamics point of view. It does so by revisiting some concepts from Chapter 2 and briefly revisiting formulations from Section 3.3.2 to extend them to the case of jerk-bounded (acceleration-based) point to point pose motion generation.

Given a d-sized state and m-sized input system, it can be described with a discrete time linear system (indexed with n) as

$$x_{n+1} = Ax_n + Bu_n \quad A \in \mathbb{R}^{d \times d}, B \in \mathbb{R}^{d \times m} \quad (4.21)$$

revisited ↑ ↓

where A and B represent its state and input matrices whereas x and u are the state and input vectors. Smooth point to point pose motion generation can be achieved by generating trajectories at the acceleration level. Choosing the control input at the task level to be the acceleration, allows predicting the evolution of the end effector (system) state — pose and twist — over a short period of time to generate a jerk-bounded motion.
Discretizing a receding time window in h time steps of duration T starting at time $t_n$ (total duration hT) yields the discretized time steps inside the horizon, defined as $t_k = t_n + kT$ for $k = 0, 1, \dots, h$ and one can obtain $\mathbf{x}(t_k) = \mathbf{x}_{k|n}, \mathbf{u}(t_k) = \mathbf{u}_{k|n}$.

One can formulate a linear receding horizon expression as:

$$ \mathbf{x}_{k+1|n} = A\mathbf{x}_{k|n} + B\mathbf{u}_{k|n} \quad \text{for } k = 0, 1, \dots, h-1 \quad (4.22) $$

revisited ↑ ↓

Subsequent sections drop the $n$ subscript to simplify the notation, making $\mathbf{x}_{k|n}$ equivalent to $\mathbf{x}_k$.

This linear time discrete dynamic model expression can straightforwardly be written in a recursive matricial form over the whole horizon:

$$ \bar{\mathbf{X}} = A'\underline{\mathbf{X}} + B'\underline{\mathbf{U}} \quad (4.23) $$

revisited ↑

where $A', B'$ are matrices constructed from $A, B$ via the recursive propagation of (3.29) over the time horizon. Moreover, $\bar{\mathbf{X}}$ and $\underline{\mathbf{U}}$ respectively correspond to the future states and control inputs of the system, as described in:

$$ X_{0\dots h} = \begin{bmatrix} \mathbf{x}_k \\ \mathbf{x}_{k+1} \\ \vdots \\ \mathbf{x}_{k+h} \end{bmatrix} \qquad \mathbf{u}_{0\dots h-1} = \begin{bmatrix} \mathbf{u}_k \\ \mathbf{u}_{k+1} \\ \vdots \\ \mathbf{u}_{k+h-1} \end{bmatrix} \quad (4.24) $$

revisited ↑

$$ \bar{\mathbf{X}} = X_{1\dots h} $$

$$ \underline{\mathbf{X}} = X_{0\dots h-1} $$

$$ \underline{\mathbf{U}} = U_{0\dots h-1} $$

From these propagated linear dynamics, one can formulate the problem of finding a trajectory over a gliding time horizon. In a generic way, this concerns the problem of generating a constrained and time discrete trajectory over the system state. Point to point trajectories in a system state minimize the distance between the starting and desired states of a system. This problem can be mathematically expressed as a the linear optimal control problem over a receding horizon as a Quadratic Program (QP) with the following cost function:

$$ \min_{\mathbf{x},\mathbf{u}} \|\bar{\mathbf{X}}_t - \bar{\mathbf{X}}\|_P^2 + \|\underline{\mathbf{U}}\|_R^2 \quad (4.25) $$

revisited ↑ ↓

subject to:

$$ \bar{\mathbf{X}} = A'\underline{\mathbf{X}} + B'\underline{\mathbf{U}} \quad (4.26) $$

revisited ↑

$$ C_{x_m} \leq C_x \mathbf{x}_k \leq C_{x_M} \quad (4.27) $$

revisited ↑ ↓

$$ C_{u_m} \leq C_u \mathbf{u}_k \leq C_{u_M} \quad (4.28) $$

revisited ↑ ↓

where $\bar{\mathbf{X}}_t$ is the target state $\mathbf{x}_t$ vectorized over the horizon. This is an important detail and one of the original features of the proposed approach: no pre-interpolation or a priori trajectory generation is required. The resulting motion is solely shaped by the considered constraints on the admissible task space motions. This implicitly yields a time optimal trajectory over the horizon.

Regarding $P, R$, they respectively represent symmetric positive definite weighting
matrices for the states and inputs throughout the horizon. These are the same weighing matrices shown in (3.33), where $P_T$ explicitly expresses the importance of the terminal state, whereas here it is embedded within $P$. The same logic applies to the control input weights, which are embedded in $R$:

$$ P = \underbrace{\begin{bmatrix} P_k & & & & \\ & P_{k+1} & & & \\ & & \ddots & & \\ & & & P_{k+h-1} & \\ & & & & P_T \end{bmatrix}}_{h \text{ times}} \quad R = \underbrace{\begin{bmatrix} R_k & & & & \\ & R_{k+1} & & & \\ & & \ddots & & \\ & & & R_{k+h-1} & \\ & & & & R_{k+h} \end{bmatrix}}_{h \text{ times}} \quad (4.29) $$

For a typical usage, the weights employed remain constant throughout the horizon and these matrices are defined as:

$$ P_{k+i} = I_{d \times d} \quad \text{for } i = 0, \dots, h-1 \quad (4.30) $$

$$ P_T = I_{d \times d} w_T \quad (4.31) $$

$$ R_{k+i} = I_{d \times d} \gamma \quad \text{for } i = 0, \dots, h \quad (4.32) $$

where $w_T, \gamma$ denote the weight for the terminal state and the control inputs in the horizon, while the weight of the remaining states is set to 1. The control input weight $\gamma$ shown in (3.47) is the vector version that contains the weight values for each step (assuming they are not the same).

Finally, $C_x, C_u$ allow formulating linear constraints with respect to the state and input, while $x_m, x_M$ and $u_m, u_M$ respectively designate the state and input bounds.

This problem needs to be continuously evaluated to find a new horizon trajectory respecting the constraints. The starting and target states $X_0, X_t$ ultimately close the re-planning feedback loop and add the reactivity needed in an interactive scenario.

For the problem at hand, to generate a constrained position and orientation trajectory, the system state needs to contain the pose and twist of the end effector. Meanwhile, to obtain a smooth motion, the control input is considered at the acceleration level. The goal at each discrete step of the horizon is to reach the final target with a null velocity. A greater importance (weight) is given to the last step of the horizon to favour convergence towards the target with $P_T$.

Equation (3.47) under constraints (3.31) and (3.34) constitutes the general linear MPC formulation as a QP. It progressively provides a trajectory to track for the end-effector as the optimization window evolves.

The linearity of this MPC formulation is a mandatory feature for the corresponding optimization problem to be solved at each control time step. This problem needs to overcome two main challenges: it needs to be computationally tractable to be solved in coherence with the real time requirements of collaborative scenarios; furthermore, writing this problem in a linear fashion, given that the Cartesian pose of the end-effector $\mathcal{X}$ belongs to $\mathbb{SE}(3)$, is not trivial. The following sections propose a way to tackle both problems.
### 4.3.1 Reduced Formulation ↑

The cost function (3.47) minimizes the distance between the current and the desired states over the state and control input variables. However, due to the equality condition over a horizon shown in (3.31), the states of the system throughout the time horizon are completely determined by the control input in that same horizon (for a fixed starting state). This section shows how the same problem may be reduced to an optimization over the control input, effectively reducing the size of the optimizing variables, to obtain a less computationally intensive problem that can be solved at the control rate of the robot.

Assuming constant linearized dynamics for a short period of time allows performing an optimization over the estimated future states. The assumption is explicitly considered in (3.29). While subsequent estimations become progressively less precise (due to the decreasing validity of the constant linearized dynamics assumption), this does not pose an issue if the linearization and the horizon recalculation are performed frequently.

However, the key consideration is that this assumption allows formulating the future state in a horizon as a function of the initial state $x_0$ and the control horizon $\underline{U}$: more explicitly, a function $\bar{X}(x_0, \underline{U})$.

To do so, one may develop (3.29) for every state $x_k$ (for $k = 1...h$) as a function of the initial state $x_0$ and the successive control inputs $u_0, u_1...u_h$:

$$x_1 = Ax_0 + Bu_0 \quad (4.33)$$

$$x_2 = Ax_1 + Bu_1 \quad (4.34)$$

$$= A(Ax_0 + Bu_0) + Bu_1$$

$$= A^2x_0 + ABu_0 + Bu_1$$

$$x_3 = Ax_2 + Bu_2 \quad (4.35)$$

$$= A(A^2x_0 + ABu_0 + Bu_1) + Bu_2$$

$$= A^3x_0 + A^2Bu_0 + ABu_1 + Bu_2$$

⋮

$$x_H = A^Hx_0 + A^{H-1}Bu_0 + A^{H-2}Bu_1... + ABu_{h-2} + Bu_{h-1} \quad (4.36)$$

arriving at a matricial form:

$$\underbrace{\begin{bmatrix} x_1 \\ x_2 \\ x_3 \\ \vdots \\ x_h \end{bmatrix}}_{\bar{X}} = \underbrace{\begin{bmatrix} B \\ AB & B \\ A^2B & AB & B \\ \vdots \\ A^{h-1}B & A^{h-2}B & \dots & AB & B \end{bmatrix}}_{\bar{A}} + \underbrace{\begin{bmatrix} u_0 \\ u_1 \\ u_2 \\ \vdots \\ u_{h-1} \end{bmatrix}}_{\underline{u}} + \underbrace{\begin{bmatrix} A \\ A^2 \\ A^3 \\ \vdots \\ A^h \end{bmatrix}}_{\bar{B}} x_0 \quad (4.37) \quad \text{revisited } \downarrow$$
that can be synthesized into a simple algebraic expression:

$$ \bar{X}(x_0, \underline{U}) = \hat{A}\underline{U} + \hat{B}x_0 \quad (4.38) $$

that can be directly embedded into (3.47):

$$ \min_{\underline{u}} \|\bar{X}_t - \bar{X}(x_0, \underline{U})\|_P^2 + \|\underline{U}\|_R^2 \quad (4.39) $$

but optimizing only over the control inputs $\underline{u}$. Nevertheless, it can still be subject to linear constraints:

$$ C_{x_m} \le C_x x_k \le C_{x_M} \quad (4.40) $$

$$ C_{u_m} \le C_u u_k \le C_{u_M} \quad (4.41) $$

$$ C_{\dot{u}_m} \le C_{\dot{u}} \dot{u}_k \le C_{\dot{u}_M} \quad (4.42) $$

with: $\dot{u}_k = \frac{(u_{k+1} - u_k)}{T}$

where the control input constraints in (4.41) are kept the same as in (3.35) and (3.36). Meanwhile, the state constraints need to be expressed with respect to the optimization variable with (4.38). In the case of simple state bounds (where $C_x$ is an identity matrix), it can be done straightforwardly with:

$$ \bar{X}_m - \hat{B}\underline{x}_0 \le \hat{A}\underline{U} \le \bar{X}_M - \hat{B}\underline{x}_0 \quad (4.43) $$

where the term $\hat{B}\underline{x}_0$ can be treated as constant, because $x_0$ is the measured initial state of the system (at the start of the horizon) and:

$$ \bar{X}_m = \begin{bmatrix} \mathbf{x}_{m|k+1} \\ \mathbf{x}_{m|k+2} \\ \vdots \\ \mathbf{x}_{m|k+h} \end{bmatrix} \qquad \bar{X}_M = \begin{bmatrix} \mathbf{x}_{M|k+1} \\ \mathbf{x}_{M|k+2} \\ \vdots \\ \mathbf{x}_{M|k+h} \end{bmatrix} \quad (4.44) $$

express the limits for the state at each discretized step in the horizon. Here, they are chosen constant to coincide with those in (3.35), but the expression can accommodate potentially evolving limits.

The relevance of this formulation comes from optimizing the QP expression to reduce computation times. In fact, the solving times suffer from the “curse of dimensionality” and scale exponentially with the size of the optimizing variable. This is why reducing its size is a simple strategy with a direct impact on performance [Ferreau, 2011] without recurring to customized solvers that need to better exploit the sparsity of the problem matrices to achieve the same gains.

### 4.3.2 Dynamics-based MPC in the Tangent Space ↑

This section revisits some concepts from Sections 3.3.3 and 3.3.4 to focus on the formulations introduced in Section 3.3.5, that were employed to perform optimizations in the tangent space of pose manifold, for a first approach to motion generation with an MPC. This time, even though some of the equations are the same, they are specialized to obtain an acceleration-based formulation. Additionally, the lift function is different and offers some computational advantages that aid in achieving
re-planning at the control-rate.

The first step is understanding how (3.27) can express the pose dynamics in a linear form. Consider the first-order integration of a pose in SE(3) subject to an infinitesimal body twist increment for a single time step T:

$$ \mathcal{X}_{k+1} = \mathcal{X}_k e^{\mathrm{T}\nu_k + \frac{T^2}{2}\dot{\nu}_k} \quad (4.45) $$

revisited ↑ ↓

$$ \nu_{k+1} = \nu_k + \mathrm{T}\dot{\nu}_k \quad (4.46) $$

revisited ↓

where (4.46) is added to relate the twist to a spatial acceleration $\dot{v}$ integration step for the same time period T. The objective is to include the pose and the twist into the state $x$ of the system, and use the spatial acceleration as a decision variable or control input $u$. This way, (4.45) and (4.46) can be expressed in the form of (3.27) to be used for linear MPC with (4.39).

The term $e^{\mathrm{T}\nu_k}$ in (4.45) is the equivalent homogeneous matrix to a floating body moving at $\nu_k$ for a duration of T. It is computed using the matrix exponential, that subjectively maps the Lie algebra $\mathfrak{se}(3)$ onto the Lie group $\mathrm{SE}(3)$. Its inverse operation is known as the logarithmic map.

This relation can be exploited to represent $\mathcal{X}$ in a vector form. In fact, given some initial pose $\mathcal{X}_0$, one can devise a function $\psi$ (referred to as the *lift function*) that maps $\mathcal{X}_k$ to a vector $\xi_k$. The general principles behind this approach are further explained in Section 2.4 or, more briefly, in [Forster et al., 2015]; [Solà et al., 2018]; [Torres Alberto et al., 2022a]. Such function is defined as:

$$ \psi(\mathcal{X}_k) = \log(\mathcal{X}_0^{-1}\mathcal{X}_k) \qquad \psi^{-1}(\xi_k) = \mathcal{X}_0 e^{\xi_k} \quad (4.47) $$

revisited ↑

$$ \psi : \mathrm{SE}(3) \rightarrow \mathfrak{se}(3) \qquad \log(\mathcal{X}_0^{-1}\mathcal{X}_k) \rightarrow \xi_k \quad (4.48) $$

revisited ↑

$$ \psi^{-1} : \mathfrak{se}(3) \rightarrow \mathrm{SE}(3) \qquad \mathcal{X}_0 e^{\xi_k} \rightarrow \mathcal{X}_k \quad (4.48) $$

The lift function maps elements from the $\mathrm{SE}(3)$ manifold $\mathcal{M}$ to its tangent space at $\mathcal{X}_0$: $\psi : \mathcal{M} \rightarrow \mathcal{T}_{\mathcal{X}_0}\mathcal{M}$. Notice that the roto-translational displacement $\Delta\mathcal{X}_k \in \mathrm{SE}(3)$ (or equivalent homogeneous matrix) needed to move from $\mathcal{X}_0$ to $\mathcal{X}_k$ can be computed with $\Delta\mathcal{X}_k = e^{\psi(\mathcal{X}_k)}$. The function $\psi^{-1}$ is referred to as the *retract function*.

With the MPC being formulated in $\mathfrak{se}(3)$, the lift operator $\psi$ is used to map the current state and the desired pose into $\mathfrak{se}(3)$ to perform the optimization. Meanwhile the retract operator $\psi^{-1}$ is used to map the computed trajectory back to $\mathrm{SE}(3)$. This enables directly embedding the *lifted* pose $\xi = \psi(\mathcal{X})$ into the discretized state of the system $x$, along with the body twist $v$. Then, choosing the spatial acceleration $\dot{v}$ as the input:

$$ x_k = \begin{bmatrix} \xi_k \\ \nu_k \end{bmatrix} \qquad u_k = \dot{v}_k \quad (4.49) $$

revisited ↑ ↓

where the reader is reminded that the $k$ sub-index symbolizes the discretized step within the horizon. In other words, with this notation, $x_k$ corresponds to the state at the step $k$ in the horizon starting at time $t_n$, also denoted as $x_{k|n}$. The same notation applies to the other variables. Then, once the MPC optimization is solved, it is
straightforward to obtain the desired twists and spatial accelerations from $x$ and $u$. Yet, for the desired pose $\mathcal{X}$, the retract function is required:

$$ \mathcal{X}_k = \psi^{-1}(\xi_k) \qquad (4.50) $$

Furthermore, in order to link (4.45) and (4.46) with (3.27) (with linear MPC in mind), it needs to be linearized. Computing $\psi(\mathcal{X}_{k+1})$ yields:

$$ \psi(\mathcal{X}_{k+1}) = \log(\Delta\mathcal{X}_k e^{\mathrm{T}\nu_k + \frac{T^2}{2}\dot{\nu}_k}) \qquad (4.51) $$

This expression has a first-order approximation in its Lie algebra (see Equations 68-74 in [Solà et al., 2018]):

$$ \xi_{k+1} = \log(e^{\xi_k} e^{\mathrm{T}\nu_k + \frac{T^2}{2}\dot{\nu}_k}) \approx \xi_k + \mathrm{J}_R^{\mathrm{log}}(\xi_k)(\mathrm{T}\nu_k + \frac{T^2}{2}\dot{\nu}_k) \qquad (4.52) $$

revisited ↑ ↓

where $\mathrm{J}_R^{\mathrm{log}}(\xi_k)$ is the right-trivialized Jacobian of the logarithmic map, evaluated at $\xi_k$. A closed-form expression can be found in [Barfoot et al., 2014]; [Solà et al., 2018] and is implemented in the Pinocchio library [Carpentier et al., 2019]. It relates the additive increments in $\mathcal{T}_0\mathcal{M}$ (the tangent map at the origin) to the rightmultiplied increments in $\mathcal{S}\mathcal{E}(3)$.

Equation (4.52) offers the missing piece to have a concrete definition for (3.27) when considering the acceleration $\dot{\nu}_k$ as the control input:

$$ A_k = \begin{bmatrix} \mathbf{I}_6 & \mathrm{T}\mathrm{J}_R^{\mathrm{log}}(\xi_k) \\ \mathbf{0}_6 & \mathbf{I}_6 \end{bmatrix} \qquad B_k = \begin{bmatrix} \frac{T^2}{2}\mathrm{J}_R^{\mathrm{log}}(\xi_k) \\ \mathrm{T}\mathbf{I}_6 \end{bmatrix} \qquad (4.53) $$

revisited ↑ ↓

finally offering a concrete definition for the propagation of the state of the system as in (4.38).

The final step is to exploit these identities in the cost function (4.39), but a proper way to calculate the distance in the tangent space has not yet been presented. In fact, a remarkable property of the lift and retract functions in (4.48) is that the geodesic path between $\mathcal{X}_0$ and $\mathcal{X}_t$ can be interpolated for any pose in between with a parameter $\alpha \in [0, 1]$:

$$ \mathcal{X}(\alpha) = \psi^{-1}(\alpha\psi(\mathcal{X}_t)) = \mathcal{X}_0 e^{\alpha\psi(\mathcal{X}_t)} \qquad (4.54) $$

It is straightforward to notice that $\mathcal{X}(0) = \mathcal{X}_0$ and $\mathcal{X}(1) = \mathcal{X}_t$.

This geodesic path is obtained by minimizing the Log-Euclidean ([Jayasumana et al., 2014], Table 1) distance metric between the lifted pose $\xi_k$ and the lifted target pose $\xi_t = \psi(\mathcal{X}_t)$:

$$ \|\xi_t - \xi_k\|_2^2 \qquad (4.55) $$

Then one can define a metric for the state space that calculates the target-related cost in (4.39). Assuming the target state $x_t$ contains the lifted target pose and a null (zero)
body twist:

$$ \mathbf{x}_t = \begin{bmatrix} \xi_t \\ 0 \end{bmatrix} \qquad (4.56) $$

revisited ↑ ↓

for each step in the discretized horizon, one can use the metric:

$$ \|\mathbf{x}_t - \mathbf{x}_k(\mathbf{x}_0, \mathbf{u}_0, \dots, \mathbf{u}_{k-1})\|_2^2 \qquad (4.57) $$

where $\mathbf{x}_k(\mathbf{x}_0, \mathbf{u}_0, \dots, \mathbf{u}_{k-1})$ is just (4.38) defined for a single step k in the horizon. In this way, minimizing (4.57) for all discretized steps, it leads to minimizing the geodesic distance towards the target pose over the horizon. This effectively induces a generated motion that follows the geodesic path in the direction of the target while respecting the constraints imposed in the optimization.

### 4.3.3 Interpolating the Horizon Trajectory ↑

It is also important to highlight that the discretization step T within the horizon typically needs to be longer than the robot control period. This is necessary in order to decrease resolution times (by reducing the size of the decision variables in the optimization problem while still being able to optimize long enough into the future for the solution to be significant). This remains true even if the MPC optimization runs at every control loop. By doing this, the MPC can contemplate longer horizons, leading to enhanced performance and ensuring that constraints are met. Nonetheless, it implies that the output horizon trajectory from the MPC must be interpolated to determine the subsequent desired state / input.

In the case of the twist and input, their value at some desired time can be obtained by a simple linear interpolation. Consider a desired time within two steps in the horizon $t_d \in [t_k, t_{k+1}]$ and some variable discretized within a horizon $\mathbf{y}$, then the linear interpolation function $f_{\text{lerp}}(\mathbf{y}, t_d)$ is defined as:

$$ \mathbf{y}(t_d) \approx f_{\text{lerp}}(\mathbf{y}, t_d) = \mathbf{y}_k + \frac{(t_d - t_k)}{T} (\mathbf{y}_{k+1} - \mathbf{y}_k) \qquad (4.58) $$

such that $\mathbf{y}(t_k) = \mathbf{y}_k$ and $\mathbf{y}(t_{k+1}) = \mathbf{y}_{k+1}$. Then the desired trajectory can be interpolated from the body twist and spatial acceleration with $f_{\text{lerp}}(\mathbf{v}, t_d)$ and $f_{\text{lerp}}(\dot{\mathbf{v}}, t_d)$, respectively.

Interpolating the desired pose in the obtained horizon trajectory requires a bit more work but can be done by using the exponential map in a similar way to the lift function. Under the same assumptions for $t_d$ as in (4.58), one can define an interpolation scheme in the pose manifold as:

$$ \mathcal{X}(t_d) \approx \mathcal{X}_k e^{\alpha \delta} \qquad \delta = \log(\mathcal{X}_k^{-1} \mathcal{X}_{k+1}) \qquad \alpha = \frac{(t_d - t_k)}{T} \qquad (4.59) $$
<figure><img src="image_33.png" /><figcaption>Figure 4.2: The implemented control architecture is based on the one proposed in Section 4.1.2. It is composed of a cascade loop: 1) a task-space Model Predictive Controller for online re-planning (described in Section 4.3); 2) a constrained Task Space Inverse Dynamics solver to follow the planned trajectory (described in Section 4.3). The feedback is closed with the current state of the robot.</figcaption></figure>

revisited ↑ ↓

## 4.4 Experimental Validation: Online Re-planning at the Control Rate ↑

In the experiment, the robot is directed to align itself in order to grasp a plastic piece in the workspace, despite not having prior knowledge of the piece location or orientation (the pose of the object is sensed in real time by a camera array). The purpose is to test the robot capability to adapt on-the-fly to unexpected changes in the target pose. The outcomes illustrate a near-time optimal movement, combined with frequent online adjustments, all while adhering to the imposed workspace constraints.

Some of these results are exemplified in Figure 4.3 with an online trajectory adaptation in two situations: during the alteration of the maximum Cartesian velocity (constraints); and in the case of a unplanned changes to the target pose. These results are qualitative but they confirm the overall ability of the proposed control approach to adapt the robot motion to changing tasks conditions, highlighting several noteworthy features of the approach including:

1. constraints compliance.

2. the ability to manage instantaneously switching targets.

3. the ability to continuously adapt the trajectory towards a continuously moving target.

4. the effect of the modification of velocity constraints in the MPC on the generated motion horizon at each control step.

A detailed video of the experiment may be found by following the link in Figure 4.7.

### 4.4.1 Experimental Procedure & Setup ↑

A three phase experiment is proposed, each progressively increasing the complexity of the task. The setup and phases are illustrated in Figure 4.4. After completing each phase, the robot returns to its "home pose" before embarking on the subsequent phase. The specifics of the phases are as follows:
<figure><img src="image_34.png" /><figcaption>Figure 4.3: A linear *Model Predictive Control (MPC)* is used to compute a position and orientation trajectory towards a target (plastic brick) in a receding horizon. (a) and (b) highlight the effects of changing the task space constraints on the fly (in this case max velocities) on the covered distance for one horizon. (c) and (d) illustrate the continuous adaptation of the trajectory when the target location is modified online.</figcaption></figure>

<figure><img src="image_35.png" /><figcaption>video/link</figcaption></figure>
<figure><img src="image_36.png" /></figure>

<figure><img src="image_37.png" /><figcaption>Figure 4.4: The experimental setup is composed of a torque-controlled 7-DoF Franka Emika Panda serial robot and an Optitrack camera array. Reflective markers are placed on a plastic brick. The camera array is used to detect the position and orientation of the plastic brick in the workspace. The robot is instructed to grab the brick without prior knowledge of its pose. The experiment consists of three stages of increasing complexity: 1- non-moving brick; 2- slowly moving brick; 3- the brick is held by the operator and tracked in real time.</figcaption></figure>

1. **Non moving target:** The simplest task is presented in the first phase. The robot is requested to pick-up the plastic brick placed in the workspace on top of an irregular surface (to induce non-trivial orientations). The pose of the brick is not known a priori and is detected via a vision system. The plastic piece is not moved during this phase. Given this target pose information $\mathcal{X}_t$, the robot starts its motion towards the brick using the control scheme shown in Figure 4.1. Once the object is reached, the robot grasps it and releases it after a few seconds. The tracking results of this phase are shown in Figure 4.5.

2. **Slowly moving target:** For the second phase, the robot goes back to its neutral position and the brick is placed at a new starting pose. After a few seconds, the robot starts its motion towards this new goal. While in motion, the brick is slowly displaced by a human and the robot adapts its trajectory online to reach it.

3. **Operator moving target:** In the third phase, the human holds the plastic brick and moves it around randomly at moderate speed while the robot tracks it in real time. To increase the complexity the operator also holds the robot for a few seconds to interrupt its movement, showcasing the re-planning capabilities of the approach. This phase is illustrated in snapshots of the video in Figure 4.6.

The setup is shown in Figure 4.4. Perception of the plastic brick is provided by an Optitrack camera array with reflective markers attached. The experiment is carried out on a torque-controlled 7-Degree of Freedom (DoF) Franka Emika Panda serial robot.

The control architecture in use is grounded on the design detailed in Section 4.1.2. It
is shown in Figure 4.2 and runs at 1kHz. This system is structured as a cascade loop, consisting of two primary components:

1. A task-space Model Predictive Controller for real-time re-planning, as outlined in Section 4.3.

2. A constrained Task Space Inverse Dynamics solver, dedicated to adhering to the mapped trajectory, also elucidated in Section 4.3.

The feedback loop is completed using the robot current state. In order to keep up with the control rate, the MPC uses a 750ms control horizon ($h = 5, T = 150$ms), which yields an average computation time<fnref n="2" /> of 0.1ms (max 0.2ms) using qpOASES [Ferreau et al., 2014].

The same Quadratic Program (QP) solver is used to solve the constrained task space inverse dynamics. The values of gains and weights for this inner QP controller are summarized in Table 4.1. It is just recalled here that a larger cost is assigned to the terminal step of the horizon to favour convergence towards the target. Implementation details may be found in the source code<fnref n="3" />.

<table><caption>Table 4.1: Values of the gains and weights retained for the inverse dynamic solver used for task-space tracking shown in Figure 4.2 and described in Section 4.2.</caption><thead><tr><th>Gains / Weights</th><th>K<sub>p</sub></th><th>K<sub>d</sub></th><th>K<sub>q</sub></th><th>K<sub>q̇</sub></th><th>w<sub>reg</sub></th></tr></thead><tbody><tr><td>Value</td><td>150</td><td>24.5</td><td>100</td><td>10</td><td>10<sup>-5</sup></td></tr><tr><td>Units</td><td>s<sup>-2</sup></td><td>s<sup>-1</sup></td><td>N.m.s<sup>-2</sup></td><td>N.m.s<sup>-1</sup></td><td>N<sup>-2</sup>.m<sup>2</sup></td></tr></tbody></table>

Details on the resulting motion from one run of this experiment are provided in Figure 4.5 for the first of the described phases. While the brick stays in a fixed pose during this phase, it serves to showcase two behaviors: first, it shows the online re-planning algorithm uses all the acceleration available to attain pseudo-optimal trajectories; secondly, it does so even when the provided target pose is discontinuous (at the start of the phase).

The nuances of the other two experiments are best understood through the video accompanying this study, which can be referenced in Figure 4.7. The dynamic nature of the moving target produces an adaptive behavior that is most effectively interpreted within the video context. Nonetheless, snapshots from the video can be seen in Figure 4.6. These captures illustrate the experiment third phase where an operator holds the brick. The objective here is to demonstrate the robot real-time adaptability, even when faced with external disturbances, such as being manipulated by an operator, as highlighted in Figures 4.6(d) and 4.6(e).

### 4.4.2 Analysis ↑

Figure 4.5(a) shows the error between the current and the target poses. The discontinuity seen at the beginning corresponds to the start of the experiment sequence, where the target pose switches from the “home pose” to the plastic brick pose. This

<footnote n="2">²The experiments were run on desktop computer equipped with a CPU i9-10900K and 32GB of ram.</footnote>

<footnote n="3">³https://gitlab.inria.fr/auctus-team/people/nicolas-torres/lmpcpoly-ws/</footnote>
<table><caption>Table 4.2: Cartesian constraints for $v, \dot{v}, \ddot{v}$ for the MPC during the experiment. The MPC used for motion generation is shown in Figure 4.2 and described in Section 4.2.</caption><thead><tr><th></th><th>ν<sub>max</sub></th><th>dot<sub>max</sub></th><th>ddot<sub>max</sub></th></tr></thead><tbody><tr><td>linear</td><td>0.2 (m/s)</td><td>2 (m/s<sup>2</sup>)</td><td>1000 (m/s<sup>3</sup>)</td></tr><tr><td>angular</td><td>0.8 (rad/s)</td><td>5 (rad/s<sup>2</sup>)</td><td>3000 (rad/s<sup>3</sup>)</td></tr><tr><td colspan="4">-ν<sub>max</sub> ≤ ν ≤ ν<sub>max</sub>, -dot<sub>max</sub> ≤ dot ≤ dot<sub>max</sub>, -ddot<sub>max</sub> ≤ ddot ≤ ddot<sub>max</sub></td></tr></tbody></table>

highlights the convergence towards the requested pose without any a priori knowledge on the trajectory to follow. Moreover, it shows that even in the presence of a discontinuous target (a task change), the approach succeeds in providing a smooth transition that, while respecting constraints, progressively adapts the motion.

Notice that this is not the resulting tracking error of following the trajectory planned by the MPC with the constrained task-space inverse dynamics solver, i.e. the tracking performance of following a desired trajectory computed with the MPC. Such curve is shown in Figure 4.5(b). It is bounded to 19.7mm with a mean value of $4\text{mm} \pm 1$ for the linear component of the motion and $5.18 \times 10^{-2}\text{rad}$ with a mean value of $2.5 \times 10^{-3}\text{rad} \pm 5.5 \times 10^{-3}$ for the angular component.

The final error is not exactly zero ($2.1\text{mm}$ and $10.4 \times 10^{-2}\text{rad}$) due to the relatively low gains of the Proportional Derivative (PD) controller and the absence of an integral term to reject disturbances due to dry friction at the joint level. Yet, the robot is able to grasp the targeted object at the end of the motion as can be seen in the accompanying video (see Figure 4.7).

Figure 4.5(c) shows both the planned (by the MPC) and the actual velocities; as opposed to the acceleration and jerk curves (respectively in Figures 4.5(d) and 4.5(e)) that only show the planned trajectories.

The first thing to notice is that the constraints on the planned trajectory are met by the MPC, as can be seen on the velocity and jerk curves.

It is also interesting to see that the obtained linear velocity profile is similar, to some extent, to a trapezoidal acceleration profiles, maximizing the use of some of the prescribed Cartesian limits shown in Table 4.2. This confirms the near-optimal character of the generated trajectory.

Finally, one can notice that the robot end-effector velocity surpasses the established limits. This is discussed in Section 4.5.
<figure><img src="image_38.png" /><figcaption>Figure 4.5: In the first phase, the robot is tasked to pick up the plastic brick positioned on an uneven surface to induce a non-trivial orientation. The brick pose, unknown beforehand, is identified through a vision system and remains stationary. Using this information, the robot moves to the brick as per the control scheme shown in Figure 4.1. After grasping, it holds the brick for a few moments and then releases it. The tracking results indicate target poses set in real-time. In each graph, the red, green, and blue curves denote x, y, and z axis components. Actual robot velocities are shown with solid lines, while the desired velocities (calculated by the MPC and denoted $v_{\text{mpc}}$) are dotted. Shaded regions mark the bounds set for the MPC.</figcaption></figure>
<figure><img src="image_39.png" /><figcaption>Figure 4.6: This showcases the third phase of the experiment. Certain key moments of the accompanying video have been captured in still images to provide a more immediate understanding. During the third phase of the experiment, the focus shifts to the robot real-time adaptability and its capability to re-plan its actions in response to dynamic changes. During this phase, a human operator holds the plastic brick, moving it in an unpredictable manner at a moderate pace. As the brick moves, the robot diligently tracks its position and orientation in real-time. Adding to the complexity, the operator intervenes by restraining the robot movement for a short period (shown in (d) and (e)), serving as an external disturbance. This interruption aims to highlight the robot proficiency in rapidly recalculating and adjusting its strategy. The images depict three elements: on the left, the real robot as seen during the test is shown; on the bottom right, a virtual counterpart displays both the target pose (the brick pose) and the horizon trajectory calculated in real-time; meanwhile, the top right presents graphs illustrating the tracking error and planned velocities.</figcaption></figure>
## 4.5 Discussion

The modular nature of the proposed architecture offers the potential to generically consider different types of task inputs. It can for example be easily adapted to use a discretized pose path, which could come from an obstacle avoidance solver. Hence, the same architecture can accommodate a complex pathing solution while retaining its core structure.

This architecture can also accommodate different types of task space to joint space solvers. This is an important feature as not all robots allow for joint torque control.

Moreover, the cost function of the Model Predictive Control (MPC) and the Cartesian constraints offer a generic way to design human-aware solutions considering the non-trivial cognitive aspects of human-robot collaboration. For example, representing the human motion capabilities as Cartesian constraints [Skuric et al., 2022b].

Furthermore, the controller's interactive capabilities demonstrate its ability to adjust to the physical attributes of the operator. For instance, in a pick-and-place scenario where the robot retrieves an item from a human, it will approach the human. This allows the operator to choose a comfortable height or location for interaction. This example is comparable to the scenario shown in the experimental interactive tracking task. However, more complex tasks where the robot needs to assist the human have the potential to showcase a stronger effect.

This is further enabled by decoupling the task-related definition and constraints from the joint-level controller. Indeed, some constraints are intrinsically task related and somewhat independent of the robot performing the task while others are strongly related to the whole robot motion. It is for example the case for safety-aware constraints [Joseph et al., 2018a] or the exploitation of the robot redundancy to perform secondary tasks without compromising performance. Examples of the later case are shown for safety purposes in [Joseph et al., 2019]; [Mansfeld et al., 2017] and for situation awareness in [Camblor et al., 2022].

Figure 4.5(c) shows that the real end-effector velocity loosely follows the planned velocity, going beyond the established limits in some cases. The measured velocity is being used as the initial state of the MPC at each time step, causing the associated Quadratic Program (QP) to become infeasible. This characteristic is intrinsic to any QP-based architecture and it can be considered both a problem and an advantage: for one, it allows easily detecting ill-conditioned initial states that could result in undefined behaviors (for example, to trigger an emergency stop mode); on the other hand, it demands that a proper strategy is put in-place to deal with this situation.

For this experiment, the inner loop controller continues to follow the last horizon that was successfully computed until the MPC can compute a new one. This allows the execution to continue smoothly.

Nevertheless, this opens one fundamental question that naturally arises from this type of cascade control architectures: the compatibility between outputs of each stage. For instance, the MPC could plan a trajectory that respects the Cartesian constraints even if it is instantaneously unfeasible in a control step or may lead to an inevitable joint-level constraint violation in the near future [Rubrecht et al., 2010].
To solve this problem, joint-level constraints [Prete, 2018] should be implicitly considered at the MPC level.

There exist efficient methods to effectively formulate the actual robot capacities as Cartesian constraints [Skuric et al., 2021a]. This type of constraints goes beyond classical box-like bounds and is a step towards more complex adaptation of the robot behaviour based on its motion capabilities. Integrating these into the MPC is addressed in the next chapter.

## 4.6 Conclusion

<figure><img src="image_40.png" /></figure>

This Chapter proposes an efficient linear model predictive control approach that can deal with the online planning of a motion in $SE(3)$ in the task space.

The main features of the proposed control approach lie in its ability to generate optimal Cartesian motions which dynamically account both for targets updated on-the-fly and evolving constraints. By utilizing a receding horizon approach, the robot gains the capability to interactively adjust and adapt to task changes.

In the context of collaborative robotics, utilizing this controller ensures the safety of a human operator sharing the workspace with the robot by adjusting maximum velocity constraints based on sensor feedback.

The next chapter focuses on extending linear constraints in the *Model Predictive Control (MPC)* for the Cartesian pose state, to limit the effective position and orientation space. This work also opens doors to potentially exploiting a more efficient expression of Cartesian constraints such as convex polytopes that consider the actual robot joint space capabilities in the receding horizon optimization stage.

The reduction of the computation time required to solve the *MPC* problem is also an important matter. It will be pursued to refine the computation of the trajectory over the horizon and potentially improve the performance of the closed-loop.

<figure><img src="image_41.png" /><figcaption>Figure 4.7: QR code for the video of the high-frequency acceleration-based Model Predictive Controller experiment for full-pose online constrained motion generation. On PDF file readers, it is also a clickable hyperlink.</figcaption></figure>
Chapter 5

Actuation-aware Task-space Model
Predictive Control

In collaborative robotics, the ability to adapt swiftly and seamlessly
to changes in the task and the surrounding environment, particularly
in shared spaces with humans, is of paramount importance. The pro-
posed modular control architecture grounded in Model Predictive Con-
trol strikes a balance between optimal task achievement and efficient
computation. However, in order to obtain optimal behaviors that effi-
ciently exploit the robot capabilities, they must be considered during the
trajectory planning stages.

To further enhance the control strategy, this chapter bridges the gap
between task-space planning and actuation. It introduces joint space
constraints to better represent the robot actual actuation capabilities. It
outlines a systematic method for the explicit consideration of these con-
straints from the task space, resulting in a dynamic and responsive con-
trol strategy. This innovative approach allows for the fine-tuning of task-
space movements while maintaining some level of "actuation aware-
ness" to ensure the robot stays within its operational limits.

Through experiments with Franka Emika robots, the relevance
and effectiveness of this control architecture are demonstrated in
scenarios where tasks are dynamically modified in real-time, show-
casing its potential in real-world applications of collaborative robotics.
# Contents

<table><tr><td>5.1</td><td>Introduction</td><td>103</td></tr><tr><td>5.1.1</td><td>Rationale: Feasible Online Motion Generation</td><td>103</td></tr><tr><td>5.1.2</td><td>Actuation-aware Control Framework</td><td>104</td></tr><tr><td>5.2</td><td>From Joint-space to Task-space Constraints</td><td>105</td></tr><tr><td>5.2.1</td><td>Task-space Feasible Sets as Convex Polyhedrons</td><td>106</td></tr><tr><td>5.2.2</td><td>Robot Actuation Capacities</td><td>107</td></tr><tr><td>5.2.3</td><td>From Joint-space Constraints to Task-space Feasible Motions</td><td>108</td></tr><tr><td>5.2.4</td><td>From Joint Bounds to Convex Reachable Workspace</td><td>111</td></tr><tr><td>5.3</td><td>Actuation-aware Task-space Re-planning</td><td>113</td></tr><tr><td>5.3.1</td><td>Some MPC notions and notations</td><td>113</td></tr><tr><td>5.3.2</td><td>About the Linear Inequality Constraints</td><td>114</td></tr><tr><td>5.3.3</td><td>Acceleration-based Position Re-planning</td><td>115</td></tr><tr><td>5.3.4</td><td>Position Re-planning Constraints</td><td>116</td></tr><tr><td>5.3.5</td><td>Position Re-planning Constraints Simulations</td><td>117</td></tr><tr><td>5.3.6</td><td>Acceleration-based Pose Re-planning</td><td>124</td></tr><tr><td>5.3.7</td><td>Pose Re-planning Constraints</td><td>125</td></tr><tr><td>5.4</td><td>Experimental Validation</td><td>127</td></tr><tr><td>5.4.1</td><td>Control Architecture Implementation</td><td>128</td></tr><tr><td>5.4.2</td><td>Experimental Setups & Software Infrastructure</td><td>129</td></tr><tr><td>5.4.3</td><td>Experimental Scenarios</td><td>131</td></tr><tr><td>5.4.4</td><td>Experimental Design & Objectives</td><td>131</td></tr><tr><td>5.4.5</td><td>About the Measured Result Quantities</td><td>133</td></tr><tr><td>5.4.6</td><td>Results: Joint Bounds Aware Planning</td><td>135</td></tr><tr><td>5.4.7</td><td>Results: Joint Velocity Modulation Task-space Planning</td><td>137</td></tr><tr><td>5.4.8</td><td>Discussion</td><td>144</td></tr><tr><td>5.5</td><td>Towards Viable Joint Constraints</td><td>145</td></tr><tr><td>5.6</td><td>Conclusion</td><td>149</td></tr></table>
**5.1 Introduction**

Previous chapters introduced a control strategy tailored for the requirements of col-
laborative robotics scenarios. For instance, these scenarios imply a dynamic shared
working environment (between the robot and the human). This entails the need for
the robot to adapt on-the-fly to a set of changing conditions that describe its envi-
ronment. To add to this, the task can also evolve throughout the collaborative work
cell operation depending on multiple factors like the task sequence, the physical
characteristics of the operator or certain unpredictable events that may occur. All
these complex conditions must be considered by the control architecture, which is in
charge of continuously providing a coherent answer that correctly adapts the robot
behaviour in ways that stay safe and properly exploit the robot capacities to perform
the productive activity.

This last characteristic can be further decomposed in multiple points. For instance, "continuously adapting" speaks to the real-time immediacy requirement of the adaptation. The robot needs to evaluate the new conditions and provide an answer in a timely manner not only because the performance requirements of the collaborative task demand it but also because human safety depends on these decisions. Secondly, this also implies that the robot must continuously switch between generated trajectories that properly consider the current task priorities and constraints. This transitions need to be smooth enough to avoid undesirable jerky behaviors and to stay within operating limits.

**5.1.1 Rationale: Feasible Online Motion Generation**

This last point is the focus of this chapter. Integrating the operating limits of a task is already a complex challenge because it requires the generated trajectories to remain valid with respect to arbitrary constraints. As proposed in Chapter 4 this is achievable thanks to optimization-based re-planning. Yet, the challenge is exacerbated by the layered control loop that must constantly take decisions that are coherent at all stages. In other words, the workspace constraints and/or task may impose some safety requirements (like limiting the operating velocity) but these need to be somehow linked to the robot actuation capabilities which highlights a crucial underlying question: What use is a trajectory that cannot be executed? Indeed, trajectory planning is implicitly intertwined with the control inputs and it should always (ideally) yield feasible trajectories.

Considering the robot actuation capabilities during task planning is complex enough
due to having to "articulate" task-space motion requirements to joint-space actua-
tors, which already entails choosing the optimal robot configuration to realize the
desired motion. Beyond that, doing so in a reactive manner (i.e. in real time) quickly
becomes an untractable problem. Evidently, the robustness of the planning algo-
rithms, meaning the capabiliy to consistently generate realizable trajectories is the
highest priority. This is why recurring to simplifications allow achieving the desired
robustness by accepting a performance loss trade off.

Indeed, often manufacturers offer constant Cartesian limits that can be used to solve
planning problems. This constitutes a simplifying strategy that allows modeling ac-
tuation capabilities with an acceptable approximation as a constant task space limit.
The tradeoff is two-sided: for one, sometimes this corresponds to undervaluing the
capabilities, provoking a performance loss but remaining robust from a feasibility
<figure><img src="image_42.png" /><figcaption>Figure 5.1: The proposed modular control architecture is based on a cascade loop composed of: 1) a task-space Model Predictive Controller (Model Predictive Control (MPC)) for online re-planning; 2) a constrained Task Space Inverse Dynamics solver to follow the planned trajectory. The whole control loop runs synchronously at the control rate. The Workspace Analysis block is in charge of sensing the environment, treating the information and transmitting the relevant information (workspace configuration and constraints) to the controller blocks.</figcaption></figure>

revisited ↑ ↓

point of view; and other times, due to the non-linear dynamics of the actuators, it may constitute an overvaluing. This last case could be considered potentially more dangerous as it breaks the underlying assumptions of the operating conditions, effectively increasing the risk of implicitly driving the system outside specifications *by design*.

Such situation is dangerous if it can put human lives at risk. Furthermore, inefficient use of the robot real capabilities also leads to dimensioning robots above the manufacturing task requirements, to compensate. In term, this means that potentially bigger and more dangerous [Joseph, 2018] robots are employed, provoking, yet again, an unnecessary safety risk.

### 5.1.2 Actuation-aware Control Framework ↑

This work focuses on the online motion adaptation of robot movements according to changing conditions and tasks in its environment. A concrete use case for this is the generation of trajectories safe for robot-human workspace sharing that are coherent with the robot capacities (feasible) and optimal with respect to some high-level criteria (like, for example, minimal energy expenditure).

The design principles underneath this control architecture remain consistent with the modular and task-centric point of view of this work. The modularity stems from
decoupling task-related behaviors and constraints from the actual execution in the robot, providing at least some level of independence from the robot architecture.

The proposal builds up on the previous chapter and is shown in Figure 5.1. The main focus of this chapter is the Workspace Analysis block that is generically named as a block in charge of measuring environment state and formulating it in a way that can be exploited by the control and planning blocks.

The task-centric design is revealed in the user configurability such as the ability to impose workspace constraints without being conditioned by the robot architecture or even the task. In other words, for example, safety constraints may be imposed as long as the re-planning algorithm exposes the related variables (for instance, in this case, the position and velocity). This means that task design, which can be defined with respect to the state of the collaborative assignment may be independent from the task constraints. Meanwhile, if the task constraints need to be enforced, there exists a mechanism that can limit the planned trajectories within these restrictions (along with other workspace constraints).

Another responsibility of this blocks is the convexification of the workspace con-
straints. For instance, the occupancy of the workspace may be defined by non-
convex polyhedrons, yet a convex space definition is required for most optimization-
based planning algorithms (such as the present MPC used). This convexification is
actually application-dependent but here is generically named as a way to find the
continuous space safe for the robot to perform a task without causing safety con-
cerns. A concrete application of these is modeling the human body or the obstacles
in the workspace, this way the robot can continuously consider them while perform-
ing its task.

Finally, the biggest focus of this chapter, is the consideration of the robot real capaci-
tites. The proposal is to exploit a mathematical formalism to formulate the joint level
constraints in the same space as the planning algorithm (the task space). This way,
the optimization planning can actually have a more complete picture of the set of
constraints that need to be considered in real-time. Not only does this help reduce
or eliminate the guessing work of finding the fastest trajectory for a task-space task,
but it also helps compatibility the planned trajectory with the actuation capabilities
of the tracking controller.

## 5.2 From Joint-space to Task-space Constraints ↑

As mentioned in the introduction, one of the challenges of task-space planning is to correctly consider the actuation capacities that belong in the configuration space of the robot. Fortunately, some mathematical formalisms allow representing these capacities in the cartesian space.

The proposal in this work is to exploit the linear inequality mechanisms present in *Quadratic Program (QP)* optimization. This way, if the *Joint-space Constraints (JSC)* can somehow be represented as linear inequalities of task-space variables (i.e. those present in the *Model Predictive Control (MPC)*), they can be directly embedded into the problem constraints.
<figure><img src="image_43.png" /><figcaption>Figure 5.2: Zoom into the Workspace Analysis Block. It allows transforming user-defined workspace limits, joint-space capacitites and the task state into the “workspace configuration”, which can be exploited as constraints and/or in the task function by the planner.</figcaption></figure>

revisited ↑ ↓

This section lies the foundations on how the JSC can be projected on to the Cartesian-space to provide the mathematical support needed actuation-aware task-space re-planning.

### 5.2.1 Task-space Feasible Sets as Convex Polyhedrons ↑

The JSC, once projected on to the Cartesian-space, define feasible sets. These sets relate the Cartesian-space motion quantities that are actually coherent with the actuation capabilities of the robot. The feasible sets that are introduced in this section all purposely share the convexity property, which makes it possible for them to take the form of a convex polyhedral through different methods [Gouttefarde et al., 2010]; [Skuric et al., 2021a]. These polyhedral can be defined in a generic way as:

$$ \mathcal{P}_{x|y} = \{x \in \mathbb{R}^n \mid x = Ay + b, y \in [y_m, y_M]\} \qquad (5.1) $$

where $\mathcal{P}$ defines a convex polyhedron<fnref n="1" /> over $x$ due to the bounds on $y$ [Fukuda et al., 2004]. This definition implies that the interval domain of the $y$ variable defines a projected constrained space on the $x$ variable through the linear transformation operator $A$. This constrained space is shifted from the origin by $b$. The shape of the projected space is only affected by $A$ and $b$ determines affine spaces through a “bias” or displacement in the domain of $x$.

The interest in these convex sets lies in that they admit half-plane representations (or H-rep for short). In other words, they can be reformulated as linear inequalities over the variable $x$, allowing to effectively compress the domain information of $y$ into the space of interest. The half-plane linear inequalities are defined as:

$$ C_x x \le d_x \quad (x \in \mathcal{P}_{x|y}) \qquad (5.2) $$

A concrete application of this is the main focus of this chapter. Task-centric motion planning focuses on the Cartesian space. Meanwhile, the actuation capabilities of a

<footnote n="1">¹Bounded convex polyhedron are also referred to as polytopes [Fukuda et al., 2004].</footnote>
robot belong in the configuration space. The formula in (5.1) allows the expression of the actuation capabilities as convex sets in the Cartesian space.

Assume that the task-space domain is represented by the variable $x$ and the joint-space by the variable $y$. The major hypothesis for actuation-aware task-space planning is that if a trajectory defined in the domain of $x(t)$ belongs in the set $P_{x|y}(y)$ (the Cartesian-space Constraints (CSC)), then it will also be compatible with the constraints on $y$ (the JSC).

Should this hypothesis prove true, this constitutes a major advantage of the convex sets representations as it allows to represent the actual robot capacities and embed them in a QP for linear MPC as linear inequalities.

A comprehensive introduction to polyhedral sets applied to robotics and biomechanics can be found in Antun Skuric's Thesis<fnref n="2" />, including a way to approximate the robot reachable space [Skuric et al., 2022c] and human-robot collaboration scenarios based on an estimate of the human wrench capabilities [Skuric et al., 2021b]; [Skuric et al., 2021a].

### 5.2.2 Robot Actuation Capacities ↑

The interest of this work lies in generating “smooth” and feasible trajectories. From a mathematical standpoint, this translates to generating $C^2$-continuous joint trajectories $q(t) \in \mathbb{R}^n$ (for an n-dof serial robot) such that:

$$ 
\begin{aligned}
\dot{Q} &= \{q(t) \in \mathbb{R}^n \mid q \in [q_m, q_M]\} \\
\dot{\ddot{Q}} &= \{\dot{q}(t) \in \mathbb{R}^n \mid \dot{q} \in [\dot{q}_m, \dot{q}_M]\} \\
\ddot{\ddot{Q}} &= \{\ddot{q}(t) \in \mathbb{R}^n \mid \ddot{q} \in [\ddot{q}_m, \ddot{q}_M]\}
\end{aligned} 
\quad (5.3) \quad \text{revisited ↑}
$$

where $Q$ designates the set of feasible joint configurations; $\dot{Q}, \ddot{Q}, \dddot{Q}$ respectively refer to velocity, acceleration and jerk constraints, that form the kinematic JSC. More specifically, the desired $q(t)$ is jerk bounded and is smooth up to the second derivative $\ddot{q}(t)$.

The robot motors are non-linear actuators that vary their behaviour according to the exerted joint velocities and torques. Modeling them requires a trade-off between complexity and correctly representing its behaviour. For instance, the dynamic robot model presented in Section 4.2.1 could be employed to bound the maximum joint accelerations from limits on the torque $\tau$ and its derivative $\dot{\tau}$.

Nevertheless, this work assumes constant joint acceleration and jerk capacities represented in $\dot{Q}, \ddot{Q}$ respectively. These are conveniently announced by the manufacturer [Franka Emika, n.d.] Even though this constitutes an approximation, it provides an acceptable framework that improves feasibility by going up to a higher order [Gallant et al., 2018].

<footnote n="2">²https://auctus-team.gitlabpages.inria.fr/team-members/antunskuric/</footnote>
### 5.2.3 From Joint-space Constraints to Task-space Feasible Motions ↑

The previous section presented the joint-space constraints modeling aspects of the robot. These constraints effectively determine a set of feasible motions that the robot is capable of realizing. Some mathematical formalizations are required in order to start connecting both quantities, which can be achieved through differential methods that project joint-space movements onto the Cartesian space.

A first step towards this objective is to extend the mathematical model introduced in previous chapters to relate robot reconfiguration trajectories to task-space motions. This can be achieved by revisiting the kinematic and dynamic aspects from previous chapters, through the subsequent time derivatives of the twist:

$$v = J(q)\dot{q} \qquad (5.4) \qquad \text{revisited } \uparrow$$

$$\dot{v} = J(q)\ddot{q} + \dot{J}(q, \dot{q})\dot{q} \qquad (5.5) \qquad \text{revisited } \uparrow$$

$$\ddot{v} = J(q)\ddot{q} + \ddot{J}(q, \dot{q}, \ddot{q})\dot{q} + 2\dot{J}(q, \dot{q})\ddot{q} \qquad (5.6)$$

$$v, \dot{v}, \ddot{v} \in \mathfrak{sc}(3) \qquad q, \dot{q}, \ddot{q} \in \mathbb{R}^n$$

where $J$ refers to the robot analytical Jacobian and $\dot{J}, \ddot{J}$ to its time derivatives; moreover, $v, \dot{v}, \ddot{v}$ respectively denote the twist, spatial acceleration and jerks generated by a given joint velocity $\dot{q}$, acceleration $\ddot{q}$ and jerk $\dddot{q}$.

In order to differentiate between linear (as in positional displacement) and angular motions, a simple notation addendum denotes both parts of the Jacobian and its derivatives:

$$J = \begin{bmatrix} J_l \\ J_a \end{bmatrix} \qquad \dot{J} = \begin{bmatrix} \dot{J}_l \\ \dot{J}_a \end{bmatrix} \qquad \ddot{J} = \begin{bmatrix} \ddot{J}_l \\ \ddot{J}_a \end{bmatrix} \qquad (5.7)$$

$$J_l, \dot{J}_l, \ddot{J}_l, J_a, \dot{J}_a, \ddot{J}_a \in \mathbb{R}^{3 \times n}$$

which separate the upper and lower (in the convention used here) parts of the matrices.

Using the linear components of (5.4) to (5.6) along with the robot capacities as introduced in (5.3) allows for a first approach at relating the JSC to define the feasible sets of Cartesian variables of the same orders, i.e. velocity, acceleration and jerk, which in this case are denoted as $\dot{p}, \ddot{p}, \dddot{p}$ respectively to make their relation to the position time derivative clear as well as making a distinction from the 6D pose motions variables. The sets are defined as:

$$\mathcal{P}_{\dot{p}|\dot{Q}}(q_k) = \{\dot{p} \in \mathbb{R}^3 | \dot{p} = J_l(q_k)\dot{q}, \dot{q} \in \dot{Q}\} \qquad (5.8) \qquad \text{revisited } \downarrow$$

$$\mathcal{P}_{\ddot{p}|\ddot{Q}}(q_k, \dot{q}_k) = \{\ddot{p} \in \mathbb{R}^3 | \ddot{p} = J_l(q_k)\ddot{q} + B_{\ddot{p}}(q_k, \dot{q}_k), \ddot{q} \in \ddot{Q}\} \qquad (5.9)$$

$$B_{\ddot{p}}(q_k, \dot{q}_k) = \dot{J}_l(q_k, \dot{q}_k)\dot{q}_k$$

$$\mathcal{P}_{\ddot{p}|\ddot{Q}}(q_k, \dot{q}_k, \ddot{q}_k) = \{\ddot{p} \in \mathbb{R}^3 | \ddot{p} = J_l(q_k)\ddot{q} + B_{\ddot{p}}(q_k, \dot{q}_k, \ddot{q}_k), \ddot{q} \in \ddot{Q}\} \qquad (5.10)$$

$$B_{\ddot{p}}(q_k, \dot{q}_k, \ddot{q}_k) = 2\dot{J}_l(q_k, \dot{q}_k)\ddot{q}_k + \ddot{J}_l(q_k, \dot{q}_k, \ddot{q}_k)\dot{q}_k$$

where $q_k, \dot{q}_k, \ddot{q}_k$ denote the measured robot state at a time instant $k$; the
<figure><img src="image_44.png" /><figcaption>Figure 5.3: The translational velocity polyhedron. It visualizes the feasible velocity set $P_{\vec{p}|\vec{Q}}$ as defined in (5.8) once transformed into the “equivalently occupied space”. Multiplying the set by a small time interval $\Delta t = 0.15$ corresponds to the space the end-effector would move to at a constant speed for the time period. This visualization shows two distinct robot configurations: The first one represents a commonly used initial pose. The second configuration serves as an arbitrary example, illustrating how feasible velocities vary depending on the alignment of the robot actuators. This approach helps provide a clear depiction of the robot instantly possible velocities.</figcaption></figure>

sets $P_{\vec{p}|\vec{Q}}$, $P_{\vec{j}|\vec{Q}}$, $P_{\vec{p}^{\ddot{}}|\vec{Q}^{\ddot{}}}$ respectively designate the feasible velocities, accelerations and jerks due to the joint-space capacities in velocities, accelerations and jerks in each case. The dependencies on the measured state were explicitly added to show that some terms do not affect the projection of the joint capacities onto the Cartesian space but rather act as a constant bias or displacement over the origin. These biases are denoted as $B_{\vec{p}^{\ddot{}}}$, $B_{\vec{j}^{\ddot{}}}$ for the acceleration and jerk, respectively.

Note that these biases, as displacements from the origin, they augment the risk of producing an incompatible optimization problem causing the QP solver to fail. In practice, the joint velocity and acceleration measures are also noisy, contributing to a degraded precision in the capacity sets. A strategy adopted in this work is to consider them approximately zero:

$$B_{\vec{p}}(q_k, \dot{q}_k) \approx 0 \qquad B_{\vec{j}^{\ddot{}}}(q_k, \dot{q}_k, \ddot{q}_k) \approx 0 \qquad (5.11)$$

which a relatively good assumption at low to moderate speeds. When operating at higher velocities, eliminating these biases also allows to always include the origin in
the intersection set, increasing the chances of obtaining a compatible QP problem.

In order to visualize the feasible velocities set $\mathcal{P}_{\dot{\mathbf{p}}|\dot{\mathcal{Q}}}$ defined in (5.8), a simple fashion consists in converting it to the “equivalently occupied space” of moving at constant speed for a small time delta. To do this, the set can be multiplied by a $\Delta t = 0.15$ to obtain the visualization in Figure 5.3, which is shown for two different robot configurations: the first one corresponds to a rather neutral pose commonly used as a starting configuration; the second configuration shows an arbitrary example of how the feasible velocities evolve according to the alignment of the actuators.

Note that (5.8) to (5.10) effectively take the same convex form as the definition in (5.1), making it possible to be re-formulated as linear inequalities.

Additionally, the sets in (5.8) to (5.10) model the attainable motions in the Cartesian space *without* constraining the orientation of the body. In other words, these formulations do not constraint the angular motion component, effectively leaving complete freedom for the body to change orientation during these motions. This might pose a problem when trying to plan a pure translational trajectory with a fixed orientation. In such case, these definitions need to further constraint the rotational motion to a null value:

$$ \mathcal{L}_{\dot{\mathbf{p}}|\dot{\mathcal{Q}}}(\mathbf{q}_k) = \{ \dot{\mathbf{p}} \in \mathbb{R}^3 \mid \dot{\mathbf{p}} = J_l(\mathbf{q}_k)\dot{\mathbf{q}}, J_a(\mathbf{q}_k)\dot{\mathbf{q}} = \mathbf{0}_3, \dot{\mathbf{q}} \in \dot{\mathcal{Q}} \} \quad (5.12) $$

revisited ↑ ↓

$$ \mathcal{L}_{\ddot{\mathbf{p}}|\ddot{\mathcal{Q}}}(\mathbf{q}_k, \dot{\mathbf{q}}_k) = \{ \ddot{\mathbf{p}} \in \mathbb{R}^3 \mid \ddot{\mathbf{p}} = J_l(\mathbf{q}_k)\ddot{\mathbf{q}} + B_{\ddot{\mathbf{p}}}, J_a(\mathbf{q}_k)\ddot{\mathbf{q}} = \mathbf{0}_3, \ddot{\mathbf{q}} \in \ddot{\mathcal{Q}} \} \quad (5.13) $$

$$ \mathcal{L}_{\ddot{\mathbf{p}}' | \ddot{\mathcal{Q}}}(\mathbf{q}_k, \dot{\mathbf{q}}_k, \ddot{\mathbf{q}}_k) = \{ \ddot{\mathbf{p}}' \in \mathbb{R}^3 \mid \ddot{\mathbf{p}}' = J_l(\mathbf{q}_k)\ddot{\mathbf{q}}' + B_{\ddot{\mathbf{p}}'}, J_a(\mathbf{q}_k)\ddot{\mathbf{q}}' = \mathbf{0}_3, \ddot{\mathbf{q}}' \in \ddot{\mathcal{Q}} \} \quad (5.14) $$

where $\mathbf{0}_3$ designates a zero-valued 3-vector; the bias terms $B_{\ddot{\mathbf{p}}}, B_{\ddot{\mathbf{p}}'}$ coincide with their previous definitions and their dependency on the measured states $\mathbf{q}_k, \dot{\mathbf{q}}_k, \ddot{\mathbf{q}}_k$ was dropped for clarity. The added equality constraint on the angular component effectively restricts the joint motions to those that act on the kernel of the angular Jacobians and its derivatives $J_a, \dot{J}_a, \ddot{J}_a$. This way, the sets designated with $\mathcal{L}$ serve to clearly denote the purely translational feasible motions.

Finally, these definitions helped build an intuitive approach to the feasible sets while also drawing attention to the coupling effect of the model on the orientation and translation. However, the interest of this works lies in full pose trajectories planning. Thus, the same sets must be extended for 6D motions, which fortunately can be done in a straightforward way:

$$ \mathcal{P}_{\nu|\dot{\mathcal{Q}}}(\mathbf{q}_k) = \{ \nu \in \mathfrak{se}(3) \mid \nu = J(\mathbf{q}_k)\dot{\mathbf{q}}, \dot{\mathbf{q}} \in \dot{\mathcal{Q}} \} \quad (5.15) $$

revisited ↑ ↓

$$ \mathcal{P}_{\dot{\nu}|\ddot{\mathcal{Q}}}(\mathbf{q}_k, \dot{\mathbf{q}}_k) = \{ \dot{\nu} \in \mathfrak{se}(3) \mid \dot{\nu} = J(\mathbf{q}_k)\ddot{\mathbf{q}} + B_{\dot{\nu}}(\mathbf{q}_k, \dot{\mathbf{q}}_k), \ddot{\mathbf{q}} \in \ddot{\mathcal{Q}} \} \quad (5.16) $$

$$ B_{\dot{\nu}}(\mathbf{q}_k, \dot{\mathbf{q}}_k) = \dot{\mathcal{J}}(\mathbf{q}_k, \dot{\mathbf{q}}_k)\dot{\mathbf{q}}_k $$

$$ \mathcal{P}_{\ddot{\nu}|\ddot{\mathcal{Q}}}(\mathbf{q}_k, \dot{\mathbf{q}}_k, \ddot{\mathbf{q}}_k) = \{ \ddot{\nu} \in \mathbb{R}^3 \mid \ddot{\nu} = J(\mathbf{q}_k)\ddot{\mathbf{q}} + B_{\ddot{\nu}}(\mathbf{q}_k, \dot{\mathbf{q}}_k, \ddot{\mathbf{q}}_k), \ddot{\mathbf{q}} \in \ddot{\mathcal{Q}} \} \quad (5.17) $$

$$ B_{\ddot{\nu}}(\mathbf{q}_k, \dot{\mathbf{q}}_k, \ddot{\mathbf{q}}_k) = 2\dot{\mathcal{J}}(\mathbf{q}_k, \dot{\mathbf{q}}_k)\ddot{\mathbf{q}}_k + \ddot{\mathcal{J}}(\mathbf{q}_k, \dot{\mathbf{q}}_k, \ddot{\mathbf{q}}_k)\dot{\mathbf{q}}_k $$

where most of the previous conclusions still stand but the notation should make it clear now that the full Jacobian matrix and its derivatives are used. This way, the full pose derivative variables can be constrained (i.e. the twist $\nu$, spatial acceleration $\dot{\nu}$ and jerk $\ddot{\nu}$).
### 5.2.4 From Joint Bounds to Convex Reachable Workspace ↑

The reachable workspace due to the joint bounds $Q$ is purposely omitted from the previous section because its consideration requires some special treatment. To begin, the reachable workspace may be modeled as the set of reachable positions or poses in the workspace. For a first approach, the focus is kept on the positional sense of the set.

For instance, the set of feasible positions that can be achieved by a robot end-effector can be defined as:

$$ \mathcal{P}_{p|Q}(q_k) = \{ \mathbf{p} \in \mathbb{R}^3 | \mathbf{p} = \text{FK}(\mathbf{q}), \mathbf{q} \in [q_m, q_M] \} \quad (5.18) $$

which, unlike the definitions from the previous section, employs a non-linear Forward Kinematics (FK) function<fnref n="3" />. This provokes that no convexity characteristics can be inferred from the set $\mathcal{P}_{p|Q}$, which is a problem for the intended use in this work (to be embedded into optimization algorithms).

To circumvent this problem, a convex set can be constructed through a variational approach, by employing a first order expansion on the position. Assuming a discretized position trajectory at a time step k, yields:

$$ \mathbf{p}_{k+1} = \mathbf{p}_k + d\mathbf{p}_k \qquad d\mathbf{p}_k = J_l d\mathbf{q}_k \quad (5.19) $$

where the main interest lies in computing $\mathbf{p}_{k+1}$ given a configuration differential $dq$. Then, redefining this infinitely small variation to evaluate the available joint motion (signifying it is no longer assumed to be small) yields a configuration delta $\Delta q$. It can be computed to be the distance towards each of the joint bounds to define a set with respect to the current configuration:

$$ \Delta \mathbf{q}_k \in [\Delta \mathbf{q}_m(\mathbf{q}_k), \Delta \mathbf{q}_M(\mathbf{q}_k)] \qquad \Delta \mathbf{q}_m(\mathbf{q}_k) = \mathbf{q}_m - \mathbf{q}_k \qquad \Delta \mathbf{q}_M(\mathbf{q}_k) = \mathbf{q}_M - \mathbf{q}_k \quad (5.20) $$

Finally, integrating these expressions allows defining a convex reachable space set as:

$$ \hat{\mathcal{P}}_{p|Q}(\mathbf{q}_k) = \{ \mathbf{p} \in \mathbb{R}^3 | \mathbf{p}(\Delta\mathbf{q}), \Delta\mathbf{q} \in [\Delta\mathbf{q}_m, \Delta\mathbf{q}_M] \} \qquad (5.21) \\ \mathbf{p}(\Delta\mathbf{q}) \sim \mathbf{p}_k + J_l(\mathbf{q}_k)\Delta\mathbf{q} \qquad \mathbf{p}_k = \text{FK}(\mathbf{q}_k) $$

revisited ↓

where this time around, the term $\mathbf{p}_k$ provides the constant bias for the convex set and can be computed by using the FK on the measured robot configuration $\mathbf{q}_k$. The definitions for $\Delta \mathbf{q}_m, \Delta \mathbf{q}_M$ are provided in (5.20).

This definition constitutes a rough approximation of the instantaneously feasible space, because, in reality, this space is not necessarily convex. Yet, for configurations $\mathbf{q}$ relatively close to the joint bounds (small $\Delta \mathbf{q}$ values), the approximation becomes progressively better:

$$ \lim_{\Delta \mathbf{q} \to 0} \hat{\mathcal{P}}_{p|Q}(\mathbf{q}_k) \approx \mathcal{P}_{p|Q}(\mathbf{q}_k) \quad (5.22) $$

Moreover, bigger $\Delta \mathbf{q}$ values imply that the joint bounds are far enough that even

<footnote n="3">³Prioritizing notation clarity over consistency, the Forward Kinematics function is denoted with the same name (as FK($\mathbf{q}$)) both in the *position* sense (which outputs a 3-vector position) as well as in the *full pose* sense, which outputs a homogeneous transformation matrix in $\mathbb{SE}(3)$.</footnote>
if the approximation is bad, their consideration is not urgent. In a worst case scenario that the robot motion tends towards the joint bounds, the increasingly better instantaneous precision of the approximation helps appropriately consider the joint bounds, provided that the convex set is re-computed frequently (which is an acceptable assumption in collaborative robotics scenarios that require reactivity).

The reachable space is visualized in Figure 5.4 for two configurations. The figure shows $P_{p|Q}$ from (5.21) with a Jacobian $J_l$ multiplied by a factor of 0.15. This factor is necessary for visualization purposes because the approximation is rather coarse in extremes that correspond to the farthest from the joint bounds. The first configuration, shown in Figure 5.4(a), corresponds to a commonly used starting pose. This is a rather neutral configuration and it shows a symmetrical reachable space. In contrast, Figure 5.4(b) shows an extreme case of a twisted configuration where the robot was purposely put near the joint bounds. In such case, the approximation of the reachable space through the projections in the direction of the joint bounds becomes more precise and, as shown in the image, the robot is completely unable to move in this direction.

Just like with (5.8) to (5.10), the convex reachable space definition in (5.21) works under the assumption of an unrestricted orientation. In order to approximate the reachable space that can be attained without reorienting the body, the same principles introduced for (5.12) to (5.14) can be applied. This further restricts the joint configuration motions to the kernel of the angular component of the Jacobian through an extra equality constraint:

$$ \mathcal{L}_{p|Q}(q_k) = \{ \mathbf{p} \in \mathbb{R}^3 \mid \mathbf{p}(\Delta\mathbf{q}), J_a(q_k)\Delta\mathbf{q} = \mathbf{0}_3, \Delta\mathbf{q} \in [\Delta q_m, \Delta q_M] \} \quad (5.23) $$

revisited ↑ ↓

where $\Delta q_m, \Delta q_M$ are defined in (5.20) and $\mathbf{p}(\Delta\mathbf{q})$ is defined in (5.21).

The same definition in (5.18) can be made over the realizable poses of the robot as:

$$ \mathcal{P}_{\chi|Q}(q_k) = \{ \chi \in \mathbb{S}\mathbb{E}(3) \mid \chi = \text{FK}(q), q \in [q_m, q_M] \} \quad (5.24) $$

again without any potential inferences on the convexity properties of the set. However, the same expression in (5.21) cannot be straightforwardly extended for the poses. However, because this time the objective is to also represent the feasible orientations, the additional restrictions from (5.23) are not necessary. In fact, in order to define a convex set that represents the instantaneous reachable poses, it is required to go through an auxiliary variable: the exponential pose representations introduced in a general way in Chapter 2 and in a more applied way (to MPC) in Chapters 3 and 4.

To do so, the set is to be defined on to the logarithm of the pose $\xi = \log(\chi)$. Then, revisiting the first-order algebraic expression in the tangent space one obtains:

$$ \xi_{k+1} = \log(e^{\xi_k} e^{\Delta t v_k}) \approx \xi_k + \text{dlog}_{\xi_k} \Delta t v_k \quad (5.25) $$

revisited ↑

that relates the pose logarithm $\xi$ with the twist $v$. Now, applying the same logic as in (5.21) is possible:

$$ \hat{P}_{\xi|Q}(q_k) = \{ \xi \in \mathfrak{se}(3) \mid \xi(\Delta q), \Delta q \in [q_m - q_k, q_M - q_k] \} \quad (5.26) \\ \xi(\Delta q) \sim \xi_k + \text{dlog}_{\xi_k} J \Delta q \qquad \xi_k = \log(\chi_k) $$

revisited ↑ ↓

where $P_{\xi|Q}$ defines a convex set over the pose logarithm. This set is defined on
the tangent space of the poses, $sc(3)$. The interpretability of this expression is not directly translated for the space of the poses, $SE(3)$. However, for optimization purposes, as the proposed approach in this work is based on using the pose tangent space, this effectively provides the necessary means to limit the solution space. Then, once the optimization is finished, the actual pose can be recovered by computing the exponential of the solution.

## 5.3 Actuation-aware Task-space Re-planning ↑

The current work is concerned with the online generation of trajectories subject to constraints originated from safety risks and/or feasibility restrictions, due to the limited actuation capacities of the robot.

The core contribution of this chapter concerns how to integrate the feasibility constraints introduced in Sections 5.2.3 and 5.2.4 into the task-space re-planning scheme of the proposed Model Predictive Control (MPC).

### 5.3.1 Some MPC notions and notations ↑

This section quickly revisits some definitions for generic MPC from Chapter 4 that are necessary for the contributions provided in the current chapter.

Assuming a d-sized state and m-sized input system, it can be described with a generic discrete time linear system (indexed with n) as

$$x_{n+1} = Ax_n + Bu_n \quad A \in \mathbb{R}^{d \times d}, B \in \mathbb{R}^{d \times m} \quad (5.27) \quad \text{revisited} \uparrow$$

where $A$ and $B$ represent its state and input matrices whereas $x$ and $u$ are the state and input vectors. Smooth point to point pose motion generation can be achieved by generating trajectories at the acceleration level. Choosing the control input at the task level to be the acceleration, allows predicting the evolution of the end effector (system) state — pose and twist — over a short period of time to generate a jerk-bounded motion.

Discretizing a receding time window in h time steps of duration T starting at time $t_n$ (total duration hT) yields the discretized time steps inside the horizon, defined as $t_k = t_n + kT$ for $k = 0, 1, \dots, h$ and one can obtain $x(t_k) = x_{k|n}$, $u(t_k) = u_{k|n}$. A linear state propagation over the receding horizon expression is defined as:

$$x_{k+1|n} = Ax_{k|n} + Bu_{k|n} \quad \text{for } k = 0, 1, \dots, h-1 \quad (5.28) \quad \text{revisited} \uparrow$$

Subsequent sections drop the $n$ subscript to simplify the notation, making $x_{k|n}$ equivalent to $x_k$. This propagation can be synthesized into a simple linear algebraic expression:

$$\bar{X}(x_0, \underline{u}) = \hat{A}\underline{u} + \hat{B}x_0 \quad (5.29) \quad \text{revisited} \uparrow$$
where:

$$ \underbrace{\begin{bmatrix} x_1 \\ x_2 \\ x_3 \\ \vdots \\ x_h \end{bmatrix}}_{\bar{x}} = \underbrace{\begin{bmatrix} B \\ AB & B \\ A^2 B & AB & B \\ \vdots \\ A^{h-1} B & A^{h-2} B & \dots & AB & B \end{bmatrix}}_{\bar{A}} + \underbrace{\begin{bmatrix} u_0 \\ u_1 \\ u_2 \\ \vdots \\ u_{h-1} \end{bmatrix}}_{\bar{u}} + \underbrace{\begin{bmatrix} A \\ A^2 \\ A^3 \\ \vdots \\ A^h \end{bmatrix}}_{\bar{B}} x_0 \quad (5.30) \quad \text{revisited } \uparrow $$

This formulation can be employed to define a pose motion generation problem as an optimization problem over the control input that minimizes the distance towards a target state. Because the state can be computed from a control input horizon, the optimization maintains the capability of recovering the state throughout the horizon, allowing to also consider state constraints while reducing the optimization variables (as it only needs to consider the control inputs), effectively reducing resolution times.

This problem is defined as a MPC control scheme that, thanks to the linear algebraic state propagation, can be formulated as a Quadratic Program (QP):

$$ \min_{u} \| \bar{X}_{t} - \bar{X}(x_{0}, \underline{u}) \|_{P}^{2} + \| \underline{u} \|_{R}^{2} \quad (5.31) \quad \text{revisited } \uparrow $$

$$ C_{x_m} \leq C_x x_k \leq C_{x_M} \quad (5.32) \quad \text{revisited } \uparrow $$

$$ C_{u_m} \leq C_u u_k \leq C_{u_M} \quad (5.33) \quad \text{revisited } \uparrow $$

$$ C_{\dot{u}_m} \leq C_{\dot{u}} \dot{u}_k \leq C_{\dot{u}_M} \quad (5.34) \quad \text{revisited } \uparrow $$

$$ \text{with: } \dot{u}_k = \frac{(u_{k+1} - u_k)}{T} $$

optimizing over the control inputs $u$. Where $P, R$ designate semi-positive definite matrices containing weighting terms for the input throughout the horizon and $\bar{X}_t$ corresponds to the target state vector repeated throughout the horizon.

Furthermore, (4.40) allows for an explicit expression of state bounds. Meanwhile, (4.41) allow a more generic expression of linear constraints over the control input. Note that in practice, for this formulation, (4.40) is also formulated as linear constraints over the control input, a consequence of (5.29). However, the versatility of this linear constraints is the main focus of the chapter, that is employed to achieve "actuation-aware" re-planning.

### 5.3.2 About the Linear Inequality Constraints $\uparrow$

The previous section presented a generic MPC formulation that can be applied for generalized systems. In the interest of motion generation with MPC in mind, the state and input of the system are specialized either for position control or for pose control.

In a collaborative robotics scenario, and contemplating the proposed control architecture, a common need from the control perspective is how to constraint the motions for needs that go beyond actuation limits. These may originate from safety needs (operating velocity), workspace limitations (for example, with safety zones or
virtual walls), task limitations (for example that explicitly require smooth motions), user preferences, etc. In fact, one can imagine any number of applications that impose arbitrary restrictions over the motion and these need to be considered by the planning algorithm continuously.

In order for these to be readily integrated into the task-space re-planning scheme proposed as an MPC, they need to be expressed as linear inequality constraints over either the state or the input of the system, as introduced in the most generic way that a QP admits in (5.32) to (5.34). Yet, they can also be used to formulate simple variable bounds or zonotopes [Bouchard et al., 2009]. For instance, the interval set of a variable defined as:

$$ \mathcal{P}_{y|\mathcal{Y}} = \{ \mathbf{y} \in \mathbb{R}^3 \mid \forall \mathbf{y} \in [\mathbf{y}_m, \mathbf{y}_M] \} \quad (5.35) $$

where $\mathbf{y}$ generically represents the variable constrained within some set $\mathcal{Y}$ with limits $\mathbf{y}_m, \mathbf{y}_M$ (its upper and lower bounds), also admits a linear inequality representation as:

$$ C_l \le C_y \mathbf{y} \le C_u \quad (5.36) $$

or equivalently:

$$ \begin{bmatrix} C_y \\ -C_y \end{bmatrix} \mathbf{y} \le \begin{bmatrix} C_u \\ -C_l \end{bmatrix} \quad (5.37) $$

where $C_y$ is an identity and $C_l, C_u$ contains the variable bounds. This last form shows that the linear inequality constraints can be used to embed complex constraints in the form of (5.2), because (5.35) is in fact a special case of (5.1).

The objective of this section is to show how this kind of formulation can be used to express Cartesian-space Constraints (CSC), either from the feasibility sets introduced in Section 5.2 or from other workspace / task restrictions.

### 5.3.3 Acceleration-based Position Re-planning ↑

For a pure translational motion re-planning (i.e. position control), the generic MPC formulations presented in Section 5.3.1 need to be specialized. More concretely, the system state and input need to be defined. Based on a second order expansion of the position:

$$ \mathbf{p}_{k+1} = \mathbf{p}_k + \mathrm{T} \dot{\mathbf{p}}_k + \frac{T^2}{2} \ddot{\mathbf{p}}_k \quad (5.38) $$

$$ \dot{\mathbf{p}}_{k+1} = \dot{\mathbf{p}}_k + \mathrm{T} \ddot{\mathbf{p}}_k \quad (5.39) $$

one can define a system based on the acceleration as the control input:

$$ \mathbf{x}_k = \begin{bmatrix} \mathbf{p}_k \\ \dot{\mathbf{p}}_k \end{bmatrix} \qquad \mathbf{u}_k = \ddot{\mathbf{p}}_k \qquad \mathbf{x}_t = \begin{bmatrix} \mathbf{p}_t \\ \mathbf{0} \end{bmatrix} \quad (5.40) $$

where $\mathbf{p}, \dot{\mathbf{p}}$ respectively designate the position and velocity and $\ddot{\mathbf{p}}$ denotes the acceleration. Meanwhile, $\mathbf{x}_t$ contains the target state: the target position and a null velocity. Then, to complete the state-space system definition, the expressions for
the A, B matrices are required:

$$A_k = \begin{bmatrix} \mathbf{I}_3 & \mathbf{T} \mathbf{I}_3 \\ \mathbf{0}_3 & \mathbf{I}_3 \end{bmatrix} \qquad B_k = \begin{bmatrix} \frac{T^2}{2} \mathbf{I}_3 \\ \mathbf{T} \mathbf{I}_3 \end{bmatrix} \qquad (5.41)$$

which provide all the necessary elements to complete the acceleration-based definition of the MPC for reactive position re-planning.

### 5.3.4 Position Re-planning Constraints ↑

In the interest of safety-aware planning, a multitude of approaches can be embedded as constraints. For instance, some strategies to increase safety through velocity modulation employ damage risks mitigation measures [Palleschi et al., 2021]; furthermore the technical specification for collaborative robotics contemplates velocity-limited operation zones [ISO/TS-15066, 2016]. In terms of position control, safety-related zones and velocity limits can be denoted with an S as:

$$\mathcal{P}_{p|S} = \{ \boldsymbol{p} \in \mathbb{R}^3 \mid \forall \boldsymbol{v} \in [\boldsymbol{p}_{m|S}, \boldsymbol{p}_{M|S}] \} \qquad (5.42)$$

$$\mathcal{P}_{\dot{\boldsymbol{p}}|S} = \{ \dot{\boldsymbol{p}} \in \mathbb{R}^3 \mid \forall \dot{\boldsymbol{p}} \in [\dot{\boldsymbol{p}}_{m|S}, \dot{\boldsymbol{p}}_{M|S}] \} \qquad (5.43)$$

where $\mathcal{P}_{p|S}$ designates the space deemed safe for the robot to operate in; while $\mathcal{P}_{v|S}$ designates the maximum operating velocity.

Another way to effectively reduce the reachable geometrical workspace is through a time-varying version of $\mathcal{P}_{v|S}(t)$, by gradually reducing the maximum velocities when the robot approaches the limits of the allowed space. However, while some constraints must be time-varying, in this case, it is counter-productive as it adds unnecessary dynamics to constraints that can already be formulated explicitly, making the horizon re-planning more robust.

Meanwhile, manufacturers also provide task-space related maximum velocities, accelerations and jerks, which can be denoted with an M as:

$$\mathcal{P}_{\dot{\boldsymbol{p}}|M} = \{ \boldsymbol{v} \in \mathbb{R}^3 \mid \forall \dot{\boldsymbol{p}} \in [\dot{\boldsymbol{p}}_m, \dot{\boldsymbol{p}}_M] \} \qquad (5.44)$$

$$\mathcal{P}_{\ddot{\boldsymbol{p}}|M} = \{ \boldsymbol{v} \in \mathbb{R}^3 \mid \forall \ddot{\boldsymbol{p}} \in [\ddot{\boldsymbol{p}}_m, \ddot{\boldsymbol{p}}_M] \} \qquad (5.45)$$

$$\mathcal{P}_{\dddot{\boldsymbol{p}}|M} = \{ \boldsymbol{v} \in \mathbb{R}^3 \mid \forall \dddot{\boldsymbol{p}} \in [\dddot{\boldsymbol{p}}_m, \dddot{\boldsymbol{p}}_M] \} \qquad (5.46)$$

These kind of constraints make sense from a usability point of view as they are easy to interpret and practical from a task-centric design proposed in this work. Yet, the linear inequalities form show that they could potentially be used to express more complex convex sets like the ones introduced in Section 5.2.1 that admit the same form as (5.1). For instance, this means that the shape of the allowed geometrical space in $\mathcal{P}_{p|S}$ does not need to be rectangular (as in virtual walls parallel to the world frame planes). These arbitrary and application-dependent sets are denoted with A.
Altogether, the intersection of all set of CSC related to the task-space, are denoted as *workspace* constraints with a *W*:

$$P_{p|W} = P_{p|S} \cap P_{p|M} \cap P_{p|A} \qquad (5.47)$$

$$P_{\dot{p}|W} = P_{\dot{p}|S} \cap P_{\dot{p}|M} \cap P_{\dot{p}|A} \qquad (5.48)$$

$$P_{\ddot{p}|W} = P_{\ddot{p}|S} \cap P_{\ddot{p}|M} \cap P_{\ddot{p}|A} \qquad (5.49)$$

$$P_{\ddot{p}|W} = P_{\ddot{p}|S} \cap P_{\ddot{p}|M} \cap P_{\ddot{p}|A} \qquad (5.50)$$

Finally, the focus of this section is on how to add actuation awareness onto the task-space re-planning, while also considering the workspace constraints. To complete the position control MPC problem that contemplates all these restrictions, this definition is necessary:

$$p_k = \mathrm{FK}(q_k) \qquad (5.51)$$

$$\dot{p}_k = J_l q_k \qquad (5.52)$$

$$p \in P_p \qquad P_p = P_{p|Q}(q_k) \cap P_{p|W} \qquad (5.53)$$

$$\dot{p} \in P_{\dot{p}} \qquad P_{\dot{p}} = P_{\dot{p}|\dot{Q}}(q_k) \cap P_{\dot{p}|W} \qquad (5.54)$$

$$\ddot{p} \in P_{\ddot{p}} \qquad P_{\ddot{p}} = P_{\ddot{p}|\ddot{Q}}(q_k, \dot{q}_k) \cap P_{\ddot{p}|W} \qquad (5.55)$$

$$\ddot{p} \in P_{\ddot{p}} \qquad P_{\ddot{p}} = P_{\ddot{p}|\ddot{Q}}(q_k, \dot{q}_k, \ddot{q}_k) \cap P_{\ddot{p}|W} \qquad (5.56)$$

where $p_k, \dot{p}_k$ respectively designate the initial position and velocity. These equations express the convex sets that need to be to be formulated as linear constraints as in (4.40) and (4.41). If fixed orientation is required along with position re-planning, the feasible sets may be replaced with their pure translation equivalent based on $\mathcal{L}$, as defined in Sections 5.2.3 and 5.2.4.

### 5.3.5 Position Re-planning Constraints Simulations ↑

In order to offer an intuitive comprehension of the effects of the constraints as formulated previously, this section presents a series of simulations. The simulations included contemplate 3 scenarios:

1. Zonotope constraints with zero initial conditions shown in Figure 5.5

2. Zonotope constraints with non-zero initial conditions shown in Figure 5.6

3. Polyhedral constraints with non-zero initial conditions shown in Figure 5.7

In all cases, a single horizon with a fixed target position was computed and visualized. The horizon duration is hT=4s (as shown in the trajectories), with a configuration h=100 and T=40ms. The input weight is $\gamma = 5^{T_2/2}$, and the terminal state weight is:

$$P_T = \begin{bmatrix} \mathbf{I}_{3 \times 3} & 100 & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{I}_{3 \times 3} & 100T \end{bmatrix} \qquad (5.57)$$
as defined in (4.29) and (4.30).

Furthermore, in all the scenarios, the target position is outside of the position con-
straints $P_p$ to showcase what happens when the target is unreachable. In such case,
the distance-minimizing cost function as formulated in the MPC drives the system
towards it until it reaches the closest point within the constraints.

On the same note, it can also be seen in all velocity horizons that the ending velocity
tends towards the origin (i.e. a zero value). This is a feature of the definition in (5.40)
that drives the robot towards the target while also trying to inhibit the motion. This
definition has a double effect: for one, it favors a stable convergence towards the
target position (to arrive at a motionless state); the second effect is related to the
fail safe mechanism: in case the MPC optimization fails, the robot will continue to
execute the last successfully computed horizon. This target state ensures the horizon
always converges to a more controllable (slower) state, which should in theory also
increase the chances of spontaneously resolving the situation.

Another significant result that can be appreciated across all scenarios is the result-
ing acceleration curve: it approximately resembles an optimal planning algorithm
based on a trapezoidal acceleration profile. This showcases that a close to optimal
use of the available capacities (kinematic constraints in velocity and acceleration).
In the case of polyhedral constraints, the same conclusion can be drawn regarding
the saturation of the available capabilities. However, given the irregular shape of
the polyhedron, the curves no longer seem to align with a trapezoidal acceleration
profile. Yet, as can be appreciated in the 3D plots, the resulting horizon uses the
maximum available capacities by aligning with the faces of the polyhedron.

The simulation 1, shown in Figure 5.5, constitutes the simplest scenario. The initial
position is at the origin and the initial velocity is null. This result highlights two
main results:

* The path taken constitutes shortest path towards the terminal position. The terminal position, as explained above, does not coincide with the target one but rather the closest possible, due to the imposed constraints.

* The resemblance to a trapezoidal acceleration profile is the most clear from all scenarios.

In simulation 2 (shown in Figure 5.5), the objective is to show a non trivial ini-
tial state. While the position still starts at the origin, the initial velocity is set
to $\dot{p} = [2 \ 1.3 \ 1]$. The resulting position trajectory highlights the effects of these
conditions: due to the limited kinematics capacities, the optimal trajectory tends
approximates a curved path towards the ending position. In a hypothetical infinite
capacities scenario, the path would be swiftly corrected towards a straight line.

The relevance of this last result becomes even more prominent in an online re-
planning scenario like the collaborative tasks proposed in this work. In such case,
the MPC would has to continuously compute new trajectories based on evolving ini-
tial conditions, closing the feedback loop. Furthermore, this also demonstrates that
in a scenario where the task (cost function) is switched or time-varying, the obtained
trajectories remain coherent with the available kinematic capacities, showcasing a
smooth transition that takes into account the starting point.
Finally, Figure 5.7 shows simulation 3. The objective is to showcase the combined situations of the previous scenarios with a non trivial velocity convex set. For instance, this is analogous to employing the feasible velocity sets devised from the joint capacities. The result shows the same conclusions as before can be extrapolating, keeping in mind that the velocity capacities are now heavily restricted by a polyhedron: the resulting position shows a curvy path that smoothly transitions towards the ending position; while the velocity path saturates the available capacity by “sticking” to the faces of the polyhedron.
<figure><img src="image_45.png" /><figcaption>Figure 5.4: The translational convex reachable space approximation polyhedron. This il-
lustrates the attainable space for two different configurations. It presents $\mathcal{P}_{p|Q}$ obtained
from (5.21) with the multiplication of the Jacobian $J_l$ by a scaling factor of 0.15. This
scaling is essential for visualization since the approximation becomes coarse in extreme
cases, particularly those farthest from the joint boundaries. The first configuration, dis-
played in Figure 5.4(a), represents a commonly used initial pose. This configuration is
relatively neutral and displays a symmetrical attainable region. In contrast, Figure 5.4(b)
illustrates an extreme scenario where the robot intentionally approaches the joint bound-
aries. In such instances, the approximation of the attainable region improves signifi-
cantly in the projections aligned with the joint boundaries. As seen in the image, the
robot mobility in this direction becomes severely restricted.</figcaption></figure>
<figure><img src="image_46.png" /><figcaption>Figure 5.5: Example horizon with zonotope constraints and zero initial conditions: The simulation begins at the origin with zero velocity, illustrating the system response to constraints. The trajectory demonstrates the shortest path towards the nearest feasible point within the constraints, since the target position is unreachable due to the imposed limitations. Notably, the trajectory closely resembles an optimal planning algorithm utilizing a trapezoidal acceleration profile, showcasing efficient utilization of available kinematic resources.</figcaption></figure>
<figure><img src="image_47.png" /><figcaption>Figure 5.6: Example horizon with zonotope constraints and non-zero initial velocity: The simulation highlights the impact of the initial velocity on the trajectory. Due to limited kinematic capacities, the optimal trajectory takes on a curved path towards the target. This observation is particularly relevant in online re-planning scenarios, where the system must continuously compute new trajectories based on evolving initial conditions, demonstrating the capability for smooth transitions that consider the starting point in a loop.</figcaption></figure>
<figure><img src="image_48.png" /><figcaption>Figure 5.7: Example horizon with polyhedral constraints, non-zero initial velocity: This third simulation implements non-trivial polyhedral velocity constraints. This combines elements from the previous scenarios while mimicking the constraints derived from joint capabilities. The resulting trajectory exhibits a curved path that gradually transitions toward the target position. It is worth noting that the trajectory saturates the available velocity capacity by closely following the edges of the polyhedron, demonstrating how the system optimally utilizes constrained kinematic resources to approach the goal.</figcaption></figure>
### 5.3.6 Acceleration-based Pose Re-planning ↑

Full pose motion re-planning (i.e. position and orientation control) requires some more work than pure positional control. The mathematical foundations that allow to perform optimizations for constrained motion generation with an MPC were first introduced in Sections 3.3.3 and 3.3.4. This section quickly revisits these definitions for pose control based on the spatial acceleration as introduced in Chapter 4. The re-introduction is necessary to construct the integration of actuation aware constraints that, combined to the computational advantages of the approach, aid in achieving re-planning at the control-rate.

A procedure similar to the previous section can be used to achieve full pose motion control. A first step consists in revisiting the expressions analogous to (5.38) and (5.39):

$$ \mathcal{X}_{k+1} = \mathcal{X}_k e^{\mathrm{T}\nu_k + \frac{T^2}{2}\dot{\nu}_k} \qquad (5.58) $$

revisited ↑

$$ \mathbf{v}_{k+1} = \mathbf{v}_k + \mathrm{T}\dot{\mathbf{v}}_k \qquad (5.59) $$

revisited ↑

Consider the first-order integration of a pose in SE(3) subject to an infinitesimal body twist increment for a single time step T shown in (5.58). This propagation shows the pose trajectories as the successive group actions (exponentials of the twist) on the Lie manifold. The objective is to include the pose and the twist into the state x of the system, and use the spatial acceleration $\dot{\mathbf{v}}$ as a decision variable or control input u. This way, (5.58) and (5.59) can be expressed in the form of (5.28) to be used for linear MPC with (4.39).

By using the lift function introduced in (4.47), one can map $\mathcal{X}_k$ to a vector $\xi_k$. The lift function maps elements from the SE(3) manifold $\mathcal{M}$ to its tangent space at $\mathcal{X}_0$: $\psi: \mathcal{M} \to \mathcal{T}_{\mathcal{X}_0}\mathcal{M}$. The function $\psi^{-1}$ is referred to as the retract function.

With the MPC being formulated in $sc(3)$, the lift operator $\psi$ is used to map the current state and the desired pose into $sc(3)$ to perform the optimization. Meanwhile the retract operator $\psi^{-1}$ is used to map the computed trajectory back to SE(3). This enables directly embedding the *lifted* pose $\xi = \psi(\mathcal{X})$ into the discretized state of the system x, along with the body twist $\nu$. Then, choosing the spatial acceleration $\dot{\mathbf{v}}$ as the input:

$$ \mathbf{x}_k = \begin{bmatrix} \xi_k \\ \nu_k \end{bmatrix} \qquad \mathbf{u}_k = \dot{\mathbf{v}}_k \qquad (5.60) $$

revisited ↑

where the reader is reminded that the k sub-index symbolizes the discretized step within the horizon. In other words, with this notation, $\mathbf{x}_k$ corresponds to the state at the step k in the horizon starting at time $t_n$, or equivalently, denoted as $\mathbf{x}_{k|n}$. This expression has a first-order approximation in its Lie algebra (see Equations 68-74 in [Solà et al., 2018]):

$$ \xi_{k+1} = \log(e^{\xi_k} e^{\mathrm{T}\nu_k + \frac{T^2}{2}\dot{\nu}_k}) \approx \xi_k + \mathrm{J}_R^{\log}(\xi_k)(\mathrm{T}\nu_k + \frac{T^2}{2}\dot{\nu}_k) \qquad (5.61) $$

revisited ↑

where $\mathrm{J}_R^{\log}(\xi_k)$ is the right-trivialized Jacobian of the logarithmic map, evaluated at $\xi_k$. A closed-form expression can be found in [Barfoot et al., 2014]; [Solà et al., 2018] and is implemented in the Pinocchio library [Carpentier et al., 2019]. It relates the additive increments in $\mathcal{T}_0\mathcal{M}$ (the tangent map at the origin) to the rightmultiplied increments in SE(3).
Equation (4.52) offers the missing piece to have a concrete definition for (3.27) when considering the acceleration $\dot{v}_k$ as the control input:

$$A_k = \begin{bmatrix} I_6 & T J_R^{\log}(\xi_k) \\ 0_6 & I_6 \end{bmatrix} \qquad B_k = \begin{bmatrix} \frac{T^2}{2} J_R^{\log}(\xi_k) \\ TI_6 \end{bmatrix} \qquad (5.62) \qquad \text{revisited} \uparrow$$

finally offering a concrete definition for the propagation of the state of the system as in (5.29).

Assuming the target state $x_t$ contains the lifted target pose and a null (zero) body twist:

$$x_t = \begin{bmatrix} \xi_t \\ 0 \end{bmatrix} \qquad (5.63) \qquad \text{revisited} \uparrow$$

### 5.3.7 Pose Re-planning Constraints ↑

Just as in Section 5.3.4, the objective of this section is to formulate the set of constraints that model workspace-related restrictions along with actuation capabilities.

Most of the constraints introduced for position control can be straightforwardly extended for the pose, except for the safety zone. The same principles used in Section 5.2.4 for the formulation of the convex reachable space for the pose in (5.26) can be employed for this case. The main caveat is that, because the MPC problem was formulated in the body frame, the reachable space also must be expressed in the same frame (i.e. a moving frame aligned with the body). In other words, instead of defining the allowed “workspace”, it must be formulated as the allowed “space displacement”.

The formulation can be constructed by analogy from a position displacement zonotope constraint:

$$p_k + \Delta p \le p_M \rightarrow \Delta p \le \Delta p_M \qquad \Delta p_M = p_M - p_k \qquad (5.64)$$

that can also be formulated as a polyhedron:

$$C_i \Delta p \le C_i \Delta p_M \qquad (5.65)$$

where $C_i$ denotes the first row of some linear inequality constraint matrix $C$. For the zonotope case, it is implied that $C_i = [1 \ 1 \ 1]^T$. In that case, the variable $\Delta p$ and $C_i$ are both in the same frame (implicitly, the local world-aligned frame). Assuming the constrained variable $\Delta p$ is now in the body frame (i.e. rotated), the inequality needs to be brought into the same frame, by applying the current orientation $R_k$:

$$R_k^T C_i \Delta p \le R_k^T C_i \Delta p_M \qquad (5.66)$$

The same procedure can be followed for the case of pose constraints with:

$$\mathbf{Ad}_{\chi_k}^{-1} C_i C_i \le \mathbf{Ad}_{\chi_k}^{-1} C_i \mathrm{dlog}_{\xi_k} \xi \le \mathbf{Ad}_{\chi_k}^{-1} C_i C_u \qquad (5.67)$$

where $C_i, C_u$, that respectively denote the original lower and upper bounds, must be continuously updated to account for the movement of the end-effector, just like with $\Delta p_M$. The matrix $\mathbf{Ad}_{\chi_k}^{-1}$ corresponds to the adjoint map that allows changing
the frame of quantities in the tangent space.

And now, the safety-related constraints denoted with an $\mathcal{S}$ can be formulated:

$$ \mathcal{P}_{\xi|\mathcal{S}} = \{\xi \in \mathfrak{sc}(3) \mid \forall \mathbf{v} \in [\xi_{m|\mathcal{S}}, \xi_{M|\mathcal{S}}]\} \qquad (5.68) $$

$$ \mathcal{P}_{v|\mathcal{S}} = \{v \in \mathfrak{sc}(3) \mid \forall \dot{\mathbf{p}} \in [v_{m|\mathcal{S}}, v_{M|\mathcal{S}}]\} \qquad (5.69) $$

where now the bounds are expressed in the body frame. So, for instance, $v_{M|\mathcal{S}}$ denotes the maximum body twist.

The case of the manufacturer-provided task-space limits is even more direct and are denoted with an $\mathcal{M}$ as:

$$ \mathcal{P}_{v|\mathcal{M}} = \{v \in \mathfrak{sc}(3) \mid \forall v \in [v_m, v_M]\} \qquad (5.70) $$

$$ \mathcal{P}_{\dot{v}|\mathcal{M}} = \{\dot{v} \in \mathfrak{sc}(3) \mid \forall \dot{v} \in [\dot{v}_m, \dot{v}_M]\} \qquad (5.71) $$

$$ \mathcal{P}_{\ddot{v}|\mathcal{M}} = \{\ddot{v} \in \mathfrak{sc}(3) \mid \forall \ddot{v} \in [\ddot{v}_m, \ddot{v}_M]\} \qquad (5.72) $$

again with the same clarifications about the frame. Then, the set of *workspace* constraints can be formulated (denoted with $\mathcal{W}$):

$$ \mathcal{P}_{\xi|\mathcal{W}} = \mathcal{P}_{\xi|\mathcal{S}} \cap \mathcal{P}_{\xi|\mathcal{M}} \cap \mathcal{P}_{\xi|\mathcal{A}} \qquad (5.73) $$

$$ \mathcal{P}_{v|\mathcal{W}} = \mathcal{P}_{v|\mathcal{S}} \cap \mathcal{P}_{v|\mathcal{M}} \cap \mathcal{P}_{v|\mathcal{A}} \qquad (5.74) $$

$$ \mathcal{P}_{\dot{v}|\mathcal{W}} = \mathcal{P}_{\dot{v}|\mathcal{S}} \cap \mathcal{P}_{\dot{v}|\mathcal{M}} \cap \mathcal{P}_{\dot{v}|\mathcal{A}} \qquad (5.75) $$

$$ \mathcal{P}_{\ddot{v}|\mathcal{W}} = \mathcal{P}_{\ddot{v}|\mathcal{S}} \cap \mathcal{P}_{\ddot{v}|\mathcal{M}} \cap \mathcal{P}_{\ddot{v}|\mathcal{A}} \qquad (5.76) $$

where the sub-index $\mathcal{A}$ denotes arbitrary and application-dependent sets.

Finally, the focus of this section is on how to add actuation awareness onto the task-space re-planning, while also considering the workspace constraints. To complete the pose control MPC problem that contemplates all these restrictions, this definition is necessary:

$$ \mathcal{X}_k = \mathrm{FK}(\mathbf{q}_k) \qquad (5.77) $$

$$ \mathbf{v}_k = J_l \mathbf{q}_k \qquad (5.78) $$

$$ \xi \in \mathcal{P}_p \qquad \mathcal{P}_{\xi} = \mathcal{P}_{\xi|Q}(\mathbf{q}_k) \cap \mathcal{P}_{\xi|W} \qquad (5.79) $$

$$ \mathbf{v} \in \mathcal{P}_v \qquad \mathcal{P}_v = \mathcal{P}_{v|\dot{Q}}(\mathbf{q}_k) \cap \mathcal{P}_{v|W} \qquad (5.80) $$

$$ \dot{\mathbf{v}} \in \mathcal{P}_{\dot{v}} \qquad \mathcal{P}_{\dot{v}} = \mathcal{P}_{\dot{v}|\dot{Q}}(\mathbf{q}_k, \dot{\mathbf{q}}_k) \cap \mathcal{P}_{\dot{v}|W} \qquad (5.81) $$

$$ \ddot{v} \in \mathcal{P}_{\ddot{v}} \qquad \mathcal{P}_{\ddot{v}} = \mathcal{P}_{\ddot{v}|\ddot{Q}}(\mathbf{q}_k, \dot{\mathbf{q}}_k, \ddot{\mathbf{q}}_k) \cap \mathcal{P}_{\ddot{v}|W} \qquad (5.82) $$

where $\mathcal{X}_k, \mathbf{v}_k$ respectively designate the initial pose and twist. These equations expresses the convex sets that need to be to be formulated as linear constraints as in (4.40) and (4.41).
<table><caption>Table 5.1: Overview of the experimental validation, showing the experimental objectives, scenarios and setups used in each case.</caption><thead><tr><th rowspan="2">Objective<br>(Section 5.4.4)</th><th rowspan="2">Setup<br>(Section 5.4.2)</th><th colspan="3">Scenarios</th></tr><tr><th>Motionless Planning<br>(Scenario 1)</th><th>Back &amp; Forth<br>(Scenario 2)</th><th>Interactive<br>(Scenario 3)</th></tr></thead><tbody><tr><td rowspan="2">Joint Bounds<br>Modulation<br>(Section 5.4.6)</td><td>Simulation</td><td rowspan="2">Figure 5.11</td><td rowspan="2">Figure 5.12</td><td rowspan="2"></td></tr><tr><td>Real Robot</td></tr><tr><td rowspan="4">Joint Velocity<br>Modulation<br>(Section 5.4.7)</td><td>Simulation</td><td colspan="3">Without Re-orientation<br>Figure 5.13</td></tr><tr><td>Simulation</td><td colspan="3">With Re-orientation<br>Figure 5.14</td></tr><tr><td>Real Robot</td><td colspan="3">Without Re-orientation<br>Figure 5.15</td></tr><tr><td>Real Robot</td><td colspan="3">Figure 5.16</td></tr></tbody></table>

## 5.4 Experimental Validation ↑

Section 5.3.5 offered an intuitive way to comprehend the effects of using an *Model Predictive Control (MPC)* for position planning while subject to polyhedral constraints. The objective was to provide an analysis in simple scenarios without actual re-planning. This section proposes a series of experimental settings that showcase the full reactive capabilities of the controller proposed in this manuscript. Also, unlike the previous experiments that focused on position planning (for the interpretability), this section provides a full pose control demonstration.

The main purpose is to provide an experimental validation of the claimed features. In this context, the focus of these experiments is to highlight the actuation-awareness functionality described in Section 5.1.2 and subsequently formulated in Sections 5.2 and 5.3. In particular, the scenarios provide the bases to analyze the effects of re-planning while considering: the joint velocities; and the joint limits.

Refer to Table 5.1 for an overview of the experimental validations performed.

All in all, the main conclusions of these experiences are:

* Trajectory planning from a purely task-space centric point of view while considering joint level actuation limits is achieved.

* It does so while performing a reactive /collaborative task (i.e. online).

* The trajectory also takes into account extra workspace constraints.

* It retains the features highlighted in previous chapters.
<figure><img src="image_49.png" /><figcaption>Figure 5.8: The implemented control architecture is based on the one proposed in Section 4.1.2. It is composed of a cascade loop: 1) a task-space Model Predictive Controller for online re-planning (described in Section 4.3); 2) a constrained Task Space Inverse Dynamics solver to follow the planned trajectory (described in Section 4.3). The feedback is closed with the current state of the robot.</figcaption></figure>

revisited ↑ ↓

### 5.4.1 Control Architecture Implementation ↑

The control architecture resembles the experimental setup in Chapter 4. However, the control architecture is extended to include the feasibility sets. The differences are highlighted in Figure 5.8. The main changes lie in the workspace analysis features, which now considers both the workspace constraints with the joint actuation limits. The geometric workspace available is not included though, as they are analyzed broadly in the position control MPC whose example simulations are shown in Section 5.3.5. Roughly speaking, this architecture is composed of an MPC block

<table><caption>Table 5.2: MPC parameters used in this implementation. In the case of $w_T$, the terminal weight is homogeneized to account for the dimensional difference in the state (i.e. positions and velocities) as defined in (5.60).</caption><thead><tr><th>Name</th><th>Symbol</th><th>Value</th></tr></thead><tbody><tr><td>Horizon Steps</td><td>h</td><td>5</td></tr><tr><td>Step Duration</td><td>T</td><td>100ms</td></tr><tr><td>Horizon Duration</td><td>hT</td><td>0.5s</td></tr><tr><td>Terminal Weight</td><td>w<sub>T</sub></td><td>1, T</td></tr><tr><td>Terminal Weight Matrix</td><td>P<sub>T</sub></td><td><b>I</b><sub>6×6</sub> <b>0</b><sub>6×6</sub> <b>0</b><sub>6×6</sub> <b>I</b><sub>6×6</sub> T</td></tr><tr><td>Regularization Weight</td><td>γ</td><td>10<sup>-2</sup>T/2</td></tr></tbody></table>

for online pose re-planning while a second inverse dynamic solver executes the resulting trajectories. This loop runs at a 1kHz in sync with the actual robot control
rate.

The parameters employed for the MPC configuration are shown in Table 5.2. The block is configured for a 0.5s horizon duration to achieve under 1ms computation times while leaving some time for the inverse dynamics block resolution, respecting the robot control rate.

The actuation aware constraints allow the MPC to plan a task-space trajectory that actually accounts for the joint capacities and current state of the robot. During the experimental scenarios, these constraints are manipulated to highlight their effects on the resulting behavior. The Cartesian-space Constraints (CSC) and Joint-space Constraints (JSC) provided by the manufacturer and employed in this experiment are shown in Tables 5.3 and 5.4, respectively.

<table><caption>Table 5.3: Franka Emika Cartesian trajectory requirements, as specified on their site: https://frankaemika.github.io/docs/control_parameters.html.</caption><thead><tr><th colspan="2">Translation</th><th colspan="2">Rotation</th></tr></thead><tbody><tr><td>v<sub>M</sub></td><td>1.7 m/s</td><td>ω</td><td>2.5 rad/s</td></tr><tr><td>v̇<sub>M</sub></td><td>13 rad/s<sup>2</sup></td><td>ω̇</td><td>25 rad/s<sup>2</sup></td></tr><tr><td>v̇̇<sub>M</sub></td><td>6500 rad/s<sup>3</sup></td><td>ω̈</td><td>12500 rad/s<sup>3</sup></td></tr></tbody></table>

<table><caption>Table 5.4: Franka Emika joint trajectory requirements, as specified on their site: https://frankaemika.github.io/docs/control_parameters.html.</caption><thead><tr><th></th><th>Joint 1</th><th>Joint 2</th><th>Joint 3</th><th>Joint 4</th><th>Joint 5</th><th>Joint 6</th><th>Joint 7</th><th>Unit</th></tr></thead><tbody><tr><th>q<sub>M</sub></th><td>2.8973</td><td>1.7628</td><td>2.8973</td><td>-0.0698</td><td>2.8973</td><td>3.7525</td><td>2.8973</td><td>rad</td></tr><tr><th>q<sub>m</sub></th><td>-2.8973</td><td>-1.7628</td><td>-2.8973</td><td>-3.0718</td><td>-2.8973</td><td>-0.0175</td><td>-2.8973</td><td>rad</td></tr><tr><th>q̇<sub>M</sub></th><td>2.1750</td><td>2.1750</td><td>2.1750</td><td>2.1750</td><td>2.6100</td><td>2.6100</td><td>2.6100</td><td>rad/s</td></tr><tr><th>q̇̇<sub>M</sub></th><td>15</td><td>7.5</td><td>10</td><td>12.5</td><td>15</td><td>20</td><td>20</td><td>rad/s<sup>2</sup></td></tr><tr><th>q̈<sub>M</sub></th><td>7500</td><td>3750</td><td>5000</td><td>6250</td><td>7500</td><td>10000</td><td>10000</td><td>rad/s<sup>3</sup></td></tr><tr><th>τ<sub>M</sub></th><td>87</td><td>87</td><td>87</td><td>87</td><td>12</td><td>12</td><td>12</td><td>Nm</td></tr><tr><th>ṫ<sub>M</sub></th><td>1000</td><td>1000</td><td>1000</td><td>1000</td><td>1000</td><td>1000</td><td>1000</td><td>Nm/s</td></tr></tbody></table>

## 5.4.2 Experimental Setups & Software Infrastructure ↑

This section delves into some details about the deployment aspects of the experiments. It addresses the experimental hardware and software were the experiments were executed and offers some specifics about the software design and choices that support it.

The experiences were conducted on a 7-Degree of Freedom (DoF) serial robot manipulator Franka Emika Panda controlled through its joint torques with Franka's open-source libraries<fnref n="4" />.

For the experiments proposed in this chapter, Two experimental setups were used:

* **Simulation:** The simulation setup is based on the Gazebo [Koenig et al., 2004] physics engine (interfaced through Franka's libraries).

<footnote n="4">⁴https://frankaemika.github.io/docs/franka ros.html</footnote>
* **Real Robot:** The hardware experimental setup is the same one used in Figure 4.4. When used, the plastic pick in the scene is detected in real time via an infra-red Optitrack camera array.

Both setups are based on the ROS [Quigley et al., 2009] framework for the general communication and visualization. In order to compute the robot modeling aspects, the Pinocchio library [Carpentier et al., 2019] is employed.

On an implementation note, the formulated MPC grows in complexity quite fast as the horizon steps are increased (in order to cover a longer duration) because the number of optimization variables is increased too. Being able to consider longer horizons while remaining computationally efficient is crucial to achieve high frequency re-planning (i.e. at the control rate).

This chapter introduced a series of polyhedral inequalities that further scale the computation requirements as they need to be applied to every step of the horizon. This puts a heavy responsibility on finding an appropriate Quadratic Program (QP) solver that can find solutions at the control rate. One feature that helps with this problem is the possibility to limit resolution times. This implies that the QP solver can at least provide an approximate solution at the control rate. Given a high enough control rate, this solution is progressively refined and limits the undesired effects of non-optimal solutions in the overall behavior, without breaking the real-time requirements of the controller. Multiple options were explored including:

* ProxQP [Bambade et al., 2022]: A relatively recent library optimized with performance in mind. In these tests, the computation time was not up to the level of other alternative. However, more importantly, at the time of writing, it does not feature a computation time limit, necessary for the real time requirements of the controller.

* OSQP [Stellato et al., 2020]: A sparse solver implementation. Due to the sparsity of the problem, it should offer some optimization advantages. In practice though, the convergence seemed more erratic, although it does feature a computation time limitation.

* **qpMAD<fnref n="5" />**: This library implements the Goldfarb-Idnani dual active set algorithm for QP [Goldfarb et al., 1983] with some added optimizations to improve computation times. It often yielded the fastest resolution times. However, in some rare cases, it seemed to have a hard time dealing with too many linear inequalities (a consequence of polyhedral constraints in a horizon), leading to computation times over the control rate period. It also does not feature a computation time limit and so it was discarded in favor of the last one.

* **qpOASES** [Ferreau et al., 2014]: This library offered the most robust behavior as well as the required capability to limit computation times. It performed consistently and either equally or on pair with qpMAD.

In the end, qpOASES was retained for the MPC and qpMAD for the inverse dynamics solver.

Though it remains an experimental implementation (not intended for production

<footnote n="5">⁵https://github.com/asherikov/qpmad</footnote>
setups), more details may be found in the open-source implementation library<fnref n="6" />. A
workspace is also readily available for experimentation purposes<fnref n="7" />.

5.4.3 **Experimental Scenarios** ↑

The experiments validate the task-space planning while taking into account the joint
constraints. Concretely, this entails obtaining full pose Cartesian trajectories while
retaining awareness of the joint level constraints. An overview of the experiences is
presented in Table 5.1.

The objective is to validate both constraints independently. Because the architecture
is tailored towards online re-planning, two types of reactive scenarios are proposed:

1. **Motionless Planning**: In this scenario, the robot is static. The only purpose is to show the effects of the joint bound constraints in the planned horizon.

2. **Back and forth scenario**: In this scenario, the robot is instructed to alternate between two predefined poses. However, these are provided as target poses on the fly, so the robot is not able to anticipate a change in target. Yet, this shows the transition capabilities during instant target changes of the architecture while providing a rather repeatable scenario that can be analyzed in more details in the resulting curves.

3. **Interactive scenario**: This scenario entails tracking a pose in real-time. The robot is instructed to follow a pose in the workspace sensed in real-time, this highlights the reactive capabilities of the architecture. The objective is to showcase the properties highlighted individually in other scenarios are maintained while in more random situations, representative to what is expected of a collaborative scenario.

For the case of the real hardware setup, the scenario 2 is shown in Figure 5.9 for the
real setup. In the example shown, the back and forth poses entail only a translational
displacement (i.e. they maintain the same orientation). The same situation is shown
during the simulation experiments, though in one example a change in orientation
is also added.

The interactive scenario 3 is shown in Figure 5.10. In this case, a plastic piece is presented in the robot workspace. Its pose is tracked using the Optitrack camera array and robot is instructed to follow it at a distance projected orthogonally over the Z axis. Although this exact scenario is not shown in simulation, a similar one is in fact devised to intuitively showcase the joint bound awareness while tracking an interactive pose in the simulation.

**5.4.4 Experimental Design & Objectives ↑**

The previous section detailed the scenarios employed. This section delves into the
hypothesis to be tested in those scenarios.

As stated before, the objective is to showcase the effect of task-pace planning while
accounting for joint level constraints. To maintain a certain level of simplicity and

<footnote n="6">⁶https://gitlab.inria.fr/auctus-team/people/nicolas-torres/lmpcpoly</footnote>

<footnote n="7">⁷https://gitlab.inria.fr/auctus-team/people/nicolas-torres/lmpcpoly-ws</footnote>
<figure><img src="image_50.png" /></figure>

video/link

<figure><img src="image_51.png" /><figcaption>Figure 5.9: An example of the back and forth scenario (as defined in Section 5.4.3) used for the experiments, showing the starting and ending poses on the real setup. This scenario is also used in simulations but the video shows one of the experiments being executed on the robot.</figcaption></figure>

<figure><img src="image_52.png" /></figure>

video/link

<figure><img src="image_53.png" /><figcaption>Figure 5.10: Showcases the interactive scenario (as defined in Section 5.4.3) used for the experiments, showing the robot tracking the pose of a plastic piece in real time. The video shows the experiment.</figcaption></figure>
intuitiveness in the experiences and results, only the joint bounds and velocities are taken into account for these objectives, leaving the JSC for the acceleration and jerk joint activated but not shown in the results. Concretely, this entails obtaining full pose Cartesian trajectories while considering joint level constraints $Q, \dot{Q}$ introduced in (5.3).

In order to test these constraints independently, two types of experiments were de-
vised:

1. **Joint bounds modulation experiments:** This type of experiments looks to highlight the effects of considering the convex reachable space approximation $\dot{P}_{\xi|Q}$ introduced in Section 5.2.4. This means that the robot should not plan to go towards unreachable spaces.

2. **Joint velocity modulation experiments:** In this case, the experiment looks to showcase the effects of $P_{v|Q}$ introduced in Section 5.2.3.

Displaying Type 1 on the actual robot is somewhat intricate as it necessitates positioning it in a configuration close to its limits. This results in rather unpredictable behaviors, a result of the robot own internal mechanism to avoid damage interacting with the controller. To avoid this situation, the joint bounds were artificially reduced to show case the robot acting as if its joint range was smaller.

Conversely, Type 2 presents its own set of challenges to showcase. For instance, the expected result is that the planned trajectories should respect the allowed joint velocities. However, the robot redundant motion capabilities means that it can perform a trajectory while also re-configuring. In other words, the robot may perform a joint trajectory that follows the desired Cartesian motion while also generating a change in its configuration that does not result in an end-effector motion. This is mathematically understood as a movement in the kernel of the Jacobian. To bypass this problem, only the joint velocity motion that do not act on this kernel need to be accounted for (as those are responsible for the actual end-effector motion). This is explained in more details while addressing the results.

5.4.5 About the Measured Result Quantities ↑

This section summarizes the quantities employed in the result curves, offering their
mathematical formulation, in the cases they are included:

* **Position:** Refers to the actual position of the end-effector in the workspace $\boldsymbol{p} = [x \ y \ z]^T$, expressed in the world frame. It is shown decomposed for its three components. Meanwhile, the target pose may also be shown with a dotted curve and with an added sub-index t in the legends: $\boldsymbol{p}_t = [x_t \ y_t \ z_t]^T$.

* **Position Error:** Refers to the error between the end-effector position and the target position (the translation component of the target pose $\mathcal{X}_t$ shown in Figure 5.8). Not to be confused with the “desired error” which would correspond to the position error between the one *planned* by the MPC (the position component of $\mathcal{X}_{\text{mpc}}$) and the current position of the end-effector (the current pose is denoted $\mathcal{X}$). This is wrongfully called *desired* because it represents the servoed trajectory that actually accounts for the planning. The way to compute this
error is:

$$e = p_t - p \qquad \chi_t = (R_t, p_t) \qquad \chi = (R, p) \qquad (5.83)$$

* **Orientation Error:** Similar to the position error shown above, this corresponds to the orientation error between the end-effector and the target orientation (the orientation component of the $\chi_t$ shown in Figure 5.8). Not to be confused with the “desired error” which would correspond to the orientation error between one planned by the MPC (the orientation component of $\chi_{mpc}$) and the current orientation of the end-effector. This is wrongfully called desired because it represents the servo-ed trajectory that actually accounts for the planning.

$$\phi = \log(R_t^{-1}R) \qquad \chi_t = (R_t, p_t) \qquad \chi = (R, p) \qquad (5.84)$$

* **Joint Configuration:** This shows the raw values of the joint positional configurations for each of the joints contained in the configuration vector $q$.

* **Normalized Joint Bound Distance:** This shows the joint configuration as normalized against the joint bound (the maximum) specified by the manufacturer (see Table 5.4). It indicates how far or close the configuration is to the respective joint bound (for a value of 1, the configuration is near its limit). Note that given the asymmetrical (as in $q_m \neq -q_M$) joint bounds of joints 4 and 6 start at zero, the definition for each joint $i$ as follows still yields a comparable quantity:

$$\hat{q}_i = |q_i|/q'_{M,i} \qquad q'_{M,i} = \max(|q_{m,i}|, |q_{M,i}|) \qquad (5.85)$$

* **Joint Velocities:** This shows the raw values for the joint velocities as measured by the robot. They are computed with a vector $\dot{q}$.

* **Normalized Joint Velocities:** This shows the joint velocities as normalized against the maximum joint velocities specified by the manufacturer (see Table 5.4). For each joint $i$, they are computed as:

$$\hat{q}_i = |\dot{q}_i|/\dot{q}_{M,i} \qquad (5.86)$$

* **Normalized Non-kernel Joint Velocities:** Similar to the previous normalized definition, this shows the joint velocities as normalized against the maximum joint velocities specified by the manufacturer (see Table 5.4). However, in this case, only the components not corresponding to the movements in the kernel of the Jacobian are accounted for. The joint velocities component projected in the kernel are denoted as $\dot{q}_{ker}$ and the normalized joint velocities, once they are removed, are denoted with $\dot{q}'$. For each joint $i$, they are computed as:

$$\dot{q}_{ker} = (I_n - J^+J)\dot{q} \qquad J^+ = J^T(JJ^T)^{-1} \qquad (5.87)$$

$$\hat{q}'_i = |(\dot{q}_i - \dot{q}_{ker,i})|/\dot{q}_{M,i} \qquad (5.88)$$

where $J^+$ corresponds to the right pseudo inverse (or right Moore-Penrose inverse) of the Jacobian.
<figure><img src="image_54.png" /><figcaption>Figure 5.11: This image showcases the results of a joint-bounds aware task-space planning addressed in Section 5.4.6. The experiment corresponds to a simulation of a motionless planning scenario (as defined in Section 5.4.3) in which the robot is driven to a configuration near its joint bounds so that it cannot move towards the target cube. When the joint bounds limits are disabled (in (a), (b) and (c)), the robot plans a horizon path through the geodesic towards the target. Conversely, once the joint bounds limits are employed in (d) and (e), the horizon becomes very small and tries to find a path that minimizes the distance to the target without violating the joint bounds constraints.</figcaption></figure>

### 5.4.6 Results: Joint Bounds Aware Planning ↑

A first approach to this type of experiment is performed in a simulation scenario. The robot is driven to a configuration near its joint bounds, a situation unsafe for execution in the real robot. The expectation is that the planned trajectory should not try to drive the robot towards unreachable poses.

Such setting is shown in Figure 5.11. The robot is instructed to follow the pose of a gray cube floating in space. The images show the robot in a configuration near its limits. The horizon trajectory is also showed. However, because horizon tries to drive the robot in an impossible direction, the robot is stuck at a local minima. The horizon corresponds to the geodesic path towards the target pose, as shown in Figures 5.11(a) to 5.11(c). Once the convex reachable space constraints are activated, the horizon no longer tries to take this path. In fact, it stays near the end-effector, as it corresponds to the closest possible configuration achievable from this configuration, as shown in Figures 5.11(d) and 5.11(e). A consequence of using a linearized approximation of the reachable space is that these constraints is that the MPC, while still able to consider them, is also prone to local minima (as the target poses are actually reachable by the robot in alternative configurations).
<table><caption>Table 5.5: Artificially limiting the joint bounds to 60% of the range for experimental validation yields the limits shown here. The full range are shown in Table 5.4.</caption><thead><tr><th></th><th>Joint 1</th><th>Joint 2</th><th>Joint 3</th><th>Joint 4</th><th>Joint 5</th><th>Joint 6</th><th>Joint 7</th><th>Unit</th></tr></thead><tbody><tr><th>q'<sub>M</sub></th><td>1.7384</td><td>1.0577</td><td>1.7384</td><td>-0.0698</td><td>1.7384</td><td>2.2515</td><td>1.7384</td><td>rad</td></tr><tr><th>q'<sub>m</sub></th><td>-1.7384</td><td>-1.0577</td><td>-1.7384</td><td>-1.8431</td><td>-1.7384</td><td>-0.0175</td><td>-1.7384</td><td>rad</td></tr></tbody></table>

## Experimental Validation on the Robot

To showcase the effects in a dynamic setting, a “back and forth” experiment (scenario 2) was executed on the robot. In order to operate the robot in undesirable conditions (i.e. near its joint bounds), the joint bounds constraints were set to an artificially constrained value (smaller than the real values). Instead of using $\hat{P}_{\xi|Q}$ as indicated in Figure 5.8, a new feasible reachable space was constructed with:

$$ \hat{P}_{\xi|Q}(q_k) = \{\xi \in \mathfrak{sc}(3) \mid \xi(\Delta q), \Delta q \in [q'_m - q_k, q'_M - q_k]\} \qquad (5.89) $$

revisited ↑

$$ Q' = \{q(t) \in \mathbb{R}^n \mid q \in [q'_m, q'_M]\} $$

revisited ↑

where the artificial joint bounds are provided by $q'_m, q'_M$ and computed using a factor $R_q \in [0, 1]$. Each element of these bounds (for each joint) is defined as:

$$ \begin{aligned} q'_{m,1} &= R_q q_{m,1} & q'_{M,1} &= R_q q_{M,1} \\ q'_{m,2} &= R_q q_{m,2} & q'_{M,2} &= R_q q_{M,2} \\ q'_{m,3} &= R_q q_{m,3} & q'_{M,3} &= R_q q_{M,3} \\ q'_{m,4} &= R_q q_{m,4} & q'_{M,4} &= q_{M,4} \qquad (5.90) \\ q'_{m,5} &= R_q q_{m,5} & q'_{M,5} &= R_q q_{M,5} \\ q'_{m,6} &= q_{m,6} & q'_{M,6} &= R_q q_{M,6} \\ q'_{m,7} &= R_q q_{m,7} & q'_{M,7} &= R_q q_{M,7} \end{aligned} $$

This helps to explicitly show the range of the joint angles with respect to their bounds, by using the normalized joint bounds defined in (5.85). The joints with asymmetrical bounds (joints 4 and 6) are the ones that render this special definition required but because both have one of their bounds approximately near zero, the “normalizations” are still comparable between all of them.

For the results of this experimental validation, refer to Figure 5.12. In this experiment, the robot starts at a neutral pose, which corresponds to the configuration at the middle between the bounds shown in (4.13) to ensure $q$ belongs to $Q'$. Then, the robot is instructed to alternate between two poses until the end of the experience, as shown in Figure 5.9, where a link to the corresponding video is also included. The first peak value in the position and orientation error (approximately before 1s) corresponds to the moment the back and forth task begins (switching from the starting neutral pose to the first pose in the cycle).

The experience shows two stages: a first stage where $R_q = 0.6$ (which yields the joint bound limits shown in Table 5.5) with the resulting constrained movement effects; a
second stage were the full range (meaning $R_q = 1$) is enabled to highlight the difference in behavior. The split between both moments is marked with a vertical black dashed line and it can be seen that it also coincides with the increase in the value of $R_q$, shown in the same curves as the normalized joint bounds distance.

The first stage shows the results of the planned trajectory when the bounds are reduced. It can be seen that the robot is not able to reach the target pose and thus the position and orientation errors never really converge to zero. In contrast, in the second stage, when the full joint range is enabled, the robot is able to minimize both errors.

Furthermore, the joint configuration trajectory changes at the moment of the switch in the $R_q$ value. It can be appreciated in the joints 4 and 6, in the joint configuration graph, where they suddenly surpass artificially imposed limits. This also leads to reducing the position and orientation target error towards zero (until the next change in target, from the back and forth scenario).

### 5.4.7 Results: Joint Velocity Modulation Task-space Planning ↑

These experiments entail a combination of back and forth and interactive scenarios. In order to showcase the velocity modulation capabilities of the feasible velocities constraint using $\hat{P}_{\xi|\dot{Q}}$ as indicated in Figure 5.8, a new feasible set with modulated capacities was defined:

$$P_{v|\dot{Q}'}(q) = \{v \in \mathfrak{S}e(3) \mid v = J(q)\dot{q}, \dot{q} \in \dot{Q}'\} \quad (5.91) \quad \text{revisited} \uparrow$$

$$\dot{Q}' = \{\dot{q} \in \mathbb{R}^n \mid \dot{q} \in [R_{\dot{q}}\dot{q}_m, R_{\dot{q}}\dot{q}_M]\} \quad (5.92) \quad \text{revisited} \uparrow$$

$$R_{\dot{q}} \in [0, 1]$$

where $R_{\dot{q}}$ designates a factor between 0 and 1 that reduces the maximum joint velocity allowed.

It is important to note that while a new $\dot{Q}'$ is employed for the MPC re-planning, the original $\dot{Q}$ is still used in the inverse dynamics controller. It would be straightforward to replace one for the other and could certainly be considered an improvement to the overall guarantees of respecting the actual constraints. However, the objective of the experience is to showcase the effects of modulating the task-space trajectory without direct access to the joint constraints. This way, the only thing that can impact the resulting trajectory is in fact the planned horizon. This helps provide a stronger argument for the capabilities of the actuation aware MPC.

Because of the robot redundancy, part of the joint motion is performed without net end-effector movement (i.e. into the Jacobian kernel). This is why two joint velocities are shown in these experiments: the raw joint velocity; a normalized joint velocity corresponding to the components not projecting to the kernel, as defined in (5.87).

To showcase the effects in a dynamic setting while modulating $R_{\dot{q}}$, four experiments were performed:

1. **Simulation without reorientation:** A back and forth between poses without changing the targets orientation is shown in Figure 5.13.
<figure><img src="image_55.png" /><figcaption>Figure 5.12: Real-world Experiment: Showcases a back and forth task scenario between two poses that have the same orientation. The objective is to show the robot behavior while the allowed joint range is artificially reduced through a factor $R_q$ between 0 and 1 (also shown in the plot) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints. The passage from $R_q = 0.6$ to $R_q = 1$ is highlighted with a dashed vertical line.</figcaption></figure>
2. **Simulation with reorientation:** A back and forth between poses with change in the target orientations is shown in Figure 5.14.

3. **Real Robot without reorientation:** A back and forth between poses with change in the target orientations is shown in Figure 5.15 and executed on the real hardware.

4. **Real Robot interactive:** An interactive setting (scenario 3) representative of collaborative task, where the robot is instructed to follow the pose of a plastic piece in real-time is shown in Figure 5.10 (with its corresponding results shown in Figure 5.16).

In all these settings the robot begins at a neutral pose and then is instructed to begin. In all experiments, while the robot is executing the task, the value of $R_{\dot{q}}$ is changed to modulate the available joint velocities. The effects are highlighted in the “non-kernel” normalized joint velocities. The values of $R_{\dot{q}}$ are also plotted in these graphs, allowing for a direct comparison (thanks to the normalized velocities).

It can be seen that in some situations, there are peaks that surpass the allowed joint velocities, even though these correspond to the “non-kernel” components. This is in part a consequence of not enforcing the same joint level constraints in the inverse dynamics controller; and in part is due to the high velocity movements requiring more servoing from the Proportional Derivative (PD) controller, which is defined in (4.9).

The general results help make an argument for the consideration of JSC into the MPC, as the joint velocities roughly follow the same pattern as the modulation set with $R_{\dot{q}}$.

Furthermore, the interactive scenario with the real robot showcases the capability of the approach to adapt to completely unknown situation in an online fashion, a crucial requisite for collaborative tasks.
<figure><img src="image_56.png" /><figcaption>Figure 5.13: Simulation: Showcases a back and forth task scenario between two poses without changing the orientation, while changing the maximum allowed $\dot{q}$ (showed with $R_{\dot{q}}$) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints.</figcaption></figure>
<figure><img src="image_57.png" /><figcaption>Figure 5.14: Simulation: Showcases a back and forth task scenario between two poses that have different orientations, while changing the maximum allowed $\dot{q}$ (showed with $R_{\dot{q}}$) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints.</figcaption></figure>
<figure><img src="image_58.png" /><figcaption>Figure 5.15: Real-world experiment: Showcases a back and forth task scenario between two poses that have the same orientation, while changing the maximum allowed $\dot{q}$ (showed with $R_{\dot{q}}$) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints.</figcaption></figure>
<figure><img src="image_59.png" /><figcaption>Figure 5.16: Real-world experiment: Showcases an interactive task scenario where the robot tracks a pose captured in real-time while changing the maximum allowed $\dot{q}$ (showed with $R_{\dot{q}}$) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints in an online fashion. The corresponding video is shown in Figure 5.10.</figcaption></figure>
5.4.8 Discussion ↑

While the results helped strongly showcase the task-space centric re-planning for joint bounds and velocity modulation, the same cannot be said of the JSC for the acceleration and jerks. For one, the measurements are noisy enough as is and would make it difficult to obtain reliable results. Furthermore, this problem would also translate to a worse estimation of the feasible motion set (for the spatial accelerations and jerk), further degrading the capabilities of the MPC to achieve the desired modulation. And finally, a bad estimation of biases in these polyhedral constraints would also contribute to incompatible constraints in the MPC, provoking feasibility issues.

An important issue when dealing with constraints is finding a way to always pro-
duce trajectories that consistently stay within limits. This issue is exacerbated by two
situations: for one, the trajectory constraints are originated from different sources
(i.e. Cartesian vs joint-level) but they still need to be expressed in the same space to
be considered at the same time; secondly, the nature of collaborative tasks further
complexifies planning because of the limited computation time and not predefined
starting conditions. While the present control architecture helps address these issues
by using horizon optimization, it cannot really provide theoretical guarantees to the
viability of the solution (viability as in not going to lead to undesirable situations,
as defined in [Prete, 2018]). With respect to the viability of JSC, a proposal is made
in Section 5.5.

Another a big problem in all optimization approaches with hard constraints is dealing with incompatible problems / constraints. For instance, this means that the if the problem is unsolvable, the robot will enter an inconsistent state as no control input can be defined. The present approach has two mechanisms that help avoid these kind of situations: first, the weight on the terminal state helps ensure the end of the horizon tends towards a safe state (in this case, motionless); secondly, if ever the MPC cannot solve the optimization, the controller will continue to use the last horizon successfully calculated.

Finally, these results highlight the potential of a modular architecture separating
tasks-space and joint-space aspects: for one it is generic enough for the application
to other robot architectures; secondly, it separates the motion problem formulation
while still allowing some aspects of the robot model to be considered at the task
level. One potential field that could benefit of this approach is humanoid robot con-
trol, where the trajectory planning is often made at the task-space level [Lober et al.,
2020] and considering the actual actuators capabilities is crucial for stable motions
like walking.
## 5.5 Towards Viable Joint Constraints ↑

As it was briefly mentioned in this chapter, an unresolved issue regarding constraints is on the future states compatibility. As referenced in [Rubrecht et al., 2012], a set of constraints may be instantly feasible but incompatible in the short-medium term, as they might lead to inevitable constraint violations. The assumption of bounded jerk imposes non instantaneous changes to the accelerations. Then, the assumptions in (5.3) imply that there is a minimal joint displacement required for the robot to decelerate completely (to reach a stopped state $\dot{q} = 0, \ddot{q} = 0$) before hitting the joint limits described by $Q$ (the jerk is not part of the stopped state because, under the current assumptions of not necessarily continuous jerk, it can be instantly set to zero). The set of states that do not lead to inevitable violations can be referred to as *viable* states [Prete, 2018].

As an example of a state leading to constraint violations, assume the joint states $q_k, \dot{q}_k, \ddot{q}_k, \dddot{q}_k$ of the robot can be measured for a time step. The robot state may be instantly valid (i.e. $q_k \in Q, \dot{q}_k \in \dot{Q}, \ddot{q}_k \in \ddot{Q}, \dddot{q}_k \in \dddot{Q}$) but for some high enough $\dot{q}_k, \ddot{q}_k$, future states of the robot could end up outside the joint bounds $q_n \notin Q$ (for some time step n>k). The same logic can be applied to a future velocity $\dot{q}_n$ (but not for the acceleration $\ddot{q}_n$, because, under the assumptions mentioned in (5.3), the decision variable $\dddot{q}_k$ can be changed instantly, and thus even if $\ddot{q}_k$ is near its limit, a maximum deceleration jerk can start reducing it immediately).

A worst-case scenario<fnref n="8" /> is shown in Figure 5.17 assuming the robot starts braking at the start of the trajectory: when the acceleration is at a maximum value and in the same sense as the current velocity (so it is driving the velocity towards its limit). Because of the smooth stopping requirements imposed, instant changes in acceleration are forbidden and will require reverting the acceleration at maximum jerk (for the acceleration to become opposite and start reducing the velocity) for a duration that depends on the jerk limits and the starting acceleration; during this time, the velocity will continue to increase towards its limit.

This phenomenon highlights an implicit incompatibility problem between constraints because they hide a potentially risky situation that may arrive from edge-case scenarios. The initial velocity shown in Figure 5.17 should not have been allowed from the start to avoid this situation. This implies that a contraction on the maximum velocity constraints caused by the higher order dynamics can help avoid inevitable violation states.

A constraint “compatibilization”<fnref n="9" /> strategy in this sense that coherently compresses the limits can be proposed by using some worst-case scenario analysis. A theoretical proposal is presented without an experimental validation.

The higher order constraints impose contractions on the lower-order constraints. A trapezoidal acceleration profile (like the one shown in Figure 5.18) is employed for the generation of smooth stopping trajectories.

The proposal for this kind of situation is analyzed in Figure 5.19: avoiding instant

<footnote n="8">⁸not the worst case because the initial velocity is not maxed out, in which case the trajectory quickly surpasses the constraints</footnote>

<footnote n="9">⁹As in long term compatibility, so it is rather a viability analysis.</footnote>
<figure><img src="image_60.png" /><figcaption>Figure 5.17: A smooth jerk-bounded (TAP) stopping trajectory example for a worst-case scenario: $v_0 = -0.7v_M$, $a_0 = -a_M$ (acceleration starts at maximum value and not inverted to velocity while velocity starts near its maximum value).</figcaption></figure>

<figure><img src="image_61.png" /><figcaption>Figure 5.18: Normalized reference TAP trajectory generated with [Berscheid et al., 2021b].</figcaption></figure>
changes in acceleration imposes a 2-stage stopping trajectory: an acceleration reversing stage; and a deceleration stage that continuously reduces the velocity up to a point where both acceleration and velocity reach zero, as is proposed in [Joseph et al., 2020].

<figure><img src="image_62.png" /><figcaption>Figure 5.19: Smooth stopping trajectory with acceleration reversing, where the initial velocity and acceleration have the same direction, requiring a max-jerk trajectory to inverse the acceleration.</figcaption></figure>

<figure><img src="image_63.png" /><figcaption>Figure 5.20: When the initial acceleration and velocity have opposed sense, the acceleration reversing stage is not necessary.</figcaption></figure>

In order to find the viable set of constraints (that will avoid inevitable violation states [Rubrecht et al., 2010]), the smooth (as in jerk-bounded) stopping trajectories need to be computed. The way to do this is described in Appendix E. Applying this enables computing the worst-case scenario displacement for $q$ and $\dot{q}$:

$$ \Delta q_{stop} = \Delta q_{R} + \Delta q_{D} \qquad (5.93) $$

$$ \Delta \dot{q}_{stop} = \Delta \dot{q}_{R} + \Delta \dot{q}_{D} \qquad (5.94) $$

where the subdindices $R, D$ refer to the reversing and deceleration stages, respectively. Furthermore, $\Delta q_R = 0, \Delta \dot{q}_R = 0$ for the stopping trajectories that require only a deceleration stage, as identified in Table 5.6 according to the initial velocity and acceleration. Extending the assumptions mentioned in (5.3) yields the viable Joint-space
<figure><img src="image_64.png" /><figcaption>Figure 5.21: When the initial acceleration and velocity have opposed sense, but the initial velocity is low enough that the Deceleration stage does not require a constant acceleration part.</figcaption></figure>

<figure><img src="image_65.png" /><figcaption>Figure 5.22: Polytope constraints from compatibilized joint-space constraints.</figcaption></figure>

revisited ↑

Constraints (JSC)[Prete, 2018][Rubrecht et al., 2012]:

$$ 
\begin{aligned}
Q_V &= \{ \mathbf{q}(t) + \Delta \mathbf{q}_{\text{stop}} \in \mathbb{R}^n \mid \mathbf{q} \in [\mathbf{q}_m, \mathbf{q}_M] \} \\
\dot{Q}_V &= \{ \dot{\mathbf{q}}(t) + \Delta \dot{\mathbf{q}}_{\text{stop}} \in \mathbb{R}^n \mid \dot{\mathbf{q}} \in [\dot{\mathbf{q}}_m, \dot{\mathbf{q}}_M] \}
\end{aligned} 
\quad (5.95) 
$$

Finally, these new sets may be used to extend the feasible sets defined in Section 5.2.3. Having ensured that the joint level constraints are viable, theoretically ensures that the polyhedral feasible sets are also viable. So, these new sets can be defined by replacing $Q_V$, $\dot{Q}_V$ with $Q$, $\dot{Q}$, respectively.
<table><caption>Table 5.6: Worst-case scenario stages required for the stopping trajectory according to the initial velocity $v_0$ and acceleration $a_0$.</caption><thead><tr><th></th><th colspan="2">a<sub>0</sub></th><th>Legends</th></tr><tr><th></th><th>-</th><th>+</th><th></th></tr></thead><tbody><tr><th rowspan="2">v<sub>0</sub></th><th>+</th><th>D R</th><td>D Only deceleration stage</td></tr><tr><th>-</th><th>R D</th><td>R Acceleration reversing and deceleration stage</td></tr></tbody></table>

## 5.6 Conclusion ↑

Being able to quickly design and deploy collaborative cells without being constrained by the robot architecture is a fundamental requirement for agile manufacturing solutions. The present chapter presented a way to extend the task-centric architecture proposal by considering joint level constraints at the task space. This is achieved by reformulating Joint-space Constraints (JSC) into the task space.

Not only does this help with the modularity of the control architecture, but it also helps fully utilize the robot capabilities, which leads to better dimensioning robots for the task required, potentially reducing the sizes and thus increasing the working cell safety and efficiency.

This work demonstrates some of the possibilities offered by an Model Predictive Control (MPC)-based architecture, showcasing the flexibility of the approach. Yet, the technique is quite open to further adaptations that can contemplate more complex scenarios. For instance, the human factor in manufacturing traditionally does not take into account the cognitive or ergonomic aspects of collaborative tasks, and these can be integrated into straightforwardly in the current form in many ways. For instance, constraints would allow expressing the human capabilities and exhaustion while tasks could be employed to optimize for better postures that minimize musculoskeletal lesions.

Furthermore, the MPC approach used here is defined for mathematically designed cost functions (i.e. minimizing a distance). Yet, one could imagine more complex tasks being devised or learned through machine learning approaches that can be readily be integrated with the same horizon optimization.
# Chapter 6

## Conclusion

This manuscript constructs some theoretical notions towards truly collaborative control approaches. Firstly, it contributes to the formulation of reactive motions trajectories. Secondly, it shows how these formulations can be exploited for online optimization-based control, achieving real-time motion adaptation. It proposes a way to integrate these high-level aspects of the tasks (behaviors) with the low level aspects of the robot architecture (like the actuators).

A first step into truly reactive motion adaptation is correctly parameterizing full pose trajectories (i.e. position and orientation). The mathematical foundations that enable formulating pose motions of free rigid body in space on the $SE(3)$ manifold are addressed in Chapter 2. These foundations were employed to formalize an integration strategy based on a linear algebraic expression. This is used to efficiently approximate an integration scheme for fast pose estimations in gliding time horizons. The importance of this strategy for pose estimation is crucial in predictive and optimization-based control.

A second step towards reactive motion adaptation is exploiting this information for decision making. Chapter 3 presents a first approach to an optimization scheme exploiting the pose estimation strategy on the manifold. It addresses the challenge of efficient workspace sharing between collaborative robots and humans by proposing a Linear Model Predictive Control (MPC) adapting to changing workspace constraints and interactive task transitions in the Cartesian space. The controller enables online re-planning of position and orientation and is demonstrated through experiments with the Franka Emika robot.

In Chapter 4, the approach is further extended to obtain smoother behaviors, by working at the dynamics level. It focuses on optimizing the formulation to balance task achievement and computation time. This architecture dissociates planning from execution, utilizing a linear formalization in $SE(3)$ to achieve task re-planning at a high-frequency closed-loop control rate. Experiments with the Franka Emika robot showcase the effectiveness of this approach in scenarios with dynamic task modifications.

Finally, Chapter 5 links back the task-space to the actuation level, introducing an enhancement to the control strategy by incorporating joint space constraints to better describe the robot actual actuation capabilities. This chapter outlines a method for transitioning from task space considerations to include these constraints, resulting in a reactive control strategy. This approach allows for the adjustment of task-space
movements while incorporating “actuation awareness” to ensure the robot operates
within its capacities.

**6.1 Perspectives ↑**

The pertinence of the proposal in this manuscript create opportunities for potential
future improvements in various aspects:

**Mathematical Formulation**

The approach presented in this manuscript exploits some mathematical structures in order to arrive at a convenient propagation scheme for pose motions. This enabled including the orientation in the planning algorithm. However, describing motions in SE(3) and/or intricate constraints in the workspace can often be nonintuitive. At the same time, while the current approach shows it is possible to generate motions in a horizon at the control rate, it did so by resorting to interpolation, which remains a rather coarse approach. These aspects are fundamental for designing more complex tasks, to formulate constraints that better adapt to collaborative scenarios and to improve the responsiveness.

While the approach shows a promising potential, the algebra employed by the algorithms can be generalized to a formulation based on geometric algebra. This approach offers the mechanisms for potentially more interpretable expressions with some efficiency gains [Löw et al., 2023].

On the constraints front, the presented approach paves the way for the study of task-specific polytope-type of constraints on manifolds that can better represent the workspace information/restrictions. One example of this would be the expression of richer orientation constraints. Beyond that, it can be used to better represent the available workspace for the robot to move (free of obstacles and humans) with an arbitrary precision. In particular, this type of polyhedrals on the manifold are known as orbitopes [Sanyal et al., 2011] and remain an open research topic for the robotics community.

**Control Architecture**

Furthermore, another unexplored question in the current manuscript pertains to the use of Model Predictive Control (MPC) at the joint level. From a performance perspective, these kind of approaches based explicitly on the joint states [Krämer et al., 2020] could deliver an improvement provided they can be implemented with approximately the same computational efficiency.

In fact, the use of this kind of approaches could be applied at two levels in the current architecture: for the task-space planning, in which case the validity of the model compared to the current approach remains an unexplored question but an interesting one; and secondly on the low level task-tracking controller.

Indeed, while the task-space planning algorithm is capable of modulating its behaviors according to a gliding time horizon, as it is now, the low level control lacks this capability and could benefit from optimizing the joint trajectory from the whole task-space horizon (even though some level of actuation awareness is already embedded in the generated horizon trajectories, in the form proposed in Chapter 5). Such an
approach presents an opportunity to improve the constraint viability through some
efficient optimization approaches like differential dynamic programming [Tassa et
al., 2014].

Another aspect unexplored in this work that merits exploration is the treatment of
tasks that require manipulation [Gold et al., 2023] and contact forces [Kleff et al.,
2022] from the same perspective of decoupling task and joint spaces. From a per-
formance point of view, it presents an interesting line of questioning. Beyond that,
it would better align to the modularity properties desired for collaborative robots.
Furthermore, from an application standpoint, this would enable evaluating the pre-
sented actuation awareness approach while exploiting other aspects of the collab-
orative task, like the wrench capacities of the robot and the human [Skuric et al.,
2021b]; [Skuric et al., 2021a].

Finally, the current manuscript focuses on collaborative robotics and more specifi-
cally, for the case of serial manipulators. However, many of the principles and results
can be straightforwardly extended to other robot architectures. For instance, some
control approaches for humanoid robots employ optimization methods to generate
task-space reference trajectories [Lober et al., 2020] because doing so in the actua-
tor space is too computationally intensive. This application could benefit from the
actuation-aware task-space planning proposal to generate a-priori feasible trajecto-
ries.

Hybrid Learning Algorithms

On a more long term perspective, MPC algorithms in general offer a deterministic approach to modulating systems behaviors. However, the same mathematical principles that render them deterministic also limit their flexibility in terms of evolving behaviors: once deployed, these algorithms behave consistently and do not offer improving or fine tuning to the situation. This offers an opportunity for hybrid learning approaches that can leverage the model-based knowledge with adaptive algorithms [Bechtle et al., 2021] or equip the inherently non-deterministic control of learning approaches with some safety awareness [Fisac et al., 2019].

Furthermore, the tasks enabled by optimization-based controllers are limited by the
design of the cost functions. In other words, the behaviors that are achievable by
these approaches are limited to what can be described mathematically. Sometimes,
finding a formulation that performs consistently proves to be a complex endeavor.
This ultimately hinders the complexity of the tasks that can be obtained and opens
doors for the exploration of learning algorithms that can better adjust their behaviors
with more finely tuned tasks [Bogdanovic et al., 2019]; [Calinon, 2020].

Human Factor Considerations

Finally, a fundamental aspect of collaborative robotics is the human-centric perspec-
tive. This means that the human factor must be accounted for within the control
schemes. Integrating these kind of aspects remains an open scientific question.
However, the proposed control architecture offers the infrastructure for the inclu-
sion of generic and human-aware constraints that can potentially express complex
information about the human state such as the fatigue level [Savin et al., 2016].

Other human aspects that offer opportunities for richer human-robot collaboration