# Predictive Control of Collaborative Robots in Dynamic Contexts

Nicolas Torres Alberto

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

## To cite this version:

Nicolas Torres Alberto. Predictive Control of Collaborative Robots in Dynamic Contexts. Robotics [cs.RO]. Universite Bordeaux, 2023. English. NNT: tel-04410378

HAL Id: tel-04410378

https://inria.hal.science/tel-04410378

Submitted on 22 Jan 2024

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

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

Distributed under a Creative Commons Attribution 4.0 International License
THÈSE PRÉSENTÉE
POUR OBTENIR LE GRADE DE

DOCTEUR
DE L'UNIVERSITÉ DE BORDEAUX

ECOLE DOCTORALE SCIENCES PHYSIQUES
ET DE L'INGÉNIEUR

AUTOMATIQUE, PRODUCTIQUE, SIGNAL ET IMAGE, INGÉNIERIE COGNITIQUE

Par Nicolas TORRES ALBERTO

# Predictive Control of Collaborative Robots
in Dynamic Contexts

Commande Prédictive de Robots Collaboratifs dans des Contextes Dynamiques

Sous la direction de : **David DANNEY**

Co-directeur : **Vincent PADOIS**

Soutenue le 23 Octobre 2023

## Membres du jury:

M. Nicolas MANSARD

M. Richard BEAREE

M. Ouiddad LABBANI

M. Sebastien RUBRECHT

M. David DANNEY

M. Vincent PADOIS

Directeur de Recherche

Professeur

Professeure des Universités

Responsable d'Equipe de Recherche

Directeur de Recherche

Directeur de Recherche

LAAS-CNRS (Toulouse)

Arts et Métiers, LISPEN (Lille)

Xlim (Limoges)

IRT Jules Verne (Nantes)

Inria Université de Bordeaux

Inria Université de Bordeaux

Rapporteur

Rapporteur

Examinatrice

Examineur

Directeur

Co-directeur
# Contents

<table><tr><td>List of Figures</td><td>v</td></tr><tr><td>List of Tables</td><td>xii</td></tr><tr><td>Acronyms</td><td>xiii</td></tr><tr><td>1 Introduction</td><td>1</td></tr><tr><td>1.1 Industrial Context: Stellantis</td><td>3</td></tr><tr><td>1.2 Agile Robotics: Human-centered Agile Manufacturing</td><td>4</td></tr><tr><td>1.3 Towards True Human-Robot Collaboration</td><td>6</td></tr><tr><td>1.3.1 Shared Workspace Requirements</td><td>8</td></tr><tr><td>1.3.2 Understanding the Requirements</td><td>9</td></tr><tr><td>1.3.3 Drafting a Proposal</td><td>10</td></tr><tr><td>1.4 Structure of this manuscript</td><td>11</td></tr><tr><td>1.5 Navigation and Notation Aspects in this Manuscript</td><td>12</td></tr><tr><td>2 Linear Pose Estimation in a Time Horizon</td><td>14</td></tr><tr><td>2.1 Introduction</td><td>16</td></tr><tr><td>2.2 Rigid Body Motion in Space</td><td>17</td></tr><tr><td>2.2.1 Orientation of a floating body in space</td><td>18</td></tr><tr><td>2.2.1.1 Some properties of rotation matrices</td><td>20</td></tr><tr><td>2.2.1.2 Axis-angle representation</td><td>21</td></tr><tr><td>2.2.1.3 Angular velocity</td><td>22</td></tr><tr><td>2.2.1.4 Axis-angle representation as the rotation Matrix exponential parameterization</td><td>24</td></tr><tr><td>2.2.2 Pose of a floating body in space</td><td>25</td></tr><tr><td>2.2.2.1 Pose as a frame displacement</td><td>26</td></tr><tr><td>2.2.2.2 Pose derivative</td><td>26</td></tr><tr><td>2.2.2.3 Screw motion as the pose matrix exponential parameterization</td><td>27</td></tr><tr><td>2.3 Rigid Body Pose Trajectory in Space</td><td>28</td></tr><tr><td>2.3.1 Pose path in space</td><td>28</td></tr><tr><td>2.3.2 Discretizing a pose path</td><td>30</td></tr><tr><td>2.3.3 Single step integration as a differential equation</td><td>31</td></tr><tr><td>2.3.4 Pose trajectory as a product of exponentials</td><td>31</td></tr><tr><td>2.4 Integration on a Lie Manifold</td><td>32</td></tr><tr><td>2.4.1 Relation between a Lie group and its algebra</td><td>33</td></tr><tr><td>2.4.2 A discrete integration scheme on the tangent space of the manifold</td><td>34</td></tr><tr><td>2.4.3 Solving a Lie group differential equation via the Magnus expansion</td><td>35</td></tr><tr><td>2.4.3.1 Truncating the Magnus expansion to approximate the logarithm map derivative</td><td>37</td></tr><tr><td>2.4.3.2 An approximative pose propagation in the logarithm map</td><td>38</td></tr><tr><td>2.5 Linear Pose Estimation in a Horizon</td><td>38</td></tr><tr><td>2.5.1 Problem statement: pose propagation in linear form</td><td>39</td></tr><tr><td>2.5.2 Discrete pose estimation in a horizon</td><td>39</td></tr></table>
<table>
  <tbody>
    <tr>
      <td>2.5.3 Simulation Experiment</td>
      <td>40</td>
    </tr>
    <tr>
      <td>2.5.4 Simulation results</td>
      <td>40</td>
    </tr>
    <tr>
      <td><b>2.6 Conclusions</b></td>
      <td><b>43</b></td>
    </tr>
    <tr>
      <td><b>3 Linear Model Predictive Control on SE(3): Velocity-based</b></td>
      <td><b>44</b></td>
    </tr>
    <tr>
      <td><b>3.1 Introduction</b></td>
      <td><b>46</b></td>
    </tr>
    <tr>
      <td>3.1.1 Rationale: Constrained Online Motion Generation</td>
      <td>46</td>
    </tr>
    <tr>
      <td>3.1.2 Control Architecture Proposal</td>
      <td>48</td>
    </tr>
    <tr>
      <td><b>3.2 Task-space Tracking</b></td>
      <td><b>50</b></td>
    </tr>
    <tr>
      <td>3.2.1 Kinematic Robot Model</td>
      <td>52</td>
    </tr>
    <tr>
      <td>3.2.2 Pose Tracking as a Quadratic Program</td>
      <td>54</td>
    </tr>
    <tr>
      <td>3.2.3 Constraints Formulation</td>
      <td>55</td>
    </tr>
    <tr>
      <td>3.2.4 On the Solver Reference Frame</td>
      <td>56</td>
    </tr>
    <tr>
      <td><b>3.3 Constrained Motion Generation with Model Predictive Control</b></td>
      <td><b>58</b></td>
    </tr>
    <tr>
      <td>3.3.1 Problem Statement: Motion Generation via MPC</td>
      <td>59</td>
    </tr>
    <tr>
      <td>3.3.2 Linear Model Predictive Control</td>
      <td>60</td>
    </tr>
    <tr>
      <td>3.3.3 Optimization on the Pose Manifold</td>
      <td>62</td>
    </tr>
    <tr>
      <td>3.3.4 Geodesic Path in the Tangent Space</td>
      <td>64</td>
    </tr>
    <tr>
      <td>3.3.5 Task-Space Linear MPC in the Tangent Space</td>
      <td>64</td>
    </tr>
    <tr>
      <td><b>3.4 Experimental Validation: The Case of Velocity Modulation</b></td>
      <td><b>66</b></td>
    </tr>
    <tr>
      <td>3.4.1 Implementing Online Re-planning</td>
      <td>67</td>
    </tr>
    <tr>
      <td>3.4.2 Numerical Simulation</td>
      <td>68</td>
    </tr>
    <tr>
      <td>3.4.3 Experimental Scenario &amp; Safety Constraints</td>
      <td>69</td>
    </tr>
    <tr>
      <td>3.4.4 Results</td>
      <td>72</td>
    </tr>
    <tr>
      <td><b>3.5 Conclusion</b></td>
      <td><b>72</b></td>
    </tr>
    <tr>
      <td><b>4 Linear Model Predictive Control on SE(3): Acceleration-based</b></td>
      <td><b>75</b></td>
    </tr>
    <tr>
      <td><b>4.1 Introduction</b></td>
      <td><b>77</b></td>
    </tr>
    <tr>
      <td>4.1.1 Robots in Changing Environments for Dynamic Tasks</td>
      <td>78</td>
    </tr>
    <tr>
      <td>4.1.2 Control Architecture</td>
      <td>79</td>
    </tr>
    <tr>
      <td><b>4.2 Acceleration-based Task-space Tracking</b></td>
      <td><b>81</b></td>
    </tr>
    <tr>
      <td>4.2.1 Dynamic Robot Model</td>
      <td>81</td>
    </tr>
    <tr>
      <td>4.2.2 Inverse Dynamics Controller as a Quadratic Program</td>
      <td>82</td>
    </tr>
    <tr>
      <td>4.2.3 Inverse Dynamics Constraints</td>
      <td>83</td>
    </tr>
    <tr>
      <td><b>4.3 MPC-based motion re-planning over a receding horizon</b></td>
      <td><b>84</b></td>
    </tr>
    <tr>
      <td>4.3.1 Reduced Formulation</td>
      <td>87</td>
    </tr>
    <tr>
      <td>4.3.2 Dynamics-based MPC in the Tangent Space</td>
      <td>88</td>
    </tr>
    <tr>
      <td>4.3.3 Interpolating the Horizon Trajectory</td>
      <td>91</td>
    </tr>
    <tr>
      <td><b>4.4 Experimental Validation: Online Re-planning at the Control Rate</b></td>
      <td><b>92</b></td>
    </tr>
    <tr>
      <td>4.4.1 Experimental Procedure &amp; Setup</td>
      <td>92</td>
    </tr>
    <tr>
      <td>4.4.2 Analysis</td>
      <td>95</td>
    </tr>
    <tr>
      <td><b>4.5 Discussion</b></td>
      <td><b>99</b></td>
    </tr>
    <tr>
      <td><b>4.6 Conclusion</b></td>
      <td><b>100</b></td>
    </tr>
    <tr>
      <td><b>5 Actuation-aware Task-space Model Predictive Control</b></td>
      <td><b>101</b></td>
    </tr>
    <tr>
      <td><b>5.1 Introduction</b></td>
      <td><b>103</b></td>
    </tr>
    <tr>
      <td>5.1.1 Rationale: Feasible Online Motion Generation</td>
      <td>103</td>
    </tr>
    <tr>
      <td>5.1.2 Actuation-aware Control Framework</td>
      <td>104</td>
    </tr>
    <tr>
      <td><b>5.2 From Joint-space to Task-space Constraints</b></td>
      <td><b>105</b></td>
    </tr>
    <tr>
      <td>5.2.1 Task-space Feasible Sets as Convex Polyhedrons</td>
      <td>106</td>
    </tr>
  </tbody>
</table>
<table>
  <tbody>
    <tr>
      <td>5.2.2 Robot Actuation Capacities</td>
      <td>107</td>
    </tr>
    <tr>
      <td>5.2.3 From Joint-space Constraints to Task-space Feasible Motions</td>
      <td>108</td>
    </tr>
    <tr>
      <td>5.2.4 From Joint Bounds to Convex Reachable Workspace</td>
      <td>111</td>
    </tr>
    <tr>
      <td><b>5.3 Actuation-aware Task-space Re-planning</b></td>
      <td><b>113</b></td>
    </tr>
    <tr>
      <td>5.3.1 Some MPC notions and notations</td>
      <td>113</td>
    </tr>
    <tr>
      <td>5.3.2 About the Linear Inequality Constraints</td>
      <td>114</td>
    </tr>
    <tr>
      <td>5.3.3 Acceleration-based Position Re-planning</td>
      <td>115</td>
    </tr>
    <tr>
      <td>5.3.4 Position Re-planning Constraints</td>
      <td>116</td>
    </tr>
    <tr>
      <td>5.3.5 Position Re-planning Constraints Simulations</td>
      <td>117</td>
    </tr>
    <tr>
      <td>5.3.6 Acceleration-based Pose Re-planning</td>
      <td>124</td>
    </tr>
    <tr>
      <td>5.3.7 Pose Re-planning Constraints</td>
      <td>125</td>
    </tr>
    <tr>
      <td><b>5.4 Experimental Validation</b></td>
      <td><b>127</b></td>
    </tr>
    <tr>
      <td>5.4.1 Control Architecture Implementation</td>
      <td>128</td>
    </tr>
    <tr>
      <td>5.4.2 Experimental Setups & Software Infrastructure</td>
      <td>129</td>
    </tr>
    <tr>
      <td>5.4.3 Experimental Scenarios</td>
      <td>131</td>
    </tr>
    <tr>
      <td>5.4.4 Experimental Design & Objectives</td>
      <td>131</td>
    </tr>
    <tr>
      <td>5.4.5 About the Measured Result Quantities</td>
      <td>133</td>
    </tr>
    <tr>
      <td>5.4.6 Results: Joint Bounds Aware Planning</td>
      <td>135</td>
    </tr>
    <tr>
      <td>5.4.7 Results: Joint Velocity Modulation Task-space Planning</td>
      <td>137</td>
    </tr>
    <tr>
      <td>5.4.8 Discussion</td>
      <td>144</td>
    </tr>
    <tr>
      <td><b>5.5 Towards Viable Joint Constraints</b></td>
      <td><b>145</b></td>
    </tr>
    <tr>
      <td><b>5.6 Conclusion</b></td>
      <td><b>149</b></td>
    </tr>
    <tr>
      <td><b>6 Conclusion</b></td>
      <td><b>150</b></td>
    </tr>
    <tr>
      <td>6.1 Perspectives</td>
      <td>151</td>
    </tr>
    <tr>
      <td>6.2 Closing Remarks</td>
      <td>153</td>
    </tr>
    <tr>
      <td><b>A Quadratic Programming</b></td>
      <td><b>154</b></td>
    </tr>
    <tr>
      <td>A.1 Weighted Least Square</td>
      <td>155</td>
    </tr>
    <tr>
      <td><b>B Discretization of Continuous LTI systems</b></td>
      <td><b>156</b></td>
    </tr>
    <tr>
      <td>B.1 Discrete-time State-space</td>
      <td>157</td>
    </tr>
    <tr>
      <td><b>C Summary: Velocity-based Pose Tracking as a Quadratic Program</b></td>
      <td><b>158</b></td>
    </tr>
    <tr>
      <td><b>D Summary: Torque-based Pose Tracking as a Quadratic Program</b></td>
      <td><b>160</b></td>
    </tr>
    <tr>
      <td><b>E Smooth stopping trajectory</b></td>
      <td><b>164</b></td>
    </tr>
    <tr>
      <td>E.1 Acceleration Reversing Stage</td>
      <td>164</td>
    </tr>
    <tr>
      <td>E.2 Deceleration Stage</td>
      <td>165</td>
    </tr>
    <tr>
      <td><b>Bibliography</b></td>
      <td><b>167</b></td>
    </tr>
  </tbody>
</table>
List of Figures

<table>
  <tbody>
    <tr>
      <td>1.1</td>
      <td>Example images of heavyly automated tasks with industrial robots. Obtained from [Stellantis Communication, 2023]; [Julien Cresp Groupe PSA, Direction de la communication, 2017a].</td>
      <td>4</td>
    </tr>
    <tr>
      <td>1.2</td>
      <td>Stellantis brands, after the merger in 2021.</td>
      <td>5</td>
    </tr>
    <tr>
      <td>1.3</td>
      <td>Example of a robot deployed inside a cage. Obtained from [Groupe PSA Communication Metz Tremery, 2018]; [Stellantis Communication, 2022a].</td>
      <td>6</td>
    </tr>
    <tr>
      <td>1.4</td>
      <td>Collaborative robots are often used in non-collaborative tasks. [Julien Cresp Groupe PSA, Direction de la communication, 2017b].</td>
      <td>7</td>
    </tr>
    <tr>
      <td>1.5</td>
      <td>Many tasks benefit from human precision and have the potential to evolve towards collaborative work cells. Obtained from [Stellantis Communication, 2022b]; [Julien Cresp/Within, 2023].</td>
      <td>8</td>
    </tr>
    <tr>
      <td>2.1</td>
      <td>A simple way to represent 3D positions and orientations in space is through vectors.</td>
      <td>18</td>
    </tr>
    <tr>
      <td>2.2</td>
      <td>Depiction of two conventional frames used to describe rigid body motions in space: S is the world or universal frame and L is a local or body frame and corresponds to a moving frame placed on the concerned body.</td>
      <td>19</td>
    </tr>
    <tr>
      <td>2.3</td>
      <td>Rotations can be represented in an axis-angle form comprised of a rotation angle around a rotation axis (vector). Figure 2.3(a) shows the movement of a point attached to a rigid body moving at an angular velocity <i>rotvel</i>. Figure 2.3(b) depicts the exponential coordinates <i>&phi;</i> of a rotation.</td>
      <td>22</td>
    </tr>
    <tr>
      <td>2.4</td>
      <td>Representation of a pose in 3D space.</td>
      <td>26</td>
    </tr>
    <tr>
      <td>2.5</td>
      <td>Representation of a position path in 3D space.</td>
      <td>29</td>
    </tr>
    <tr>
      <td>2.6</td>
      <td>Representation of a pose path in 3D space.</td>
      <td>29</td>
    </tr>
    <tr>
      <td>2.7</td>
      <td>Depiction<sup>1</sup> of an integration scheme in the tangent space.</td>
      <td>33</td>
    </tr>
    <tr>
      <td>2.8</td>
      <td>Depiction of the path taken taking the end-effector of a Franka Emika robot from an initial to a target pose <i>&chi;</i><sub>i</sub>, <i>&chi;</i><sub>t</sub> &isin; S<sub>E</sub>(3).</td>
      <td>40</td>
    </tr>
    <tr>
      <td>2.9</td>
      <td>Normalized <i>s</i>(<i>t</i>) for a time-optimal trajectory generated with a trapezoidal profile (in velocity).</td>
      <td>41</td>
    </tr>
    <tr>
      <td>2.10</td>
      <td>Variable speed integration using ME2 for a 300ms horizon.</td>
      <td>42</td>
    </tr>
    <tr>
      <td>2.11</td>
      <td>Average error for different horizon lengths.</td>
      <td>42</td>
    </tr>
  </tbody>
</table>
<table>
  <tbody>
    <tr>
      <td>4.6</td>
      <td>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.</td>
      <td>98</td>
    </tr>
    <tr>
      <td>4.7</td>
      <td>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.</td>
      <td>100</td>
    </tr>
    <tr>
      <td>5.1</td>
      <td>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. 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.</td>
      <td>104</td>
    </tr>
    <tr>
      <td>5.2</td>
      <td>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.</td>
      <td>106</td>
    </tr>
    <tr>
      <td>5.3</td>
      <td>The translational velocity polyhedron. It visualizes the feasible velocity set $P_{\dot{p}|\dot{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 showcases 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.</td>
      <td>109</td>
    </tr>
  </tbody>
</table>

revisited ↑ ↓

revisited ↑ ↓
<table>
  <tbody>
    <tr>
      <td>5.4</td>
      <td>The translational convex reachable space approximation polyhedron. This illustrates the attainable space for two different configurations. It presents P<sub>p|Q</sub> obtained from (5.21) with the multiplication of the Jacobian J<sub>l</sub> 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, displayed 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 boundaries. In such instances, the approximation of the attainable region improves significantly in the projections aligned with the joint boundaries. As seen in the image, the robot mobility in this direction becomes severely restricted.</td>
      <td>120</td>
    </tr>
    <tr>
      <td>5.5</td>
      <td>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.</td>
      <td>121</td>
    </tr>
    <tr>
      <td>5.6</td>
      <td>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.</td>
      <td>122</td>
    </tr>
    <tr>
      <td>5.7</td>
      <td>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.</td>
      <td>123</td>
    </tr>
    <tr>
      <td>5.8</td>
      <td>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.</td>
      <td>128</td>
    </tr>
    <tr>
      <td>5.9</td>
      <td>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.</td>
      <td>132</td>
    </tr>
  </tbody>
</table>

revisited ↑ ↓
<table>
  <tbody>
    <tr>
      <td>5.10</td>
      <td>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.</td>
      <td>132</td>
    </tr>
    <tr>
      <td>5.11</td>
      <td>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.</td>
      <td>135</td>
    </tr>
    <tr>
      <td>5.12</td>
      <td>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<sub>q</sub> 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<sub>q</sub> = 0.6 to R<sub>q</sub> = 1 is highlighted with a dashed vertical line.</td>
      <td>138</td>
    </tr>
    <tr>
      <td>5.13</td>
      <td>Simulation: Showcases a back and forth task scenario between two poses without changing the orientation, while changing the maximum allowed q&#x0307; (showed with R<sub>q&#x0307;</sub>) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints.</td>
      <td>140</td>
    </tr>
    <tr>
      <td>5.14</td>
      <td>Simulation: Showcases a back and forth task scenario between two poses that have different orientations, while changing the maximum allowed q&#x0307; (showed with R<sub>q&#x0307;</sub>) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints.</td>
      <td>141</td>
    </tr>
    <tr>
      <td>5.15</td>
      <td>Real-world experiment: Showcases a back and forth task scenario between two poses that have the same orientation, while changing the maximum allowed q&#x0307; (showed with R<sub>q&#x0307;</sub>) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints.</td>
      <td>142</td>
    </tr>
    <tr>
      <td>5.16</td>
      <td>Real-world experiment: Showcases an interactive task scenario where the robot tracks a pose captured in real-time while changing the maximum allowed q&#x0307; (showed with R<sub>q&#x0307;</sub>) 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.</td>
      <td>143</td>
    </tr>
    <tr>
      <td>5.17</td>
      <td>A smooth jerk-bounded (TAP) stopping trajectory example for a worst-case scenario: v<sub>0</sub> = -0.7v<sub>M</sub>, a<sub>0</sub> = -a<sub>M</sub> (acceleration starts at maximum value and not reversed to velocity while velocity starts near its maximum value).</td>
      <td>146</td>
    </tr>
    <tr>
      <td>5.18</td>
      <td>Normalized reference TAP trajectory generated with [Berscheid et al., 2021b].</td>
      <td>146</td>
    </tr>
    <tr>
      <td>5.19</td>
      <td>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.</td>
      <td>147</td>
    </tr>
  </tbody>
</table>
List of Tables

<table><thead><tr><th></th><th></th><th></th></tr></thead><tbody><tr><td>2.1</td><td>Shows the average compute time and error for the horizons shown in Figure 2.10.</td><td>To showcase the trade off between precision and computation time, this comparison shows the error between the ground truth reference (computed with (2.82)) and the proposed ME-based method (using (2.81)) for each discretized step (as well as the maximum and standard deviations). The results are showed for 2 generated trajectories with their respective references. Compute time refers to the average for each Δt step on a C++ implementation. Relative computation time is calculated with respect to each reference average computation time. The tests were executed on a laptop with a CPU Intel(R) Core(TM) i7-10610U.</td><td>42</td></tr><tr><td>4.1</td><td>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.</td><td></td><td>95</td></tr><tr><td>4.2</td><td>Cartesian constraints for v, v̇, v̇̇ for the Model Predictive Control (MPC) during the experiment. The MPC used for motion generation is shown in Figure 4.2 and described in Section 4.2.</td><td></td><td>96</td></tr><tr><td>5.1</td><td>Overview of the experimental validation, showing the experimental objectives, scenarios and setups used in each case.</td><td></td><td>127</td></tr><tr><td>5.2</td><td>MPC parameters used in this implementation. In the case of w<sub>T</sub>, the terminal weight is homogeneized to account for the dimensional difference in the state (i.e. positions and velocities) as defined in (5.60).</td><td></td><td>128</td></tr><tr><td>5.3</td><td>Franka Emika Cartesian trajectory requirements, as specified on their site: https://frankaemika.github.io/docs/control_parameters.html.</td><td></td><td>129</td></tr><tr><td>5.4</td><td>Franka Emika joint trajectory requirements, as specified on their site: https://frankaemika.github.io/docs/control_parameters.html.</td><td></td><td>129</td></tr><tr><td>5.5</td><td>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.</td><td></td><td>136</td></tr><tr><td>5.6</td><td>Worst-case scenario stages required for the stopping trajectory according to the initial velocity v<sub>0</sub> and acceleration a<sub>0</sub>.</td><td></td><td>149</td></tr></tbody></table>
# Acronyms

**CSC** Cartesian-space Constraints 107, 115, 117, 129

**DoF** Degree of Freedom vi, vii, 46, 52–54, 69, 70, 82, 83, 94, 129

**FK** Forward Kinematics 52–54, 111

**HRI** Human-Robot Interaction 46, 48, 49

**IK** Inverse Kinematics 52, 53, 55

**JSC** Joint-space Constraints 105–108, 129, 133, 139, 144, 147, 149

**LTI** Linear Time-Invariant 60

**MPC** Model Predictive Control vi–viii, x, xii, 10–12, 16, 35, 39, 43, 46, 50, 58–75, 77–81, 84, 86, 88–93, 95–97, 99, 100, 104, 105, 107, 112–118, 124–130, 133–135, 137–144, 149–153, 162, 163

**PD** Proportional Derivative 54, 81, 83, 96, 139, 159, 161, 162

**QP** Quadratic Program 16, 43, 52, 54, 55, 58, 61–63, 68, 70, 71, 74, 77, 81–83, 85, 86, 88, 95, 99, 105, 107, 109, 110, 114, 115, 130, 155, 158, 160, 161, 163

**SS** State Space 60

**TS** Technical Specification 7, 10, 66
# Chapter 1

# Introduction
# Contents

<table><tr><td>1.1</td><td>Industrial Context: Stellantis</td><td>3</td></tr><tr><td>1.2</td><td>Agile Robotics: Human-centered Agile Manufacturing</td><td>4</td></tr><tr><td>1.3</td><td>Towards True Human-Robot Collaboration</td><td>6</td></tr><tr><td>1.3.1</td><td>Shared Workspace Requirements</td><td>8</td></tr><tr><td>1.3.2</td><td>Understanding the Requirements</td><td>9</td></tr><tr><td>1.3.3</td><td>Drafting a Proposal</td><td>10</td></tr><tr><td>1.4</td><td>Structure of this manuscript</td><td>11</td></tr><tr><td>1.5</td><td>Navigation and Notation Aspects in this Manuscript</td><td>12</td></tr></table>
"When one speaks of increasing power, machinery, and industry there comes up a picture of a cold, metallic sort of world in which great factories will drive away the trees, the flowers, the birds, and the green fields. And that then we shall have a world composed of metal machines and human machines. With all of that I do not agree. I think that unless we know more about machines and their use, unless we better understand the mechanical portion of life, we cannot have the time to enjoy the trees, and the birds, and the flowers, and the green fields."

— Henry Ford in collaboration with Samuel Crowther, My Life and Work

Ever since the introduction of the moving assembly production line in the Ford Motor Company, manufacturers have strived to augment production by increasing automation and removing operators from production lines. However, the race for throughput in a world with limited resources has subtly grown into a chase for efficiency: not only does it affect production costs and competitiveness, but it also directly impacts the long term effects on the environment and, indirectly, the overall increase in the quality of life that the manufactured goods promise to provide.

Indeed, most heavy manufacturing (and specially in the context of highly automated car assembly) that are either unsafe for operators to do or simply impossible for humans to perform consistently have been automated throughout the last centuries. Some clear examples of this are shown in heavy car parts welding and assembly, like with the frames shown in Figure 1.1.

However, many assembly tasks often lack flexibility to evolve in time. Either because it is costly to adapt too often or because some tasks are simply too complex to automate. Yet, in the age of connectednes and efficiency, being able to adapt the manufacturing chain according to real time business data feedback becomes crucial.

This context motivates the resurgence of a new industrial trend towards human-centric manufacturing. Humans provide high levels of dexterity and flexibility; they can also quickly adapt to new tasks. Yet, they still need to overcome their intrinsic mechanical limits in terms of repetitive movements and forces. This is where the advancements in technology are required. Indeed, collaborative robotics arrive as the epitome of automation technologies to provide the missing synergistic boost in the manufacturing efficiency: the merge of humans and robots.

## 1.1 Industrial Context: Stellantis ↑

French have a long trajectory with car manufacturing. Since the first self-propelled vehicle built (steam-powered), attributed to the french inventor Nicolas-Joseph Cugnot in 1769 [Wikipedia, 2023], a huge automobile manufacturing industry surged, driven by demand and pushing innovation in the fabrication and assembly domains. The current work was carried out in the context of the R&D activities at one of the most emblematic french automakers groups: Group PSA (more recently Stellantis).

Stellantis is the resulting group of a merger between two international automakers: Fiat Chrysler Automobiles and Peugeot S.A. The deal was completed and the new
<figure><img src="image_3.png" /><figcaption>Figure 1.1: Example images of heavily automated tasks with industrial robots. Obtained from [Stellantis Communication, 2023]; [Julien Cresp Groupe PSA, Direction de la communication, 2017a].</figcaption></figure>

group began trading on the New York Stock Exchange as “STLA” in 2021 to become the 4th largest automaker in the world by volume. Stellantis designs, manufactures, and sells automobiles bearing its 16 brands (see Figure 1.2) and in the first half of 2023 reported a net revenue of €98.4 billion [Stellantis communication, 2023]. In this context, the group has set big ambitions for the coming electric vehicles transition in the years to come and has an interest to continue to invest in research and development to maintain and push its lead on the global market.

A big part of its global strategy is to innovate in manufacturing technologies. This thesis is situated in the context of Stellantis future perspectives for the “Usine du Future” (Factory of the Future), a set of long term objectives for the technological evolution of its factories [Beauville Dit Eynaud, 2020]; [Voilqué, 2020].

## 1.2 Agile Robotics: Human-centered Agile Manufacturing ↑

Vehicles are complex technological systems that require intricate (and often sequential) manufacturing procedures. This has provoked the car manufacturing industry to always be heavily intertwined with industrial trends, being particularly sensitive to innovations in fabrication.
<figure><img src="image_4.png" /><figcaption>Figure 1.2: Stellantis brands, after the merger in 2021.</figcaption></figure>

In the current day and age, new environmental standards for sustainable development and the increasing market demand for consumer goods frame a transition towards new trends in manufacturing. To stay competitive, manufacturers have had to adopt strategies related to the flow of data in factories, which exploits the Internet of Things to increase flexibility and efficiency in the so called industrial revolution 4.0 [Ghobakhloo, 2020]. A main feature of these trends entails the capability to obtain real-time information of the factories states and effectively modulate its production capacity according to carefully crafted plannings while also taking advantage of the demand level trends. All this is possible thanks to the introduction of new technologies that have increased the connectednes and stream of sensor data, augmenting information availability and allowing for more effective decision-making strategies to transition into “agile manufacturing” [Gunasekaran, 1999].

However, once this information starts becoming the norm, a new challenge begins to appear in the horizon: manufacturing adaptation. Indeed, there is no use for information that cannot be readily exploited to take optimal decisions. These decisions entail adapting the production capacity, reassigning resources, switching production lines, customizing products according to orders, etc. Although the factory information might be promptly available, implementing all these strategies is far from simple and demands a level of flexibility that is only beginning to be possible in modern times. Factories need to be reconfigurable and this is in line with Stellantis long term objectives [Beauville Dit Eynaud, 2020].

At the same time, the long term effects on the environments of all these production capacities must also be considered. As the environmental effects induced, in part, due to the impulsive and unhindered use of resources in the last centuries start to become more apparent, industries must reevaluate their priorities. What use are the goods and services that increase the quality of life today only to permanently hinder it in the future? Indeed, factories need to start taking special consideration to the conscious use of energy and materials that enable environmentally sustainable production.

It is in this context that new priorities start to emerge to favor human-centered strategies [Demir et al., 2019]; [Lu et al., 2022]. Humans' efficiency and capacity to learn new tasks and quickly transform production lines while limiting the wasted resources becomes crucial again. They are needed to take back the center stage
<figure><img src="image_5.png" /><figcaption>Figure 1.3: Example of a robot deployed inside a cage. Obtained from [Groupe PSA Communication Metz Tremery, 2018]; [Stellantis Communication, 2022a].</figcaption></figure>

of manufacturing facilities, just like before they were removed in favor of automa-
tion technologies due to lower productivity capabilities. This last drawback has not
changed. However, this time around, new technologies like cobots [James E. Colgate
et al., U.S. Patent 5952796A, Feb. 1996] have arrived to assist humans. These smaller
robots are inherently more efficient due to their size and were specially conceived to
work along humans. Cobots promise not only to compensate for the human short
comings but also to augment their capabilities by working in collaboration while
also adapting to the new age of efficiency, with reduced sizes.

This is the context that sets the tone for the current work: a draft proposal for strate-
gies in which robots can start to augment humans, increase efficiency and flexibility
while working in true collaboration; a proposal for cobots to start to fulfill their orig-
inal promises and transition into “Agile Robotics”.

1.3 Towards True Human-Robot Collaboration ↑

Even though cobots were conceived with shared workspaces in mind from the
start [“Cobot architecture” 2001], their original mission has not completely crys-
tallized yet. Simply using this type of robot does not necessarily make a task
collaborative. The transition towards human-robot collaboration requires a series of
adaptations in the way robots are conceived, deployed and programmed. A major
<figure><img src="image_6.png" /><figcaption>Figure 1.4: Collaborative robots are often used in non-collaborative tasks. [Julien Cresp Groupe PSA, Direction de la communication, 2017b].</figcaption></figure>

concern towards these objectives is safety [Proia et al., 2022]. The very nature of humans and robots sharing a workspace has been inconceivable thus far with big industrial robots. Consequently, to ensure human safety while incorporating robots in accessible areas, the simplest strategy possible has seen the widest industry adoption: putting robots inside restricted zones limited by cages to guarantee physical separation even in accidental scenarios. This kind of situations is shown in Figure 1.3. This results in an inefficient use of the factory space that provokes impractical restrictions to the movement and design assembly lines, instead of a continuous and fluid transition between work stations.

Nevertheless, cobots still remain more practical from an ease of use and flexibility point of view, as they are easier to deploy than heavy industrial robots in general. As such, the sight of collaborative robots being deployed absent of collaborative tasks is common in factories (Figure 1.4). Yet, the fact remains that many high dexterity task still require human manipulation and offer a huge potential for more complex collaborative work cells (Figure 1.5).

While many efforts in the literature have been made to provide methods for systematic assessments of safety and other human-centered factors in collaborative work-cells [Haddadin et al., 2009]; [Robla-Gómez et al., 2017], the main strategies widely adopted in the industry must comply with the established ISO standards [Fryman et al., 2012] and/or Technical Specification (TS) [Rosenstrauch et al., 2017]. While the existence of standardizations might seem to indicate a consensus on the mechanisms necessary for safety, these regulations are often considered problematic [Hanna et al., 2022]; [Fryman, 2014]; [Chemweno et al., 2020], due to the limited flexibility in collaborative scenarios.

Another challenge in the way of true human-robot collaboration is the traditional control approaches. Classically, robots are programmed to perform a task on repetition as fast as possible with little to no variability into each iteration. This entails predefined behaviors or trajectories that are not compatible with a symbiotic human-robot relation [L. Wang et al., 2019]. In order for robots to actually assist humans in the best way possible, they must be able to re-actively adapt to the non-deterministic way in which humans work: they must behave coherently with their surroundings and in tandem with what the human actually needs [Tsarouchi et al., 2016].
<figure><img src="image_7.png" /><figcaption>Figure 1.5: Many tasks benefit from human precision and have the potential to evolve towards collaborative work cells. Obtained from [Stellantis Communication, 2022b]; [julien cresp/Within, 2023].</figcaption></figure>

1.3.1 Shared Workspace Requirements

In order to start providing some answers to the scenario put forth this far, this section proposes a list of criteria or design principles that a robot control architecture needs to implement in order be compliant with the long term objectives of collaborative robotics work cells:

1. **Minimalism** With efficiency in mind, robots must strive to reduce their footprint as much as possible. Not only in the physical sense but rather in all aspects about their operating lifetime. However, a smaller size does in fact help achieve these objectives with multiple consequences branching from the fact that the amount of moving mass is reduced and so smaller robots influence: safety (they pose less danger in case of collisions with humans); energy usage (they consume less); they occupy less physical space and favor transportation for eventual reassignments, etc.

2. **Optimality** Again, stemming from the efficiency objectives, is the fact that robots need to make optimal use of the energy to achieve as much performance a possible. In this case, a high-performance robot is used in a broad sense as one that can achieve the required task with a task-dependent metric (for instance, a pick and place task would require robots with high lift capacity and/or speed).
3. **Reactiveness** In order to actually work in collaborative scenarios, robots are required to act according to what is happening around them. This entails complex behaviors that consider multiple aspects at the same time to continuously adapt in a way that remains safe and useful for the operator/s.

4. **Modularity** To answer the flexibility needs of modern factories, robots are required to be easily reassignable. Meaning they should be easily reconfigured and deployed into new posts without much work. Reparability, reuse and replacement capabilities should be priorities. This entails not only the robot hardware but also the design aspects of the tasks and ease of use, like the programming.

### 1.3.2 Understanding the Requirements ↑

The minimalism and optimality objectives go hand in hand. In fact, if one were able to correctly dimension both the task requirements (i.e. the weight to be transported, the necessary speed, the exerted forces, etc) and, at the same time, were able to match these to the robot capacities, then choosing the smallest robot that can effectively comply with the required tasks should be trivial. However, the non-linear relation between the robot actuators and the task requirements make this quite challenging [Skuric et al., 2021a]. Beyond that, these requirements are usually non formal specifications (as in not completely determined but rather defined as high level objectives to be performed). Nevertheless, the perfect architecture should strive to make an efficient use of the true robot capacities, while being able to easily program the robot to take optimal decisions.

Also, this objective naturally favors safety improvements: by using the robot in an optimal way (i.e. more efficiently), employing lighter robots becomes possible. From an energetic point of view, the fact that robots operate at lower kinetic energy levels contributes to safety [Joseph et al., 2018a].

Subsequently, the re-activeness requirement sets the challenge a bit higher because it implies that the robot must be able to take these optimal decisions while considering real-time aspects of the environment around it. The robot needs to simultaneously assess multiple criteria: dynamic events occurring around it (like operators and obstacles approaching, the state of an interactive task, etc); the true robot actuation capacities (in order to use them efficiently); its own state to avoid self-collisions; and the actual state of the task that it is currently performing.

This breaks apart from the traditional approach of programming robots to perform tasks at a later stage. It implies that robots are now required to be infused with "logical" behaviors that are capable of taking coherent decisions according to the situation. This entails a control architecture that acts in an online fashion.

And finally, the modularity and reconfigurability aspect of the control architecture are the least abstract aspects of these principles. This translates to how much a robot architecture depends on the actual robot architecture (i.e. its shape, actuators, form, capacities, etc).

In fact, to align with the general ease of use and human-centric (thus operator-centric) point of view, decoupling the tasks aspects from the robot control aspects
of the architecture should be favored. This way, if a designed task needs to be de-
ployed with a different robot, only some parts of the architecture need to be replaced.
Conversely, if a robot needs to be reused, only the "task planning" parts need to be
switched.

1.3.3 Drafting a Proposal ↑

Even though these principles drive the decision making process in a general way, the
scope of this work is to apply them to a control architecture. In order to set a frame-
work that answers to these requirements, a draft solution of a control architecture is
proposed.

The previous analysis outlines a solution that goes beyond the possibilities envi-
sioned by the TS for collaborative robotics [ISO/TS-15066, 2016]. While the TS does
allow some dynamic aspects like maximum velocities, safety zones and maximum
energy levels, it remains a rather inflexible solution from an architectural point of
view. For instance, it is not compatible with more intricate approaches to safety
awareness through energy modulation [Raiola et al., 2018]; [Meguenani et al., 2015].
Furthermore, its certification is complex and leads to cobots that are used in non-
collaborative scenarios or behind cages.

These problems stem from the fact that the robot is inherently controlled through
inputs in its actuation space but, in order to reactively consider safety, it needs to
account for external events occurring in the workspace. Decoupling these two as-
pects of the architecture immediately points the approach towards a modeling strat-
egy for the "external events" and for the robot capacities: constraints. Some ap-
proaches contribute towards the safety aspects while considering these capacities
in joint space [Joseph et al., 2020]. However, a method to consider workspace con-
straints in a generic way remains to be proposed.

Beyond that, the decoupling also stems from the inherent nature of these constraints: the workspace and the actuation space. It seems only logical that they should be considered separately. Nevertheless, the robot needs to take optimal decisions with respect to both. In order to do so, the constraints need to be formulated in the same space. This is relevant not only from an optimality point of view, but also from a feasibility concern [Rubrecht et al., 2010]; [Prete, 2018]. While some approaches do consider the real capacities and are capable of formulating workspace constraints [Kleff et al., 2021], they often do not decouple the task from the actuation (which does not align with the modularity objective).

The path taken throughout this thesis to start assembling all these aspects into a sin-
gle architecture is to employ an optimization-based approach. The idea is to trans-
late the decision making process to be performed in real time into an optimization
problem that the robot controller can solve as fast as possible. Motion planning
problems can be efficiently formulated as optimization problems over gliding time
windows. This way the robot does not waste time planning a global motion that will
not be executed (until the end) and instead only needs to consider the near future.

Such an approach employs Model Predictive Control (MPC) for motion generation
in the workspace similar to what is proposed in [Gold et al., 2023]. This goes in line
with the task-centered design point of view.
One final consideration to obtain a generic approach capable of full motion control: the architecture needs to be able to consider the full pose. The special emphasis on these aspects stems from the fact that oftentimes control approaches focus only on the position, as it is the most straightforward aspect of the control and the handling the orientation adds undesirable complexities. However, orientation control must also be taken into account if one strives to provide a generic solution to the problem.

While the robotics community has addressed the full pose motion formulation problem for navigation [Forster et al., 2015], the methodologies are often employed for state estimation in mobile robots [Solà et al., 2018]. However, these principles offer a convenient framework for efficient MPC approaches that properly address full pose control in serial robots, which remains to be proposed.

Finally, the proposal is to employ a decoupled cascade controller:

* A linear *MPC* for constrained motion generation in position and orientation, capable of taking into account both workspace level constraints along with actuation capacities.

* A constrained optimization-based controller at the actuation level.

The cascade scheme addresses the modularity requirements, while the minimalism depends on correctly formulating the actuation constraints and exploiting them, which is achieved through optimization. Indeed, at the heart of the proposal lies an *MPC*: it addresses the reactiveness by effectively allowing reference-less control while, at the same time, generating near optimal trajectories. Overall, this framework answers to the drafted requirements in this chapter in a generic way. This manuscript showcases the proposal for serial collaborative robots.

## 1.4 Structure of this manuscript
↑

The structure of this manuscript is as follows:

*Chapter 2:* Presents the underlying principles and modeling aspects that enable a generic approach to task-space planning with full pose control. It presents the mathematical formalism to represent the pose of a robot in the SE(3) manifold and how, its tangent space, can be employed to perform constrained optimizations. The content of this chapter is broadly covered by a published conference paper [Torres Alberto et al., 2022a].

*Chapter 3:* Presents a first approach to an ideal control architecture for collaborative robotics. It presents the underlying optimization approach based on *Model Predictive Control (MPC)* that applies the principles from *Chapter 2* to perform constrained task-space motion generation at the kinematic level. The pertinence of the proposed control architecture is demonstrated using experiments with the Franka Emika robot in a scenario implying both adaptation of the maximum allowed velocity to comply with human presence and on-the-fly updates of a Cartesian goal pose.

*Chapter 4:* This chapter extends the proposals from *Chapter 3* to provide an acceleration-based formulation of the *MPC* and robot control algorithms. The underlying principles remain the same, but the solution improves in robustness and smoothness thanks to a more efficient formulation. As an experimental validation,
the architecture is implemented to achieve a high-frequency closed-loop controller,
enabling re-planning at the control rate.

**Chapter 5:** Finally, this chapter further extends the previous control architecture with some formalism for the modeling of robot capacities in the task-space. This is used to explicitly consider the joint-space robot capacities from within the task-space re-planning algorithm (MPC) to obtain trajectories that are coherent from the Cartesian-space constraints as well as the actual actuation capabilities. In the end, a high-frequency actuation-aware control architecture is demonstrated on the real robot by modulating the available joint capacities from tasks-space planning, to obtain a motion that stays coherent throughout the whole control loop.

**Chapter 6:** Offers some closing remarks and perspectives for future research.

## Unpublished or On-going Related Work

* Online task-space trajectory planning using real-time estimations of robot motion capabilities [Skuric et al., 2022a]

Antun Skuric, Nicolas Torres Alberto, Lucas Joseph, Vincent Padois, David Daney

* Model Predictive Control for robots adapting their task space motion online [Torres Alberto et al., 2023]

Nicolas Torres Alberto, Antun Skuric, Lucas Joseph, Vincent Padois, David Daney

* Linear Model Predictive Control in SE(3) for online trajectory planning in dynamic workspaces [Torres Alberto et al., 2022b]

Nicolas Torres Alberto, Antun Skuric, Lucas Joseph, Vincent Padois, David Daney

* From a trapezoidal acceleration profile to a learnt time optimal control policy for robot braking [Esquerre-Pourtère et al., 2022]

Arthur Esquerre-Pourtère, Nicolas Torres Alberto, Vincent Padois

## 1.5 Navigation and Notation Aspects in this Manuscript

On a practical note, a small clarification about navigation is required.

To facilitate navigation throughout this manuscript, every section title features an arrow on the right that allows quickly going back to the contents table. This enables going to sections far apart by going through to the table of contents.

The general outline of the manuscript progressively builds up the methodology and concepts with each chapter. However, the chapters are intended to be mostly self-contained. As such, some sections summarize concepts from the previous ones, to refresh the concepts that are addressed.

On the same note, when definitions are revisited at later stages and it is deemed
relevant to bring it to the reader's attention, it appears on the manuscript margins like this:

$$ p \in \mathbb{R}^3 \qquad (1.1) $$

revisited ↓

where the arrow on the right is a reference to the where it is revisited, right below:

$$ p = [p_x \ p_y \ p_z] \qquad (1.2) $$

revisited ↑

This allows the reader to quickly go back and forth between both definitions. This is only used for equations and, less often, for figures. Sometimes the it indicated that the revisited element (equation or figure) is exactly the same (for convenience) and sometimes, it indicates that it is conceptually linked to a previous definition that is extended.

Furthermore, equations are referred with (equation number) (without explicitly saying it corresponds to an equation), to facilitate reading. However, when referring to tables, figures or sections, it is explicitly stated.

Finally, as can be seen in (1.1) and (1.2), matrices and vectors (like $p$) have a bold font, while scalars use a normal font (like $p_x, p_y, p_z$).
Chapter 2

Linear Pose Estimation in a Time
Horizon

This chapter delves into a strategy for pose estimation over small time
horizon, expressed through linear algebra. The significance of this ap-
proach resides in its capacity to streamline predictive and optimization-
based control in robotics, the central theme of this manuscript.

The discussion starts with the representation of rigid body
poses—position and orientation—as a singular Lie group element.
An alternative parameterization of these elements comes into view next,
employing exponential coordinates to highlight their relation to Lie
algebras. Extending this concept, the representation of rigid body move-
ments is introduced. This progression allows for the parameterization of
pose trajectories as successive group actions on the Lie manifold.

With this foundation laid, an integration strategy emerges, capable of
presenting the same trajectory on the tangent space, thereby circumvent-
ing the multiplicative propagation scheme. As a final point, the inte-
gration leverages a linearization approximation rooted in a truncation of
the Magnus expansion. This method approximates the exponential map
derivative and showcases promising results in terms of precision and
computation times.
# Contents

<table><tr><td>2.1</td><td>Introduction</td><td>16</td></tr><tr><td>2.2</td><td>Rigid Body Motion in Space</td><td>17</td></tr><tr><td>2.2.1</td><td>Orientation of a floating body in space</td><td>18</td></tr><tr><td>2.2.1.1</td><td>Some properties of rotation matrices</td><td>20</td></tr><tr><td>2.2.1.2</td><td>Axis-angle representation</td><td>21</td></tr><tr><td>2.2.1.3</td><td>Angular velocity</td><td>22</td></tr><tr><td>2.2.1.4</td><td>Axis-angle representation as the rotation Matrix exponential parameterization</td><td>24</td></tr><tr><td>2.2.2</td><td>Pose of a floating body in space</td><td>25</td></tr><tr><td>2.2.2.1</td><td>Pose as a frame displacement</td><td>26</td></tr><tr><td>2.2.2.2</td><td>Pose derivative</td><td>26</td></tr><tr><td>2.2.2.3</td><td>Screw motion as the pose matrix exponential parameterization</td><td>27</td></tr><tr><td>2.3</td><td>Rigid Body Pose Trajectory in Space</td><td>28</td></tr><tr><td>2.3.1</td><td>Pose path in space</td><td>28</td></tr><tr><td>2.3.2</td><td>Discretizing a pose path</td><td>30</td></tr><tr><td>2.3.3</td><td>Single step integration as a differential equation</td><td>31</td></tr><tr><td>2.3.4</td><td>Pose trajectory as a product of exponentials</td><td>31</td></tr><tr><td>2.4</td><td>Integration on a Lie Manifold</td><td>32</td></tr><tr><td>2.4.1</td><td>Relation between a Lie group and its algebra</td><td>33</td></tr><tr><td>2.4.2</td><td>A discrete integration scheme on the tangent space of the manifold</td><td>34</td></tr><tr><td>2.4.3</td><td>Solving a Lie group differential equation via the Magnus expansion</td><td>35</td></tr><tr><td>2.4.3.1</td><td>Truncating the Magnus expansion to approximate the logarithm map derivative</td><td>37</td></tr><tr><td>2.4.3.2</td><td>An approximative pose propagation in the logarithm map</td><td>38</td></tr><tr><td>2.5</td><td>Linear Pose Estimation in a Horizon</td><td>38</td></tr><tr><td>2.5.1</td><td>Problem statement: pose propagation in linear form</td><td>39</td></tr><tr><td>2.5.2</td><td>Discrete pose estimation in a horizon</td><td>39</td></tr><tr><td>2.5.3</td><td>Simulation Experiment</td><td>40</td></tr><tr><td>2.5.4</td><td>Simulation results</td><td>40</td></tr><tr><td>2.6</td><td>Conclusions</td><td>43</td></tr></table>
## 2.1 Introduction

The rise of collaborative robotics has led to a surge in the need for intricate control mechanisms [Ajoudani et al., 2018]. Over time, the focus on safety in human-robot interactions has intensified [Proia et al., 2021]. The essence of these interactions lies in the challenge for robots to operate in constantly changing environments. As a result, robots need to responsively adjust to unforeseen circumstances, all while upholding strict safety standards.

In this context of complex robotic applications, optimization-based controllers offer a convenient way to formulate tasks to be achieved under constraints. These problems can be formulated over one control time step, i.e. reactively [Escande et al., 2014], but can also lead to much more robust behaviours when considering control over receding time horizons. Within this paradigm, the use of Model Predictive Control (MPC) for planning has been widely adopted on humanoid systems to cope with the curse of dimensionality and the inherent complex equilibrium dynamics of these systems [Ibanez et al., 2019]. In contrast, these methods are relatively new in the context of collaborative robotics though its adoption is proving useful in multiple applications [Proia et al., 2022].

One of the main challenges in using a receding horizon formulation is effectively predicting the robot state evolution within a control horizon while ensuring low computational costs for regular updates. For instance, fast ways to estimate predictions can be employed on iterative algorithms [Müller, 2018] for online trajectory generation [Huber et al., 2020]. While some recent studies [Kleff et al., 2021] have managed to compute optimal solutions in a nonlinear fashion, there remains a preference for linear formulations. They are not only computationally efficient but also more straightforward when framing control problems. Simple linear algebraic expressions are more intuitive and can be easily incorporated into a Quadratic Program (QP) — a widely recognized effective framework for robotic problems. However, a comprehensive linearization approach that encompasses both 3D position and orientation<fnref n="1" />, and that aligns well with optimization tools like QP, remains to be proposed.

The aim of this chapter is to introduce a method for estimating the pose trajectory of a rigid body within a receding horizon at every time step. This approach can be applied to any body of a serial manipulator even though the end effector is of particular significance. The objective is to reconstruct its pose trajectory based on an initial pose and a predetermined control input time horizon, here considered at the velocity level.

The proposed method relies on the Lie algebra $SE(3)$ associated to the tangential space of a pose described in $SE(3)$. This method (based on the Magnus Expansion (ME) [Casas et al., 2006]) leads to a linear expression of the transformation between two poses, using the logarithm map. In its truncated form, it leads to a linear propagation/integration formula which has an explicit algebraic form and takes a vector form adapted to the writing of recursive state propagation general formulas.

### Structure

This chapter focuses first on the fundamental concepts of Lie groups and algebras

<footnote n="1">¹Typically of the robot end-effector.</footnote>
used as a mathematical framework for representing and manipulating the poses of rigid bodies in the context of robotics, crucial for the understanding of this work.

The chapter begins with some rigid body motion Lie groups fundamentals in Section 2.2, introducing the position, orientation and unified pose representations used throughout this work. Additionally, it provides parameterization alternatives and offers an overview on how they relate to their respective Lie group manifolds, notably $SO(3)$ and $SE(3)$.

Afterwards, Section 2.3 extends these concepts by showing how they can be employed to parameterize a pose trajectory. It begins by introducing a rigid body pose path in space and discretizing it to show how discrete pose trajectories evolve in a manifold. Then it links this parameterization scheme to a Lie group differential equation to show how poses and twists relate to each other and how poses can be propagated as a function of the rigid body twist. Finally, it reformulates this discretization scheme as a power of exponentials, a concept largely studied in the literature and of central relevance to understand pose horizon estimation strategy used in the following sections.

Then, Section 2.4 presents a more generalized view of Lie group fundamentals. Many of these were already introduced for the specific Lie groups of orientations and poses in the previous sections. Nevertheless, this section offers an overview and outlines an integration scheme that exploits the relation between Lie groups and their algebras to propagate the elements in the tangent space. This finally introduces the desired linearization strategy that allows for efficient linear pose estimation in a horizon on the tangent space.

Finally, Section 2.5 formalizes the method of linear pose estimation in a horizon and connects all the concepts previously introduced to provide a formal mathematical formulation. Finally, it presents a simple kinematic simulation to showcase the results that showcases the precision and computational speed of the approach.

## 2.2 Rigid Body Motion in Space ↑

To solve many of the problems in robotics, one starts from conveniently describing the robot positioning in 3D space. For instance, to effectively control the movements of a robotic arm, it is important to correctly parameterize its position and orientation. This section delves into the representation of a robot rigid body pose, infinitesimal pose increments, conventional frames in robotics and how these relate to the special orthogonal group $SO(3)$ and the special Euclidean group $SE(3)$. Furthermore, it presents the relationship between Lie groups and their algebras to help understand their exponential coordinates representations.

Robots can be modeled as an array of bodies that do not deform or change shape (rigid body) and whose relation is fixed. In other words, they are represented as a collection of interconnected rigid bodies whose relative distances and orientations remain constant (unless designed otherwise, like with actuated joints).

The effectiveness of the robot in carrying out tasks depends on both the nature of the task and, more importantly, the level of control over these variables. It is fundamentally important because it directly affects the incidence angle and distance at
<figure><img src="image_8.png" /><figcaption>Figure 2.1: A simple way to represent 3D positions and orientations in space is through vectors.</figcaption></figure>

which these tools interact with objects. This is why one particularly relevant body of a robotic serial arm is the end-effector, where the tools specific for the task to be performed are mounted.

The robot efficiency in executing tasks is influenced by the nature of the task and, more importantly, the control over the associated variables like the incidence angle and distance to the objects that the robot interacts with. This is why the end-effector of a robotic serial arm is often the body studied in many applications (sometimes implicitly) — it's where the tools are attached.

Many of the formulations in this work implicitly refer to motions of the end effector, but the concepts can be applied to any of the robot bodies.

The following sections introduce a way to parameterize orientations and then how to combine them along with position for a unified pose representation. They offer an overview on the subject to remind the reader of some concepts and clarify some notations. An interested reader may find more comprehensive introductions on these subjects in Chapter 3 of [Lynch et al., 2017], Section 2.2 of [Siciliano et al., 2008], Chapter 2 of [Craig, 1986]; [Murray et al., 1994].

### 2.2.1 Orientation of a floating body in space ↑

Much of this work is based on a specific representation method for orientations. This section focuses on that specific method and offers an overview of why it is convenient. For a more exhaustive introduction of the different representation alternatives, refer to Section 2.2 of [Siciliano et al., 2008].

The position of a floating body in space can be represented with a 3-vector $p \in \mathbb{R}^3$, where $p = [p_x \ p_y \ p_z]^T$ designates the displacement from the origin of the global frame to arrive at some point $p$, as shown in Figure 2.1(a).

Representing three-dimensional orientations in space requires a bit more work than positions. In general, orientations in space can be visualized as a floating XYZ frame, as shown in Figure 2.1(b). Just like with positions, they can be interpreted as the rotational displacement required for each of its axes to reach a certain angle with respect to some set of “base” axes frame, while sharing the same origin point. As such, one
needs at least 3 parameters to uniquely identify them. There exist multiple ways to parameterize orientations that have different intrinsic advantages and drawbacks that ultimately determines which one is better suited for each application.

**REMARK. Commonly used frames in rigid body motions**

This work exploits spatial algebra to express rigid body motion quantities in a concise manner. All these quantities require a frame in which they are expressed. This means that the same instantaneous body motion (analogue to a velocity) can be represented, for instance in a local frame and thus be defined with respect to a moving body or in any other arbitrary inertial frame. There are two commonly used frames in the literature when working with this kind of physical quantities: they are normally defined with respect to the body in question or to some arbitrarily chosen universal world frame, as depicted in Figure 3.2:

* **World**: A global, universal or world frame, often referred to as frame *S* or *W*.

* **Local**: Also referred to as local or body frame and designated as *L* or *B*, represents a frame placed and aligned with the designated body. This corresponds to a frame attached to the moving body.

Both of these frames are used extensively in this manuscript. Their relevance becomes clear once some Lie group fundamentals are introduced, showing that their formulations change depending on the employed frame.

For instance, Euler angles offer easy interpretability and require only 3 parameters but suffer from some singularities like the gimbal lock. To avoid these issues, quaternions offer a good alternative even though they require some deeper understanding to use and their representation is not as compact, as they use 4 parameters. However, they can also be used for composing successive re-orientations, which is a common problem in robotics, where the orientations of bodies are often defined with respect to other bodies or frames in the environment [Brockett, 1984].

Yet another way of representing orientations is rotation matrices, often denoted as *R*. These are 3 × 3 matrices with some characteristics that make them specially convenient: they can be viewed as a transformation matrix capable of performing a rotation in Euclidean space.

<figure><img src="image_9.png" /><figcaption>Figure 2.2: Depiction of two conventional frames used to describe rigid body motions in space: *S* is the world or universal frame and *L* is a local or body frame and corresponds to a moving frame placed on the concerned body.</figcaption></figure>
To construct a rotation matrix, one can imagine a frame denoted by 3 orthonormal vectors expressed a global frame $x, y, z \in \mathbb{R}^3$ representing 3 points attached to a body. Re-orienting this body results in 3 new coordinates for these points $x', y', z' \in \mathbb{R}^3$ that can be used to construct the rotation matrix as $R = [x' y' z']$. This can be interpreted visually in Figure 2.1(b). In particular, there is a set of matrices belonging to the special orthogonal group $SO(3)$ such that:

$$ R^T = R^{-1} \qquad \det R = 1 \qquad R \in \mathbb{R}^{3 \times 3} \qquad (2.1) \\ \Rightarrow R \in SO(3) $$

which can perform rotations on the Euclidean space without modifying distances: given some vector $e \in \mathbb{R}^3$, it can be rotated to obtain a new vector with the same length $e' \in \mathbb{R}^3$ as $e' = Re$.

In terms of formulation, rotation matrices are advantageous due to their ability to rotate Cartesian vectors through a matrix multiplication, unlike quaternions, which is a commonly used feature in robotics. Rotation matrices also provide a straightforward method for composing rotations. However, compared to quaternions, they are less efficient in representing orientations as they require 9 parameters instead of just 4.

The $SO(3)$ group constitutes a Lie group, i.e. a group that is also a differentiable manifold that locally resembles the Euclidean space. This kind of groups where first introduced by Sophus Lie in 1888 [Merker, 2010] and have been extensively studied in the literature. They are of special interest for the study of motions in space because they provide a natural framework for their concepts of continuous transformations.

This work uses this kind of matrices to represent orientations and rotation operations. The following sections present other special properties and their relation to the special Euclidean group.

**REMARK. Group Definition** A group is a non-empty set $\mathcal{G}$ with a binary operation denoted $\cdot$ such that any combination of two elements of $\mathcal{G}$ yields a new one: $a \cdot b = c$ where $a, b, c \in \mathcal{G}$. It must also respect the group axioms [Lang, 2002] such that:

* Associativity: for $a, b, c \in \mathcal{G}$, then: $(a \cdot b) \cdot c = a \cdot (b \cdot c)$.

* Identity element: there exists an identity element $e \in \mathcal{G}$ such that for $a \in \mathcal{G}$: $e \cdot a = a \cdot e = a$.

* Inverse element: for each element $a \in \mathcal{G}$ there exists $b \in \mathcal{G}$ such that: $a \cdot b = b \cdot a = e$.

### 2.2.1.1 Some properties of rotation matrices ↑

Notice that rotation matrices can be interpreted as the required “re-orientation” transformation to be applied over a base frame to achieve some orientation (see Figure 2.1(b)). For instance, $R_{S1}$ defines the rotation required to go from some universal frame S to some body 1 orientation. Just the same, the rotation necessary to go from the orientation of body 1 to the frame S is the inverse: $R_{1S} = R_{S1}^{-1}$, which, from the properties of $SO(3)$ in (2.1), corresponds to its transpose: $R_{S1}^{-1} = R_{S1}^T$.
Additionally, it is also possible to obtain the rotation of a body 1 with respect to some other body 2 (i.e. $R_{12}$), as long as their own orientations with respect to some other frame is known. For example, given $R_{S1}$ and $R_{S2}$:

$$R_{12} = R_{S1}^{-1} R_{S2} = R_{1\phi} R_{\phi 2} \qquad (2.2)$$

In robotics, every quantity is frequently defined with respect to some universal frame. When that is the case, the notation can be simplified such that: $R_2 = R_{S2}$. In the same way, its inverse refers to $R_2^{-1} = R_{2S}$. In contrast, if a rotation was defined with respect to some other body, then it needs to be explicitly indicated, for example: $R_{12}$.

A common challenge in robotics is determining the orientation of a body connected to other bodies by utilizing their relative rotations. Take, for instance, determining the orientation of a frame attached to a robot joint. For example, if joint 2 is connected to joint 1 through a link, their rotation matrices can be combined through multiplication, resulting in a sequence of rotations:

$$R_2 = R_{S1} R_{12} \qquad R_{S1}, R_{12}, R_2 \in SO(3) \qquad (2.3)$$

Later sections link these concepts to the rate of change in orientations. Then, these notions are extended for poses to obtain a unified representation of a position and orientation.

**REMARK.** Commutativity of $SO(n)$ A special characteristic of the $SO(n)$ for $n \le 2$ (which includes orientations in 2D) is that they are abelian groups, meaning they are commutative:

$$R_a R_b = R_b R_a \qquad R_a, R_b \in SO(n) \quad \text{for } n \le 2 \qquad (2.4)$$

However, this is not the case for the more general special orthogonal groups $SO(n)$ for $n > 2$ that are non-commutative. So the order in which the rotations are applied has an effect on the result.

### 2.2.1.2 Axis-angle representation ↑

One implication of Euler's rotation theorem [Euler, 1765] is that any displacement of a rigid body in 3D space such that a point in it remains fixed can be described by a single rotation about an axis passing through that point. In fact, there exists an exponential operation capable of mapping this axis-angle representation to the space of rotation matrices.

This method has been extensively studied in the literature. Some of the first mentions that make the connection with a spatial motion date back to 1840 by Olinde Rodrigues [Rodrigues, 1840].

Figure 2.3(a) shows a rotation example around an axis aligned with the Z axis. Alternatively, more complex rotations can also be represented for an arbitrary rotating axis, as seen in Figure 2.3(b).
<figure><img src="image_10.png" /></figure>

(a) An example re-orientation where the rotation axis is coincident with the z axis.

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

(b) A more complex example of a 3D rotation with an arbitrary rotation axis.

<caption>Figure 2.3: Rotations can be represented in an axis-angle form comprised of a rotation angle around a rotation axis (vector). Figure 2.3(a) shows the movement of a point attached to a rigid body moving at an angular velocity *rotvel*. Figure 2.3(b) depicts the exponential coordinates *φ* of a rotation.</caption>

The axis-angle representation allows describing a rotation by an angle *φ* about a unitary axis denoted $\hat{\phi} \in \mathbb{R}^3$. These parameters can be used to form a single vector $\boldsymbol{\phi} = \boldsymbol{\phi}\hat{\phi}$ (note that $\boldsymbol{\phi} = \|\boldsymbol{\phi}\|$) from which an equivalent rotation matrix $\boldsymbol{R}$ can be deduced by employing the Rodrigues' formula:

$$ \boldsymbol{R}(\boldsymbol{\phi}, \hat{\boldsymbol{\phi}}) = \boldsymbol{I}_3 + \sin \boldsymbol{\phi} \boldsymbol{K} + (1 - \cos \boldsymbol{\phi}) \boldsymbol{K}^2 \\ \boldsymbol{\phi} = [\phi_1 \quad \phi_2 \quad \phi_3]^T $$

$$ \boldsymbol{K} = \begin{bmatrix} 0 & -\phi_3 & \phi_2 \\ \phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{bmatrix} \quad (2.5) $$

While not crucial to this chapter, this formula presents a closed-form for the exponential to map 3-vectors to rotation matrices [Gogu et al., 1996]. The link with the exponential is further analyzed in the following sections.

### 2.2.1.3 Angular velocity ↑

The angular velocity is the rate of change in the orientation. It can be related to the time derivative of a rotation matrix. This result has recently been revisited in [Hamano, 2013]; [Zhao, 2016].

The physical notion can be simply constructed from the motion of a point attached to a body. In particular, Figure 2.3(a) depicts a point attached to a rigid body frame moving at an angular velocity denoted $\omega \in \mathbb{R}^3$ (a representation introduced in the previous section). If $\boldsymbol{p}(t) \in \mathbb{R}^3$ denotes the position of this point in time, then $\dot{\boldsymbol{p}}(t) \in \mathbb{R}^3$ denotes its time derivative, i.e. its velocity, then:

$$ \dot{\boldsymbol{p}}(t) = \boldsymbol{\omega}(t) \times \boldsymbol{p}(t) \quad (2.6) $$
where $\cdot \times \cdot$ denotes the vector cross-product and $\omega(t)$ describes a time-varying angular velocity. The vector cross-product can also be expressed as the product of a skew-symmetric matrix and a vector:

$$ \dot{\mathbf{p}} = \omega^{\wedge} \mathbf{p} \qquad (2.7) $$

where the time dependency was dropped for a clearer notation. Here, $\omega^{\wedge}$ denotes the Hat operation, described in (2.8).

**REMARK.** The Hat/Vee map for $SO(3)$ The Hat map allows re-parameterizing a 3-vector as a $3 \times 3$ skew-symmetric matrix. The inverse operation is known as the Vee map. One can go from one to the other through the reciprocal operations:

$$ \begin{array}{l} \text{Hat: } \cdot^{\wedge} : \mathbb{R}^3 \to \mathbb{R}^{3 \times 3} \\ \text{Vee: } \cdot^{\vee} : \mathbb{R}^{3 \times 3} \to \mathbb{R}^3 \end{array} \qquad (2.8) $$

$$ \omega^{\wedge} = \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix} \xrightarrow{\cdot^{\vee}} \omega = \begin{bmatrix} \omega_1 \\ \omega_2 \\ \omega_3 \end{bmatrix} \qquad (2.9) $$

The Hat map enables the vector cross-product to be expressed as the product of a skew-symmetric matrix and a vector. For instance, given 2 vectors $\omega, \mathbf{p} \in \mathbb{R}^3$:

$$ \omega^{\wedge} \mathbf{p} = \omega \times \mathbf{p} \qquad (2.10) $$

This same process can be applied for points attached to each of the body frame axes $\mathbf{x}(t), \mathbf{y}(t), \mathbf{z}(t) \in \mathbb{R}^3$ moving at the same angular velocity, yielding three velocity vectors: $\dot{\mathbf{x}}(t), \dot{\mathbf{y}}(t), \dot{\mathbf{z}}(t) \in \mathbb{R}^3$ that can be used to construct the angular displacement rate as a rotation matrix time derivative $\dot{\mathbf{R}}$:

$$ \dot{\mathbf{R}}(t) = \begin{bmatrix} \dot{\mathbf{x}}(t) & \dot{\mathbf{y}}(t) & \dot{\mathbf{z}}(t) \end{bmatrix} \qquad (2.11) $$

$$ = \begin{bmatrix} \omega(t) \times \mathbf{x}(t) & \omega(t) \times \mathbf{y}(t) & \omega(t) \times \mathbf{z}(t) \end{bmatrix} \qquad (2.12) $$

which can in turn be factorized as:

$$ \dot{\mathbf{R}}(t) = \mathbf{W}(t) \begin{bmatrix} \mathbf{x}(t) & \mathbf{y}(t) & \mathbf{z}(t) \end{bmatrix} \qquad \mathbf{W} = \omega^{\wedge} \qquad (2.13) $$

where $\omega^{\wedge}$ denotes the skew-symmetric matrix of the angular velocity $\omega$.

Given both $\mathbf{R}, \dot{\mathbf{R}}$ belong to $SO(3)$ (and so they respect the properties presented in (2.1)), then it can be shown that $\mathbf{W}$ belongs in its tangent space $so(3)$, composed of elements that represent infinitesimal increments of the $SO(3)$ manifold. In fact, all the elements of $so(3)$ share a rather relevant property: they are all skew-symmetric and this enables exploiting the bijective Hat/Vee operators to parameterize the
matricial elements of $so(3)$ as vectors in $\mathbb{R}^3$. Such that:

$$ \begin{aligned} \text{Hat } \omega^{\wedge} & : \mathbb{R}^3 \rightarrow so(3) \\ \text{Vee } W^{\vee} & : so(3) \rightarrow \mathbb{R}^3 \end{aligned} \qquad (2.14) $$

Furthermore, $so(3)$ constitutes the Lie algebra of the special orthogonal group $SO(3)$, a vector space together with an operation known as the Lie bracket $[\cdot, \cdot]$ satisfying the Jacobi identity [Solà et al., 2018].

The following section connects the algebra and the group elements through a map, allowing for a more compressed representations of the rotation matrices.

### 2.2.1.4 Axis-angle representation as the rotation Matrix exponential parameterization ↑

Section 2.2.1.2 briefly introduces the Rodrigues formula that maps an axis-angle representation into a rotation matrix. More recent works [Grassia, 1998] (1994), [Chirikjian, 2005] (Chapter 2, 2005) show how to exploit some properties of the rotation matrices that allows expressing them as the exponential of skew-symmetric matrices. Nowadays the method has been further generalized to enable parameterizing roto-translational transformations (i.e. pose matrices) from $SE(3)$. Some detailed overviews can be found in Sections 3.2.3 and 3.3.3 of [Lynch et al., 2017], Section 2.2.5 of [Siciliano et al., 2008] and Sections 2.2 and 3.2 of [Murray et al., 1994].

This section provides an overview of the method without an extensive development. It presents the method for orientations as it is simpler but it is extended by analogy for poses in later sections.

To understand how an axis-angle representation $\phi$ can be used to parameterize $R$ through the exponential, one needs to first understand that (2.13) can also be interpreted as a linear Lie group differential equation:

$$ \dot{R}(t) = W(t)R(t) \qquad (2.15) $$

In the most general cases, due to the non-commutative nature of the Lie groups and the fact that $W$ is time-varying, this differential equation does not admit a simple solution. For the time being, assuming a constant $W$, it is simplified to:

$$ \dot{R}(t) = WR(t) \qquad (2.16) $$

Thanks to some well-known results [Hairer et al., 2006] (see Section 3.2.3.1 in [Lynch et al., 2017]), this linear differential equation can be solved through:

$$ R(t) = e^{Wt} R_0 \qquad (2.17) $$

where the exponential refers to the matrix exponential and $R_0 = R(0)$ corresponds to the initial orientation. Furthermore, by assuming that $R_0$ is in fact the identity element of $SO(3)$ and that $t = 1$, the equation becomes:

$$ R = e^W \qquad W = \omega^{\wedge} \qquad (2.18) $$
showing that it is possible to find an element $W \in so(3)$ such that its exponential represents an element $R \in SO(3)$.

Remembering that elements of $so(3)$ can be mapped to $\mathbb{R}^3$ through the Vee map (see (2.8)), then $W = \omega^\wedge$ can also be interpreted as the axis-angle representation of a rotational movement equivalent to $R$. In fact, $R$ is the result of integrating an angular velocity $||\omega||$ around an axis $\hat{\omega}$ for a unit of time. This vector $\omega$ is referred to as the exponential coordinates of $R$.

Finally, (2.18) shows that rotation matrices can be parameterized through the exponentiation of a skew-symmetric matrix constructed from a 3-vector axis-angle representation. One could then wonder if it is also possible to retrieve this vector from a rotation matrix. And indeed it is possible via the reciprocal operation, i.e. the matrix logarithm map:

$$ \exp \omega : \mathfrak{so}(3) \rightarrow \mathfrak{SO}(3) \\ \log R : \mathfrak{SO}(3) \rightarrow \mathfrak{so}(3) \qquad (2.19) $$

Note that $\exp C = e^C$ are equivalent can used indifferently. The choice stems from either clarity or convenience. Also, the exponential and logarithm maps denoted here are defined for matrices but, as defined in (2.8), for the Lie algebra elements employed in this work, there is a bijective map between the vector and matrix forms (see (2.8)). By abuse of notation, $W \in so(3)$ may be referred to as an “angular velocity” because $W = \omega^\wedge$ and the following forms are treated as equivalent:

$$ \exp \omega \equiv \exp W \qquad \log R = \omega \equiv \log R = W \qquad (2.20) $$

This map is essential for understanding some integration techniques employed throughout this manuscript. These techniques simplify the problem of integrating rigid body dynamics when operating with group elements of a manifold like the orientation. For instance, they can be used to propagate the effects of an angular velocity on the orientation in its tangent space efficiently and later retrieve the result. This is explained in later sections of this chapter.

**REMARK.** The Rodrigues Parameters Olinde Rodrigues showed a closed-form to solve $e^{\omega^\wedge}$ from its vector form $\omega$ [Rodrigues, 1840], see (2.5).

### 2.2.2 Pose of a floating body in space ↑

Previous sections introduce a way to represent the position and orientation of a body in space. This section constructs a way to combine them into a unified representation in a single element.

The pose of a rigid body in the robot can be represented as a floating frame in space (see Figure 2.4). This frame is uniquely identified by a position $p$ and an orientation $R$. Both elements can be assembled into a 4 × 4 homogeneous transformation
<figure><img src="image_12.png" /><figcaption>Figure 2.4: Representation of a pose in 3D space.</figcaption></figure>

matrix that goes from the universal frame to the body frame B:

$$T_B = \begin{bmatrix} R & p \\ 0_{1 \times 3} & 1 \end{bmatrix} \qquad (2.21) \qquad \text{revisited } \downarrow$$

$$\text{with: } \mathbf{p} \in \mathbb{R}^3 \qquad R \in \mathrm{SO}(3) \qquad T_B \in \mathbb{R}^{4 \times 4}$$

Just as with rotation matrices, the notation is simplified when the transformation is expressed with respect to the universal frame S. This means that for the body pose defined above: $T_B = T_{SB}$.

In fact, the set of all homogeneous transformation matrices is called the special Euclidean group $\mathrm{SE}(3)$ and any element $\mathcal{X} \in \mathrm{SE}(3)$ can be denoted as: $\mathcal{X} = (R, p)$.

### 2.2.2.1 Pose as a frame displacement ↑

Just as with the rotation matrices (see Section 2.2.1.1), the elements of $\mathrm{SE}(3)$ can be interpreted as the roto-translational displacement between two frames. And just as in (2.3), they can be used to construct a body pose from their relative homogeneous transformation matrices:

$$T_2 = T_{S1} T_{12} \qquad T_{S1}, T_{12}, T_2 \in \mathrm{SE}(3) \qquad (2.22)$$

To obtain these relative transformations between two bodies 1 and 2 (i.e. $T_{12}$), one can use an expression similar to (2.2) (as long as their own poses are known with respect to some common frame), as this:

$$T_{12} = T_{S1}^{-1} T_{S2} \qquad (2.23)$$

**REMARK.** Commutativity of $\mathrm{SE}(3)$ Just like $\mathrm{SO}(3)$, the $\mathrm{SE}(3)$ group is non-commutative (see (2.4)).

### 2.2.2.2 Pose derivative ↑

Similarly to how the angular velocity relates to a rate of change in the orientation, linking the orientation manifold $\mathrm{SO}(3)$ to its tangent space $\mathrm{so}(3)$, there is a way to
describe the rate of change in a pose. This rate lies in the tangent space of the special Euclidean group SE(3), known as the algebra se(3). Elements of this algebra can be referred to as spatial twists $v \in se(3)$ (see Section 3.3 of [Lynch et al., 2017]) and are composed of an angular component $\omega \in so(3)$ (a rotational velocity, that belongs to the Lie algebra of SO(3)) and a linear component $v \in \mathbb{R}^3$ (analog to the linear velocity).

To see how $v$ relates to the pose time derivative $\dot{X}$, it is possible to extend (2.15) for pose elements in SE(3):

$$ \dot{X}(t) = \mathcal{H}(t)X(t) \qquad (2.24) $$

revisited ↓

such that $\dot{X}, X \in SE(3)$ are elements of the pose manifold and $\mathcal{H} \in se(3)$ belongs to its tangent space. This forms a linear time-varying Lie group differential equation that is non trivial to solve.

Note that $\mathcal{H}$ can then be re-parameterized as a vector through the Vee map with (2.25). The relevance of these identities becomes clearer in the following sections. Notably, the following shows how they are used for an alternative parameterization of the pose element matrices.

**REMARK.** The Hat/Vee map for SE(3) The Hat/Vee map definitions for the orientations in (2.8) can be extended in a similar way to parameterize the twists either as a matrix or as a 6-vector. This allows going back and forth between $\mathbb{R}^6$ and $\mathbb{R}^{4 \times 4}$ as:

$$ \mathbf{v}^{\wedge} = \begin{bmatrix} \mathbf{\omega}^{\wedge} & \mathbf{v} \\ \mathbf{0}_{1 \times 3} & 0 \end{bmatrix} \xrightarrow{\cdot, \wedge} \mathbf{v} = \begin{bmatrix} \mathbf{v} \\ \mathbf{\omega} \end{bmatrix} \qquad (2.25) $$

$$ \text{with:} \qquad \mathbf{v} = \begin{bmatrix} v_1 \\ v_2 \\ v_3 \end{bmatrix} \qquad \mathbf{\omega} = \begin{bmatrix} \omega_1 \\ \omega_2 \\ \omega_3 \end{bmatrix} \qquad (2.26) $$

where $\mathbf{v}^{\wedge} \in \mathbb{R}^{4 \times 4}$ and $\mathbf{v} \in \mathbb{R}^6$.

### 2.2.2.3 Screw motion as the pose matrix exponential parameterization ↑

Thanks to the Mozzy-Chasles theorem [Chasles, 1830]; [Davidson et al., 2004], the concepts from Euler's Rotation theorem can be extended to the more general rotational motions. The theorem states that any rigid body motion can be realized by a rotation about an axis along with a translation parallel to that same axis.

This means that the same strategy employed in Section 2.2.1.4 for the parameterization of rotation matrices as axis-angle vectors through the exponential map can be applied for the pose matrices. This time though, the parameterization requires 6 parameters that can be interpreted as two 3-vectors: one associated with the rotation axis and another linked to the linear displacement (see Section 3.3.3 of [Lynch et al., 2017]).
To understand this method, one can start by revisiting (2.24), assuming $\mathcal{H}$ remains constant to simplify the expression

$$ \dot{\mathcal{X}}(t) = \mathcal{H}\mathcal{X}(t) \qquad (2.27) $$

and utilize some known ordinary linear differential solutions [Hairer et al., 2006] (see Section 3.2.3.1 in [Lynch et al., 2017]):

$$ \mathcal{X}(t) = e^{\mathcal{H}t}\mathcal{X}_0 \qquad (2.28) $$

where the exponential refers to the matrix operation and $\mathcal{X}_0 = \mathcal{X}(0)$ corresponds to the initial pose. Furthermore, without loss of generality, it can be assumed that the initial pose coincides with the identity element and that for t=1:

$$ \mathcal{X} = e^{\mathcal{H}} \qquad \mathcal{H} = \xi^{\wedge} \qquad (2.29) $$

which shows that it is possible to map elements from the $SE(3)$ manifold to elements of its tangent space $sc(3)$ through the exponential operation. This vector $\xi$ is referred to as the exponential coordinates of $\mathcal{X}$. Furthermore, the reciprocal operation (the logarithm map) allows going back and forth between both representations:

$$ \exp \mathcal{H} : \mathcal{sc}(3) \rightarrow \mathcal{SE}(3) \qquad (2.30) $$

$$ \log \mathcal{X} : \mathcal{SE}(3) \rightarrow \mathcal{sc}(3) $$

For simplicity, the same notation showed in (2.20) is extended to (2.30). This means that $\mathcal{H} \in \mathcal{sc}(3)$ may be referred to as a “spatial twist” because $\mathcal{H} = \xi^{\wedge}$ and the following expressions are treated as equivalent:

$$ \exp \xi \equiv \exp \mathcal{H} \qquad \log \mathcal{X} = \xi \equiv \log \mathcal{X} = \mathcal{H} \qquad (2.31) $$

The following sections exploit the representations introduced previously to describe rigid body trajectories in space as time-varying pose elements in time.

## 2.3 Rigid Body Pose Trajectory in Space

The concepts introduced in the previous sections allow representing static positions and orientations in space and properly parameterizing them in a vector form. This section focuses on how to use these principles to parameterize a roto-translational motion in space, which is the core problem of this work. The objective is to describe the time-varying pose elements that allow the parameterization of pose trajectories.

**REMARK.** Trajectory vs Path Because there is no associated timing to each step, this is referred as a *path*. Otherwise, it is referred as a *trajectory*. Such case is treated in subsequent sections.

### 2.3.1 Pose path in space

This section deals with the problem of discretizing a position and orientation (pose) path as a function of some initial pose and the successive increments between each
<figure><img src="image_13.png" /><figcaption>Figure 2.5: Representation of a position path in 3D space.</figcaption></figure>

<figure><img src="image_14.png" /><figcaption>Figure 2.6: Representation of a pose path in 3D space.</figcaption></figure>

step. To do so, it first focuses on a position path that can be introduced more intuitively and then adds the orientations to build a full pose path.

Let us assume that it is possible to define a position path in space with respect to some parameter s as *p(s)*, where s goes from 0 (at the start of the path) to 1 (at the end). Each point in this path (that defines a position) can then be parameterized as a 3-vector such that *p(s) ∈ R³*. A representation of such path is depicted in Figure 2.5.

Extending this definition, it is possible to define a pose path in space with respect to some parameter s in the same way. Each point in this path defines a position and orientation that can then be parameterized as an homogeneous transformation matrix such that $X(s) \in SE(3)$. A representation of such path is depicted in Figure 2.6
and can be parameterized as:

$$ \mathcal{X}(s) = \begin{bmatrix} \mathbf{R}(s) & \mathbf{p}(s) \\ \mathbf{0}_{1 \times 3} & 1 \end{bmatrix} \qquad (2.32) $$

$$ \text{with:} \quad \mathbf{p}(s) \in \mathbb{R}^3 \qquad \mathbf{R}(s) \in \mathbf{SO}(3) \qquad \mathcal{X}(s) \in \mathcal{SE}(3) $$

where $\mathbf{R}(s)$ does for the orientation what $\mathbf{p}(s)$ does for the position: it parameterizes an orientation path as a function of some parameter s.

### 2.3.2 Discretizing a pose path ↑

One can discretize the s parameter to sample some of the poses throughout this path such that $\mathcal{X}_k = \mathcal{X}(s_k)$. Then the pose increment required to go from $\mathcal{X}_k$ to $\mathcal{X}_{k+1}$ is designated $\mathcal{X}_{k,k+1}$; the added comma makes the reference to each discretized step clearer but it is not needed after the following definition:

$$ \Delta\mathcal{X}_k = \mathcal{X}_{k,k+1} = \mathcal{X}_k^{-1}\mathcal{X}_{k+1} \qquad (2.33) $$

subsequently, the pose at each discretized step can be retrieved through:

$$ \mathcal{X}_{k+1} = \mathcal{X}_k \Delta \mathcal{X}_k \qquad (2.34) $$

Considering $\mathcal{X}_k$ describes the successive poses of a body moving through space, then (2.33) designates a "local" roto-translational displacement or transformation, because its base frame is $\mathcal{X}_k$, a frame moving with the body. Remembering (2.29) and (2.31), one can see that there exists an equivalent twist that represents this displacement through its exponential coordinates:

$$ \Delta\mathcal{X}_k = e^{S_k} \qquad \Delta\mathcal{X}_k \in \mathcal{SE}(3) \qquad S_k \in \mathfrak{se}(3) \qquad (2.35) $$

yielding:

$$ \mathcal{X}_{k+1} = \mathcal{X}_k e^{S_k} \qquad (2.36) $$

where the equivalent twist $S_k$ can also be considered a local or body twist. This is due to the chosen right-multiplied propagation scheme in (2.34). As this is a Lie manifold, it may also be formulated to use a left-multiplied convention:

$$ \begin{gathered} \mathcal{X}_{k+1} = \Delta\mathcal{X}'_k \mathcal{X}_k \qquad \Delta\mathcal{X}'_k = \mathcal{X}_{k+1}\mathcal{X}_k^{-1} \qquad (2.37) \\ \mathcal{X}_k, \mathcal{X}_{k+1}, \Delta\mathcal{X}'_k \in \mathcal{SE}(3) \qquad \Delta\mathcal{X}'_k = e^{\mathcal{H}_k} \qquad \mathcal{H}_k \in \mathfrak{se}(3) \end{gathered} $$

It can be shown that $\mathcal{H}$ is in fact the same twist $S$ expressed in the global frame (see (2.38)). This, by analogy extends the previous interpretation to imply that $\Delta\mathcal{X}'_k$
refers to a pose variation expressed in the universal frame.

**REMARK. World vs Body Twist** The relation between a body twist $^B\mathcal{H}$ and a twist expressed in the universal frame $^S\mathcal{H}$ is (see Section 5.1 of [Traversaro et al., 2019]):

$$ ^S\mathcal{H} = \mathcal{X}_B^B\mathcal{H}\mathcal{X}_B^{-1} \qquad \mathcal{X}_B \in \mathbb{SE}(3) \qquad \mathcal{H} \in \mathfrak{SE}(3) \qquad (2.38) $$

This also constitutes the adjoint action map of $\mathbb{SE}(3)$ over $\mathfrak{SE}(3)$. One property of this map is that:

$$ e^{\mathcal{X}\mathcal{H}\mathcal{X}^{-1}} = \mathcal{X}e^{\mathcal{H}}\mathcal{X}^{-1} \qquad (2.39) $$

Using (2.38) and (2.39) to replace in (2.36) (where $^B\mathcal{H} = \mathcal{H}$ and $^S\mathcal{H} = \mathcal{S}$) yields (2.37), showing they are indeed equivalent.

### 2.3.3 Single step integration as a differential equation ↑

The previous section served to intuitively introduce a way to propagate a pose path through a manifold as a function of a twist. This section connects concepts from the rotation and pose matrices with their derivatives (Sections 2.2.1.4 and 2.2.2.3 with derivatives in Sections 2.2.1.3 and 2.2.2.2). In fact, both derivatives are analogous because they can be interpreted as a Lie group differential equation and so only the case of the pose is revisited here.

The relevance of this comes from the fact that (2.37) is in fact the solution to the differential equation presented by the pose derivative:

$$ \dot{\mathcal{X}}(t) = \mathcal{H}(t)\mathcal{X}(t) \qquad (2.40) $$

revisited ↑

where $\mathcal{H}$ is expressed in the global frame as in (2.28); this implies it is a left-trivialized Lie differential equation (refer to world and body twists in (2.38) or, for an external reference, refer to page 112 in Section 3.5 Summary in [Lynch et al., 2017]). In contrast, the right-multiplied version of this differential equation would be:

$$ \dot{\mathcal{X}}(t) = \mathcal{X}(t)\mathcal{S}(t) \qquad (2.41) $$

where $\mathcal{S}$ represents a local twist and is integrated for a first step with (2.36).

Note that both solutions constitute a first-order Taylor integration expansion in the manifold [Forster et al., 2015]; [Solà et al., 2018] that is central to the linearization strategy that is presented in the following sections.

### 2.3.4 Pose trajectory as a product of exponentials ↑

Finally, this section presents how adding a time notion to a pose path allows formulating a trajectory in $\mathbb{SE}(3)$. As shown in (2.34), it is possible to reconstruct each discretized pose in each step as a function of the previous pose and a local pose increment. This notion can be extended for the whole path by discretizing the s parameter in N steps so that $s_k$ is defined for $k = 0, 1, \dots, N$, then:

$$ \mathcal{X}_{k+1} = \mathcal{X}_0\Delta\mathcal{X}_1\Delta\mathcal{X}_2\dots\Delta\mathcal{X}_k \qquad (2.42) $$
that can be further generalized as:

$$ \mathcal{X}_{k+1} = \mathcal{X}_0 \prod_{i=0}^{k} \Delta \mathcal{X}_i \qquad \Delta \mathcal{X}_i = \mathcal{X}_k^{-1} \mathcal{X}_{k+1} \qquad (2.43) $$

Remembering (2.35), each pose increment can be replaced for its equivalent exponential coordinates:

$$ \mathcal{X}_{k+1} = \mathcal{X}_0 \prod_{i=0}^{k} e^{S_i} \qquad S_i = \log(\Delta \mathcal{X}_i) \qquad (2.44) $$

where log denotes the matrix logarithm (see (2.30)). However, these exponential coordinates do not have a timing notion. To obtain an equivalent body twist analogous to the instantaneous velocity of the body in space it is necessary to add a time discretization to each $s_k$ such that:

$$ s(t) \in [0, 1] \qquad s_k = s(t_k) \qquad t_k = k \Delta t \qquad (2.45) $$

$$ \text{for } k = 0, 1, \dots, N \qquad (2.46) $$

And now (2.44) can be reformulated assuming the body is moving at a constant body twist $V_k$ at each time step:

$$ \mathcal{X}_{k+1} = \mathcal{X}_0 \prod_{i=0}^{k} e^{\Delta t V_i} \qquad V_i = \frac{S_i}{\Delta t} \qquad (2.47) \qquad \text{revisited } \downarrow $$

This formula shows one more application of the product of exponentials introduced by Brockett in [Brockett, 1984] for open kinematic chains.

For a single step integration, one can obtain an explicit expression as a function of the body twist based on (2.47):

$$ \mathcal{X}_{k+1} = \mathcal{X}_k e^{\Delta t V_k} \qquad (2.48) \qquad \text{revisited } \downarrow $$

Another way to interpret this propagation scheme is under the assumption of a "zero-order hold", i.e. that the body moves at a constant $V_i$ for a duration of $\Delta t$ at each step.

**REMARK. Rotation Path** The equations presented in this section were formulated for $SE(3)$ but they are analogous to their version in $SO(3)$ with $\mathcal{X} = \mathbf{R}$ and $\Delta\mathcal{X} = \Delta\mathbf{R}$.

## 2.4 Integration on a Lie Manifold $\uparrow$

Previous sections introduce the fundamental modeling aspects of floating rigid bodies in space. These were used to parameterize serial robot kinematic chains movements in the workspace as Lie group actions on a manifold. This can be specially appreciated in the product of exponentials, where the successive discrete roto-translational actions parameterize the body pose trajectory (see Section 2.3.4).
<figure><img src="image_15.png" /><figcaption>Figure 2.7: Depiction<fnref n="2" /> of an integration scheme in the tangent space.</figcaption></figure>

This section exploits the relation between the Lie groups and algebras to offer an equivalent algebraic integration scheme in the tangent space. It approaches the same concepts from a more general point of view to find a more convenient linear expression to the same problem. While the previous presentation relies on rather specific Lie groups (for the orientation and poses), this section borrows more general properties of Lie groups and applies them to present an efficient estimation strategy of the propagated poses in the manifold.

For a more comprehensive overview on the fundamentals employed in here, refer to [Solà et al., 2018].

### 2.4.1 Relation between a Lie group and its algebra ↑

For a given Lie group $\mathcal{G}$, there exists a tangent space at the identity that defines a Lie algebra $\mathfrak{g}$, the space of infinitesimal increment elements. The elements from the group can be mapped to their tangent space equivalents through the logarithm map (with the reciprocal operation being the exponential) such that for:

$$ \begin{aligned} \exp : \quad & T_e \mathcal{G} \to \mathcal{G} \\ \log : \quad & \mathcal{G} \to T_e \mathcal{G} \end{aligned} \qquad (2.49) $$

where $T_e \mathcal{G}$ denotes the tangent space of $\mathcal{G}$ at the identity element $e \in \mathcal{G}$. This same relationship applies to the two previously analyzed cases: for orientations, it applies to $SO(3)$ and $so(3)$ (see Section 2.2.1.4); and for poses, it can be applied to $SE(3)$ and $sc(3)$ (see Section 2.2.2.3).

Note that the exponential map surjectively covers the $SO(3)$ manifold. This means that, for instance, given $\phi \in so(3)$, it yields a one to one correspondence to $R \in SO(3)$ through the logarithm map as long as $||\phi|| < \pi$. This condition in turn extends for the angular component of an element of the tangent space of the pose manifold $sc(3)$ [Forster et al., 2015]; [Torres Alberto et al., 2022a].

<footnote n="2">²Based on tikz.net/manifold.</footnote>
The goal of this section is to progressively integrate a path on the manifold through the propagation of the infinitesimal changes. It is possible to exploit this relation with the tangent space to map all elements from the Lie group manifold to elements in the tangent space. This re-parameterization is often referred to as a *push* [Absil et al., 2008] operation (also denoted *lift* [Solà et al., 2018]). Furthermore, an infinitesimal action on the manifold can then be mapped to an equivalent increment in the tangent space, which is referred to as a *pushforward*<fnref n="3" /> [Saccon et al., 2013]; [Boumal, 2023]. The inverse action to retrieve the manifold element after the propagation is often referred to as *pull* (or *retract*) operation. A depiction of this process is visualized in Figure 2.7.

The following sections presents this strategy in more detail to integrate the pose actions in the tangent space through a convenient and efficient algebraic expression.

**REMARK.** The Adjoint Action map for Lie Groups The operator Ad represents the adjoint action of a Lie group G that defines a linear transformation on its Lie algebra g. For instance, given some elements $h \in g$, $y \in G$:

$$ \mathbf{Ad} : \mathcal{G} \times \mathcal{g} \to \mathcal{g} \qquad (2.50) $$

$$ \mathbf{Ad}_y h = yhy^{-1} \qquad (2.51) $$

This constitutes the general definition of the operation defined in (2.38) for $\mathbb{SE}(3)$.

### 2.4.2 A discrete integration scheme on the tangent space of the manifold ↑

At the heart of this propagation scheme on a group element $\chi \in \mathcal{G}$, is the choice of the push-pull function $\psi$. As previously introduced, a convenient choice for this is the logarithm and exponential maps [Lee, 2013] such that:

$$ \begin{aligned} \psi : & \quad \mathcal{G} \to \mathcal{g} & \log(\chi) & \to \xi \\ \psi^{-1} : & \quad \mathcal{g} \to \mathcal{G} & e^{\xi} & \to \chi \end{aligned} \qquad (2.52) \quad \text{revisited } \downarrow $$

where $\xi$ represents the analogous variable in the tangent space.

As shown in [Forster et al., 2015] for the specific case of the Lie group $SO(3)$ and in a more general sense in [Boothby, 1986] for a first-order approximation of the derivative in a differentiable manifold, it is possible to exploit the derivative of the exponential map to formulate a pushforward [Absil et al., 2008] integration scheme. Notably, in Equations 68-74 from [Solà et al., 2018], an infinitesimal variation in the manifold $d\xi$ can be approximated as:

$$ e^{\xi+d\xi} \approx e^{\xi} e^{J_R(\xi)} d\xi \qquad (2.53) $$

<footnote n="3">³In differential geometry, the pushforward refers to a linear approximation of smooth mappings that operates on tangent spaces.</footnote>
which, for small variations, is shown to be equivalent to these other approximations:

$$e^{\xi} e^{d\xi} \approx e^{\xi + J_R^{-1}(\xi)d\xi} \qquad (2.54)$$

$$\log(e^{\xi} e^{d\xi}) \approx \xi + J_R^{-1}(\xi)d\xi \qquad (2.55)$$

where $J_R$ maps variations in $\xi$ to the right multiplicative increments in the exponential map. This is in fact the exponential map derivative dexp and $J_R^{-1}$ corresponds to the logarithm map derivative. Furthermore this generalizes to a pushforward because the push function choice coincides with the exponential map.

Alternative notations to these derivatives $J_R, J_R^{-1}$ are respectively dexp, dlog. These are used indifferently throughout this manuscript with the implicit convention that they correspond to their right-trivialized convention.

This enables a first-order Taylor expansion of the integration scheme [Forster et al., 2015] analogous to (2.36) with a twist but in the tangent space:

$$\xi_{k+1} \approx \xi_k + \Delta t J_R^{-1}(\xi_k) \frac{d\xi_k}{dt} \qquad (2.56)$$

where $\frac{d\xi_k}{dt} = \mathcal{V}^\vee$ and the corresponding element $\mathcal{X}_{k+1}$ can be retrieved through the pull:

$$\mathcal{X}_{k+1} = \psi^{-1}(\xi_{k+1}) \qquad (2.57)$$

It can be seen that equation (2.56) has a linear relation with respect to the body twist. This is the foundation that enables linear Model Predictive Control (MPC) in the following chapters.

The following subsections show that there exists a way to approximate $J_R^{-1}$ to speed up computations with acceptable precision. The interest of this expression is revealed in the final section of this chapter, to perform an approximate linear pose propagation in a horizon (where the linearization is performed only at the start of a time window). This assumes that $J_R$ does not change much during the horizon, which is an acceptable compromise for any linearization approach and is also why the approximation gets worse with longer horizons.

### 2.4.3 Solving a Lie group differential equation via the Magnus expansion ↑

An alternative development path to arrive at (2.56) can follow a development from a Lie group differential equation, which is a recurrent apparition as shown in Sections 2.2.1.4 and 2.2.2.3. This is a more general approach but enables the use of the Magnus expansion solutions for its solution [Magnus, 1954]. This crucial series allows for an efficient approximation of dexp (or, equivalently, the derivative of the exponential map $J_R$ in (2.56)) through a truncation. As it is shown later, this yields some computational advantages.

Consider the following differential equation:

$$\dot{\mathcal{X}}(t) = \mathcal{X}(t)\mathcal{U}(t) \quad \text{with} \quad \mathcal{X}(t_0) = \mathcal{X}_0 \qquad (2.58)$$
with $\mathcal{X}(t) \in \mathcal{G}$ and $\mathbf{U}(t) \in \mathfrak{g}$ (as proposed in [Zanna, 1999]). It is possible to find a solution in the form:

$$ \mathcal{X}(t) = e^{\Omega(t)} \qquad \Omega(t_0) = \log(\mathcal{X}_0) \quad (\text{for } \mathcal{X}(t_0) = \mathcal{X}_0) \qquad (2.59) $$

where $\Omega(t)$ incorporates the cumulative effects of the infinitesimal element $\mathbf{U}(t)$ in time over the initial element $\mathcal{X}_0$. Note that $\mathcal{X}(t)$ and $\mathbf{U}(t)$ do not necessarily commute: this means that $\mathcal{X}_0\mathbf{U}_0\mathbf{U}_1 \neq \mathcal{X}_0\mathbf{U}_1\mathbf{U}_0$ (or for any other order for that matter). This is why $\Omega(t)$ cannot be found via a simple integration and why the Magnus expansion is required [Hairer et al., 2006].

A consequence of (2.59), is that $\dot{\mathcal{X}}(t) = e^{\Omega(t)}\mathrm{dexp}_{\Omega(t)}\Omega'(t)$. One can see the resemblance of this expression with the derivative of the exponential map<fnref n="4" /> at $x$:

$$ \frac{d}{dt} e^{x(t)} = \mathrm{dexp}_{x(t)} x'(t) \qquad (2.60) $$

From which, based on (2.58), one can identify $\mathbf{U}(t)$ as

$$ \mathbf{U}(t) = \mathrm{dexp}_{\Omega(t)} \Omega'(t) \qquad (2.61) $$

where $\mathrm{dexp}: \mathfrak{g} \times \mathfrak{g} \to \mathfrak{g}$. Then, one can retrieve $\Omega'(t)$ from (2.61) by inverting $\mathrm{dexp}$:

$$ \Omega' = \mathrm{dexp}_{\Omega}^{-1} \mathbf{U}(t) = \sum_{n=0}^{\infty} \frac{B_n}{n!} \mathbf{ad}_{\Omega}^{n} \mathbf{U}(t) \qquad (2.62) $$

where $B_n$ are the Bernoulli numbers and $\mathbf{ad}$ is the adjoint operator defined a series of Lie brackets (see (2.65) for the case of $\mathfrak{se}(3)$ and $\mathfrak{so}(3)$):

$$ \mathbf{ad}_{\Omega}^{n} = \underbrace{[\Omega(t), [\dots, [\Omega(t), \mathbf{U}(t)]]]}_{n \text{-times}} \qquad (2.63) $$

Actually, the main interest lies in finding $\Omega(t)$ that satisfies (2.58), which can be obtained through the repeated commutation and integration of $\mathbf{U}(t)$. The relation between (2.62) and (2.61) has been extensively studied in the literature, for instance in [Iserles et al., 2000]; [Iserles et al., 1999]; [Blanes et al., 2015]; [Schur, 1891]; [Rossmann, 2006]. Note that $\Omega(t) = \int_{t_0}^{t} \mathbf{U}(x)dx$ iff $\mathbf{U}$ is constant or commutes with its primitive (i.e. $[\mathbf{U}(t), \int \mathbf{U}(t)dt] = 0$). Otherwise, $\Omega(t)$ is an infinite series known as the Magnus expansion [Magnus, 1954]; [Blanes et al., 2008]:

$$ \Omega(t) = \int_{0}^{t} \mathbf{U}(t)dt + \frac{1}{2} \int_{0}^{t} \left[ \int_{0}^{t_1} \mathbf{U}(t_2)dt_2, \mathbf{U}(t_1) \right] + \dots \qquad (2.64) $$

<footnote n="4">⁴The derivative of the exponential map is often presented as $\frac{d}{dt}e^{x(t)} = e^{x(1-e^{\mathbf{ad}_x x})} \frac{dx}{dt}$ or, with a different notation but equivalently: $\frac{d}{dt}e^{x(t)} = \mathrm{dexp}_{x(t)} x'(t)$</footnote>
**REMARK. The Lie bracket and the adjoint operator**

Lie algebras such as $\mathfrak{g}$ are equipped with the commutation operator:

$$[X, Y] = XY - YX = \mathbf{ad}_{X}Y \qquad X, Y \in \mathfrak{g} \qquad (2.65)$$

This is also referred to as the *adjoint action* endomorphism, as it is a linear map onto itself such that $\mathbf{ad}_{\mathfrak{g}}: [\mathfrak{g}, \mathfrak{g}] \to \mathfrak{g}$. For the specific case of the algebras $\mathfrak{so}(3)$ and $\mathfrak{sc}(3)$, is defined as [Solà et al., 2018]:

for $\phi \in \mathfrak{so}(3)$:

$$\mathbf{ad}_{\phi_1 \phi_2} = [\phi_1, \phi_2] = \phi_1 \times \phi_2 = \phi_1 \wedge \phi_2 \qquad (2.66)$$

$$\text{with: } \mathbf{ad}_{\phi_1} = \phi_1 \wedge \in \mathbb{R}^{3 \times 3} \qquad (2.67)$$

for $\xi = [\rho^T, \phi^T]^T \in \mathfrak{sc}(3)$:

(with a linear component $\rho \in \mathbb{R}^3$ and angular component $\phi \in \mathfrak{so}(3)$)

$$\mathbf{ad}_{\xi_1 \xi_2} = [\xi_1, \xi_2] = \begin{bmatrix} \phi_1 \times \rho_2 + \rho_1 \times \phi_2 \\ \phi_1 \times \phi_2 \end{bmatrix} \qquad (2.68)$$

$$\text{with: } \mathbf{ad}_{\xi_1} = \begin{bmatrix} \phi_1 \wedge \rho_1 \wedge \phi_2 \\ 0 \wedge \phi_1 \wedge \end{bmatrix} \in \mathbb{R}^{6 \times 6} \qquad (2.69)$$

Note that $\mathbf{ad}_{\phi_1 \phi_2} \in \mathfrak{so}(3)$ and $\mathbf{ad}_{\xi_1 \xi_2} \in \mathfrak{sc}(3)$.

### 2.4.3.1 Truncating the Magnus expansion to approximate the logarithm map derivative ↑

The relevance to this chapter comes from a truncated version of (2.62) up to $n = 2$, referred to as ME2 from now on (remembering that $B_n = 0$ for all odd $n > 1$, which implies that ME3 $\equiv$ ME2). This allows approximating (2.62) in a linear form with respect to $U(t)$:

$$\Omega'(t) \approx U(t) + \frac{[\Omega(t), U(t)]}{2} + \frac{[\Omega(t), [\Omega(t), U(t)]]}{12} \qquad (2.70)$$

$$\Omega'(t) \approx U(t) + \frac{\mathbf{ad}_{\Omega(t_k)} U(t)}{2} + \frac{\mathbf{ad}_{\Omega(t_k)}^2 U(t)}{12} \qquad (2.71)$$

$$\Omega'(t) \approx KU(t) \qquad (2.72)$$

$$\text{with: } K(t) = I + \frac{\mathbf{ad}_{\Omega(t_k)}}{2} + \frac{\mathbf{ad}_{\Omega(t_k)}^2}{12} \qquad (2.73)$$

Notice that $K$ allows approximating $\exp x^{-1}$ by truncating the series. In fact, $K \approx J_R^{-1}$ from (2.56). This expression is crucial to arrive at a linear algebraic expression of the pose in the logarithm map in the following section and constitutes the main contribution of this chapter.
### 2.4.3.2 An approximative pose propagation in the logarithm map ↑

Equation (2.58) was formulated for a very general case of Lie groups, but it can be applied to the specific one of poses and orientations if one notices the resemblance with the pose and rotation matrix derivatives<fnref n="5" />:

$$ \mathcal{X}^{-1} \dot{\mathcal{X}} = \mathbf{v}^{\wedge} \Rightarrow \dot{\mathcal{X}} = \mathcal{X} \mathbf{v}^{\wedge} \qquad \mathcal{X} \in \mathbb{SE}(3) \qquad \mathbf{v} \in \mathfrak{se}(3) \qquad (2.74) $$

$$ \mathbf{R}^{-1} \dot{\mathbf{R}} = \mathbf{\omega}^{\wedge} \Rightarrow \dot{\mathbf{R}} = \mathbf{R} \mathbf{\omega}^{\wedge} \qquad \mathbf{R} \in \mathbb{SO}(3) \qquad \mathbf{\omega} \in \mathfrak{so}(3) \qquad (2.75) $$

which, under the assumption that $\mathcal{U}(t)$ remains constant throughout an integration step (meaning $\mathcal{U}(t) = \mathcal{U}$, that is necessary for (2.36)), means that $\mathcal{U}$ is in fact the twist or the angular velocity applied, for each respective case.

Furthermore, noting that:

$$ \log(\mathcal{X}(t)) = \log(e^{\Omega(t)}) = \Omega(t) \qquad (2.76) $$

then it becomes evident that $\Omega(t)$ is in fact the desired logarithm of the pose $\xi(t)$. Using a first-order Taylor expansion of the discretized $\Omega(t)$:

$$ \Omega(t_{k+1}) \approx \Omega(t_k) + \Delta t \Omega'(t_k) \qquad (2.77) $$

where $\Omega'(t_k)$ can be replaced with (2.73) for $\mathcal{U} = \mathbf{v}_k$ and $\Omega(t_k) = \xi_k$:

$$ \xi_{k+1} \approx \xi_k + \Delta t \mathbf{K}(t_k) \mathbf{v}_k \qquad (2.78) $$

In this expression, $\mathbf{K}$ only depends on $\Omega(t_k) = \xi_k$. Finally, this shows, in a practical way, how to approximate (2.36) in the form of (2.56).

The final section of this chapter performed some numerical simulations to evaluate the performance of the approximation presented here.

## 2.5 Linear Pose Estimation in a Horizon ↑

The application of this manuscript is within the context of collaborative robotics. This application requires efficient control of the robot end-effector while carefully considering the movement trajectory. This setting implies the robot operates in a continuously changing environment. In this context, the robot needs to constantly take into account these events and adjust its behavior accordingly.

To do so, optimization-based schemes could be proposed that consider multiple aspects of the application as long as there exists a way to estimate future states of the robot and predict the effect of these movements. Previous sections introduced a way to describe the pose of the end-effector in space and devised a strategy to integrate its movement based on the body twist. Additionally, an approximative way to perform this estimation was proposed (see Section 2.4.3.1). The objective of this section is to showcase the precision and computational advantage of this approximation.

<footnote n="5">⁵Expressions for these can be found in Lynch and Park's "Modern Robotics: Mechanics, Planning, and Control" (2017) [Lynch et al., 2017]. Page 112, Section 3.5 of Chapter 3 offers a table with the rotation and rigid body motion analogies. Section 3.2.3.1 delves into some linear differential equations essentials that are applied in Eq. 3.50 (Page 83), which resembles (2.58).</footnote>
A numerical experiment experiment is proposed to create the necessary conditions of an acceptable scenario for an estimation algorithm that could potentially be used for optimization methods. This experiment re-creates the hypothetical conditions of a horizon estimation: the only perfectly known information is the measured pose of the robot at the current point in time. To test the precision of estimating the future poses, a discretized and perfectly known reference pose path is necessary, from which the body twist at each descretized step can be computed. Then, the estimated poses propagated with the initial pose and the hypothetical perfectly known twists can be compared to the reference pose path to evaluate the performance of the methodology.

### 2.5.1 Problem statement: pose propagation in linear form ↑

To begin, it is necessary to describe the end-effector pose as a state-space system to clarify the involved variables and why a linear algebraic form is fundamental.

One can formulate a first order integration scheme of a system as:

$$x_{k+1} = A_k x_k + B_k u_k \qquad (2.79)$$

where $x$ and $u$ designate respectively the state and input (decision) variables of the system. Meanwhile, $A_k, B_k$ are state transition matrices that linearly propagate the state of the system.

In the interest of controlling the robot end-effector, the state $x$ describes its pose in the tangent space (see Sections 2.4.1 and 2.4.2) and the decision variable is its body twist. It can be seen that (2.56) resembles (2.79) and can be further approximated with (2.78) through the ME2 truncation.

Another implication of (2.56) is that it needs a linearization scheme at each time step $k$ for the matrices $A, B$. Nevertheless, the pose propagation in a sliding time window implies that the future states of the robot (those that are estimated throughout the horizon) are unknown. Thus, the linearization is only performed at the beginning of the horizon, on the only known state (the current measured pose). In such case, the assumption is that $A_k, B_k$ can be approximated as $A = A_0, B = B_0$ and then (2.79) is extended for a horizon:

$$x_{k+1} = A x_0 + B(u_0 + u_1 + \dots + u_k) \qquad (2.80)$$

This expression constitutes the foundation necessary for a linear estimation of a system state in some optimization methods like linear Model Predictive Control (MPC). The next section shows how it relates to pose estimations in the tangent space.

### 2.5.2 Discrete pose estimation in a horizon ↑

Assuming the pose $\mathcal{X}_k = e^{\xi_k t}$ is measured at the start of any horizon, one can assume that $K$ in (2.78) does not change significantly through a time window. This enables reconstructing the pose in a horizon as a function of the twist, through an approximation of (2.56) in the same form as (2.80), where the state $x$ and the inputs $u$ are
<figure><img src="image_16.png" /><figcaption>Figure 2.8: Depiction of the path taken by the end-effector of a Franka Emika robot from an initial to a target pose $\mathcal{X}_i, \mathcal{X}_t \in SE(3)$.</figcaption></figure>

replaced with their chosen quantities $\xi, \nu \in se(3)$, respectively:

$$ \mathcal{X}_{k+1} = e^{\xi_{k+1}} \qquad (2.81) $$

$$ \text{with: } \xi_{k+1} \approx \xi_0 + \Delta t K(t_0)(\nu_0 + \nu_1 + \dots + \nu_k) $$

where $\xi_0 = \log(\mathcal{X}_0)$ corresponds to the pose at the start of the horizon (at time $t_0$) and $K_0$ denotes ME2 evaluated at the same time point (see Section 2.4.3.1). Note that this expression effectively serves to approximate (2.47) in the tangent space.

### 2.5.3 Simulation Experiment ↑

The goal of this experience is to emulate the conditions of estimating the pose of the robot subject to some twist throughout a horizon. More specifically, the objective is to find out how precise are the results obtained from (2.81).

For this experiment, a time-optimal trajectory between two poses is discretized to obtain the ground truth poses and twists at each time step. Then, the same pose trajectory is reconstructed by assuming a known starting pose at the beginning of the horizon and applying the perfectly known twists throughout the time window. The result is then compared to the ground truth pose trajectory.

### 2.5.4 Simulation results ↑

Let the initial and target poses of a robot end-effector path be $\mathcal{X}_i, \mathcal{X}_t \in SE(3)$ as depicted in Figure 2.8. For this experiment, in order to represent a typical movement of a Franka Emika robot in its workspace, two paths with different positional and rotational displacement were generated:

* For trajectory 1: $||p_f - p_0|| = 0.71\text{m}$ and $||\text{ang}(\delta)|| = 0.94\text{rad}$.

* For trajectory 2: $||p_f - p_0|| = 0.32\text{m}$ and $||\text{ang}(\delta)|| = 0.90\text{rad}$.

The rotational displacement (the norm of the angular component $\text{ang}(\delta)$) is computed from $\delta = \log(\mathcal{X}_0^{-1}\mathcal{X}_f)$.
<figure><img src="image_17.png" /><figcaption>Figure 2.9: Normalized $s(t)$ for a time-optimal trajectory generated with a trapezoidal profile (in velocity).</figcaption></figure>

Defining $s \in [0, 1]$ to discretize the path between both poses as: $\mathcal{X}_k^* = \mathcal{X}_0 e^{s_k \delta}$ yields a ground truth reference velocity twist $v_k^* = \frac{\log(\mathcal{X}_k^{*-1} \mathcal{X}_{k+1}^*)}{\Delta t}$.

This implies that the reference trajectory can be reconstructed with:

$$ \mathcal{X}_{k+1} = \mathcal{X}_k e^{\Delta t v_k^*} \qquad (2.82) $$

$$ \text{where: } \log(\mathcal{X}_k) = \zeta_k \qquad (2.83) $$

The Ruckig library [Berscheid et al., 2021a] is used<fnref n="6" /> to compute a normalized $s(t)$ in time to find the time-optimal trajectory, following a trapezoidal trajectory (in velocity), as shown in Figure 2.9.

This showcases the precision of the linear integration of a robot end-effector pose by using (2.81) throughout an H-step horizon and comparing it against (2.82), the ground truth. Figure 2.10 depicts the obtained errors with respect to the perfect propagation.

Assuming a receding horizon approach where only horizon estimation matters, the pose is reset back to ground truth at the end of each horizon (every 300ms) to showcase the maximum error obtained with the method and to exemplify how the precision worsens towards the end of the horizon.

Table 2.1 shows the computation time and error averages obtained for the ME2 approximation. The method achieves a linear and angular error of $1.09 \pm 6.1mm$ and $0.048 \pm 0.27^{\circ}$ (for the longest trajectory), respectively. This is done in about two thirds of the time it takes to do it with the exponential (exact but non linear).

To further illustrate the effects of enlarging the horizon duration, we show in Figure 2.11 the same experiment run multiple times for different values of H (number of time steps). It can be seen that the longer the horizon, the greater the error is. Thus, a compromise must be made in order to define a horizon that allows to predict further enough in time without compromising precision.

<footnote n="6">⁶The code used for experiments in this chapter is publicly available at: https://gitlab.inria.fr/auctus-team/publications/shared-paper-code/ark2022-lin</footnote>
<figure><img src="image_18.png" /><figcaption>Figure 2.10: Variable speed integration using ME2 for a 300ms horizon.</figcaption></figure>

<table><caption>Table 2.1: Shows the average compute time and error for the horizons shown in Figure 2.10.</caption><thead><tr><th rowspan="2">Trajectory</th><th colspan="3">Compute Time</th><th colspan="3">Linear Error</th><th colspan="3">Angular Error</th></tr><tr><th>Avg</th><th>Rel</th><th>Max</th><th>Avg</th><th>Max</th><th>Std</th><th>Avg</th><th>Max</th><th>Std</th></tr></thead><tbody><tr><td>ref-1</td><td>0.23µs</td><td>100.0%</td><td>1.57µs</td><td></td><td></td><td></td><td></td><td></td><td></td></tr><tr><td>traj-1</td><td>0.15µs</td><td>62.4%</td><td>3.74µs</td><td>1.09mm</td><td>6.15mm</td><td>±1.47mm</td><td>0.05°</td><td>0.27°</td><td>±0.06°</td></tr><tr><td>ref-2</td><td>0.18µs</td><td>100.0%</td><td>0.82µs</td><td></td><td></td><td></td><td></td><td></td><td></td></tr><tr><td>traj-2</td><td>0.13µs</td><td>72.3%</td><td>0.23µs</td><td>2.26mm</td><td>12.74mm</td><td>±2.99mm</td><td>0.10°</td><td>0.57°</td><td>±0.13°</td></tr></tbody></table>

To showcase the trade off between precision and computation time, this comparison shows the error between the ground truth reference (computed with (2.82)) and the proposed ME-based method (using (2.81)) for each discretized step (as well as the maximum and standard deviations). The results are showed for 2 generated trajectories with their respective references. Compute time refers to the average for each Δt step on a C++ implementation. Relative computation time is calculated with respect to each reference average computation time. The tests were executed on a laptop with a CPU Intel(R) Core(TM) i7-10610U.

<figure><img src="image_19.png" /><figcaption>Figure 2.11: Average error for different horizon lengths.</figcaption></figure>
## 2.6 Conclusions ↑

ME2 offers not only a precision level practical for many purposes but it could also offer a speed improvement when both methods of integration are possible (the exponential integration method cannot be embedded in a *Quadratic Program (QP)*, which is the main goal of this work as it is required for linear *Model Predictive Control (MPC)*). This computational time improvement could also be exploited on robots with limited resources.

Lie algebras offer a convenient framework for pose description and system linearization on manifolds, allowing for vector expressions that can be embedded in a linear *MPC* problem, unlike their matricial forms.

A prospective interest in the future is the estimation of a bound on the maximum error induced by the proposed linear approximation.

Future chapters revisit the techniques presented here for the dynamic re-planning of end-effector *pose* trajectories subject to kino-dynamic constraints via a linear *MPC* implementation.
Chapter 3

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

Efficient workspace sharing of collaborative robots and human opera-
tors entails the need for robots to smoothly adapt tasks transitions while
respecting changing workspace constraints. This remains an unsolved
problem in the industry and goes beyond the use of a priori or a posteri-
ori safety measures: it must be tackled at the control level.

To address the need of adaptation to human presence as well as to en-
dow the robot with the ability to adapt interactively to Cartesian targets
moving in 3D space, a linear Model Predictive Controller is proposed
in this Chapter. This controller computes acceleration-bounded optimal
trajectories in SE(3) over a receding horizon, enabling position and ori-
entation online re-planning.

The pertinence of the proposed control architecture is demonstrated
using experiments with the Franka Emika robot in a scenario implying
both adaptation of the maximum allowed velocity to comply with
human presence and on-the-fly updates of a Cartesian goal pose.
# Contents

<table><tr><td>3.1</td><td>Introduction</td><td>46</td></tr><tr><td>3.1.1</td><td>Rationale: Constrained Online Motion Generation</td><td>46</td></tr><tr><td>3.1.2</td><td>Control Architecture Proposal</td><td>48</td></tr><tr><td>3.2</td><td>Task-space Tracking</td><td>50</td></tr><tr><td>3.2.1</td><td>Kinematic Robot Model</td><td>52</td></tr><tr><td>3.2.2</td><td>Pose Tracking as a Quadratic Program</td><td>54</td></tr><tr><td>3.2.3</td><td>Constraints Formulation</td><td>55</td></tr><tr><td>3.2.4</td><td>On the Solver Reference Frame</td><td>56</td></tr><tr><td>3.3</td><td>Constrained Motion Generation with Model Predictive Control</td><td>58</td></tr><tr><td>3.3.1</td><td>Problem Statement: Motion Generation via MPC</td><td>59</td></tr><tr><td>3.3.2</td><td>Linear Model Predictive Control</td><td>60</td></tr><tr><td>3.3.3</td><td>Optimization on the Pose Manifold</td><td>62</td></tr><tr><td>3.3.4</td><td>Geodesic Path in the Tangent Space</td><td>64</td></tr><tr><td>3.3.5</td><td>Task-Space Linear MPC in the Tangent Space</td><td>64</td></tr><tr><td>3.4</td><td>Experimental Validation: The Case of Velocity Modulation</td><td>66</td></tr><tr><td>3.4.1</td><td>Implementing Online Re-planning</td><td>67</td></tr><tr><td>3.4.2</td><td>Numerical Simulation</td><td>68</td></tr><tr><td>3.4.3</td><td>Experimental Scenario & Safety Constraints</td><td>69</td></tr><tr><td>3.4.4</td><td>Results</td><td>72</td></tr><tr><td>3.5</td><td>Conclusion</td><td>72</td></tr></table>
## 3.1 Introduction †

The arrival of the so-called “collaborative robotics” has brought forth an on-going paradigm shift from traditional, static robot work-spaces to dynamic environments of shared collaboration with humans. This vision projects efficient factories that seamlessly transition between robotic cells to fully interactive human-robot stations that exploit each others qualities in synergy.

Throughout this chapter, a control architecture for interactive collaborative robotics is introduced. It is based on a cascade controller composed of a task-space planner and a model-based solver. The rationale behind this architecture is presented in Section 3.1.1.

Overall the first sections present some groundwork concepts and notation conventions employed in this architecture proposal. To begin, Section 3.2 focuses on the inner stage, a constrained model and optimization-based controller. To do so, it first offers an overview of some robotics basics and then follows with some state-of-the-art applications of these modeling fundamentals applied to optimization-based control.

Afterwards, Section 3.3 delves into the task-space planner for online motion generation based on Model Predictive Control (MPC). First, some foundations for linear MPC are presented. Subsequently, it revisits the concepts introduced in Chapter 2 to formulate an optimization scheme on the pose manifold, allowing the generation of constrained motions in position and orientation. This constitutes the main contribution of this chapter.

Afterwards, Section 3.4 provides an experimental validation of the proposed architecture for a safety-aware collaborative scenario. It begins with the description of the experimental setup and objectives. Then, it goes on to offer some preliminary validation results of online re-planning on a simulated environment and finally, an experiment is carried out on a 7 Degree of Freedom (DoF) manipulator to validate the pertinence of the approach.

Finally, Section 3.5 concludes the chapter by summarizing the contributions.

An accompanying video (see Figure 3.10) offers a compact summary of the results.

### 3.1.1 Rationale: Constrained Online Motion Generation †

Collaborative robotics has garnered significant attention in the literature, with numerous studies exploring different aspects of the nature of Human-Robot Interaction (HRI) in work-cells. Some of the main concerns relate to safety and ergonomics [Haddadin et al., 2016], which are often analyzed from the application point of view to reduce complexity [Gualtieri et al., 2021]. Yet, a systematic approach that addresses all or most requirements of HRI from a control perspective in a generic way remains to be proposed.

To achieve this, a paradigm shift in control strategies is necessary. Controllers must address the needs of factories in a way that seamlessly integrates collaborative robotic cells without fences. In such scenario, robots need to evolve in environments that are not only continuously changing but can also do so in unpredictable ways.
To successfully achieve a responsive robotic behavior that adapts to these changes, ensures safety for nearby humans, and performs tasks effectively, it is crucial for the robot to smoothly transition between different movements.

These transitions require the motion to be generated in an online fashion to be able to consider the current conditions in the environment and the state of collaborative tasks. This entails a two-fold problem:

1. Firstly, the robot requires the ability to fluidly switch between task objectives.

2. Secondly, the robot needs to perform movements that will remain coherently feasible during operation.

The first problem has been addressed in [M. Liu et al., 2015]; [Salini et al., 2011] in a reactive manner, i.e. taking a decision that considers only the next state of the robot. However, ignoring the future states of the planned motion can lead to risky situations. This is why these problems normally require a motion generation that considers the task from start to finish [Kurniawati et al., 2011]. Nevertheless, such strategy is inefficient due to two reasons: for one, it might get computationally non-viable for the real-time requirements of the collaborative scenarios; and secondly, the dynamic nature of the scenario implies that the task will need to be re-planned in order to adapt to new changes before it is ever completed.

Meanwhile, the second problem may seem simpler at first glance but due to the limited intrinsic actuation capabilities of the robot, some instantaneously viable states can lead to inevitable feasibility problems. In other words, there might exist a problem of “coherency” between actions / decisions taken in the present if they do not consider how their outcome might lead the robot into invalid states in the future. Because these actuation capacities are governed by non-linear dynamics, some simplifying strategies have been proposed to preemptively avoid entering these states [Rubrecht et al., 2010]; [Prete, 2018] and even consider their effects in the workspace motions [Rubrecht et al., 2012]. Some of these strategies resort to linking workspace and joint constraints with “interfacing” quantities like the minimal motion stopping time [Joseph et al., 2020]. Nevertheless, the consideration of the workspace constraints on their own and in combination with the above problems remains an open question. While this chapter main proposal focuses mainly on a task-planner that considers the compatibility of the work-space related constraints, it still accounts for the robot capabilities through the model-based embedded controller.

In order to start providing solutions to these issues, this chapter proposes a predictive control scheme that allows considering the future states of the robot in an effort to smoothly transition between tasks while considering the long term effects of these actions (long term as in robot task execution times).

An ideal solution would be a global planning strategy able to output complete trajectories (from start to finish). However, these approaches are usually computationally expensive and quickly become not viable when the scenario requires updating the whole trajectory for every change in the environment (without really taking advantage of having computed the complete trajectory).

The proposal is to avoid the pitfalls of global planning in dynamic scenarios by
<figure><img src="image_20.png" /><figcaption>Traditional Control Architecture</figcaption></figure>

<figure><img src="image_21.png" /><figcaption>Figure 3.1: Both control architectures employ a task-planning block and a controller block. The first one is in charge of generating motions that will be executed by the latter. The planning needs to account for the "Workspace Configuration" (the state and constraints of the shared workspace). In traditional industrial robotics, the operator intervenes during task design through a slow iterative process. Once the task is defined, it only needs to be executed on repetition. In contrast, collaborative robotics requires to have the human in the loop and be considered in the task planning. This implies that online re-planning is necessary, as is proposed in the architecture at the bottom.</figcaption></figure>

revisited ↓

adopting a strategy that performs optimizations on a receding horizon, effectively providing computationally efficient solutions that often yield approximately global optimal strategies.

### 3.1.2 Control Architecture Proposal ↑

The traditional control architecture<fnref n="1" /> shown in Figure 3.1 roughly depicts a classical approach to industrial robot cells. In this approach, the operator only intervenes during the task design process, by trying to find an appropriate trajectory for some specified precision and cycle times requirements through a slow iterative process. For each iteration, the target and/or workspace constraints will be changed and executed on the robot to assess the result (or possibly simulated). Once a task trajectory is accepted, it only needs to be deployed to be executed on repetition.

In contrast, the collaborative control architecture presented in Figure 3.1 proposes a new perspective to the human in the control loop: the task now involves the HRI and so the human becomes a part of the dynamics that need to be accounted for during planning. It is one potential solution that accounts for the requirements stated in Chapter 1 and is composed of three main blocks:

1. Task-space Motion Generation

2. Model-based Tracking Controller

<footnote n="1">¹In this case, control architecture is used in a vague way to refer to both the task design process and control during execution.</footnote>
### 3. Workspace Analyzer

The first block consists of a task planner. From a human and task-centered point of view, task planning at the Cartesian level offers some advantages:

* It simplifies the robot programmer's work, as it corresponds to the natural space where the task will be performed.

* All the task-related information regarding objectives, limits, etc is expressed in the same space (as opposed to having to mix joint-space and Cartesian constraints).

* And finally, it decouples task description from the robot architecture, embedding some desirable modularity properties potentially either reusing designs or replacing parts.

The Cartesian trajectory planner block consists of an algorithm to generate a motion primitive that responds to the tasks needs while respecting the workspace configuration (constraints, sensor information, etc) and task objectives. For example, a planer could define a task that exploits the ergonomics of the HRI [Spaa et al., 2020] to reduce operator musculoskeletal stress. Furthermore, this block is in charge not only of dealing with task-related information (like its objectives) but it is also in charge of seamlessly articulating the task objectives with the workspace configuration (the state and constraints of the shared workspace). Another common example scenario, for tasks without physical interaction, is the essential requisite for safety: being able to reactively create paths that avoid collissions [Du et al., 2018]; [Mohammed et al., 2017]; [Safeea et al., 2019] or minimize the hazard. Due to the feedback loop between the task state and the environment information, this effectively implements trajectory planning in an online fashion.

At the second level, the model-based tracking controller block is in charge of executing the planned trajectory, i.e. translating the Cartesian movement into a joint-space motion to comply with the task needs. As this involves translating the motion into a control input, the constraints involved in this block mainly concern the actuation model of the robot in order to produce feasible servoing. While the mathematical formulation of this block might be generic and applicable to a multitude of robots, their implementation is actually robot-dependent, as it embeds the physical properties.

Finally, at the heart of this architecture, lies the workspace analyzer block. Though accurately sensing the state of the environment around the robot is challenging in itself [Halme et al., 2018], this approach to a reactive control architecture also requires sensing the workspace and actually translating this information in a way that can be exploited by the model-based controller and trajectory re-planner blocks in a coherent way (referred as workspace configuration in this manuscript). This is implementation-dependent and aims at extracting and synthesizing the environment information. For instance, in the case of some motion generating algorithms, this can mean using specific mathematical formulations for describing obstacles [Krämer et al., 2020]; [Spahn et al., 2023] or predicting human motions in the workspace [Bajcsy et al., 2021]; [Weitschat et al., 2018].

Added to this problem is the fact that environmental conditions are susceptible to
sensor noise and sudden changes, though to circumvent this problem, some spe-
cial considerations have been proposed that avoid discontinuous changes that could
result in instantly incompatible problems [Tan et al., 2015].

Note that the architecture is presented in a generic way to showcase its modularity as a framework for collaborative scenarios. Indeed, its blocks can be replaced depending on the application but the genericity of task-related planning with constraints can be applied to other safety-aware control architecture contributions [Benzi et al., 2021]; [Lihui Wang et al., 2013]; [Scalera et al., 2022].

For the collaborative scenarios considered in this manuscript, the proposal is to use a
linear MPC for task-space planning that can be formulated as a quadratic cost func-
tion subject to linear constraints. While similar architecture approaches have been
employed in humanoid control [Wieber, 2006]; [Lober et al., 2020], the application to
collaborative robotics remains relatively new. In this context, the added value comes
from offering a mechanism to formulate the workspace configuration through a con-
vex polyhedral (linear constraints) and effectively allows the optimization problem
to yield constrained trajectories over a sliding time window.

The proposed architecture aims at addressing the needs of agile manufacturing for robotics presented in Chapter 1, which is the reason behind some of the choices. For instance, by decoupling the Cartesian-space planning from the joint-space controller, the architecture effectively decouples task-related aspects (cost function, constraints, etc) from the actuation-related concerns (actuator limits, type of control input, robot model). The rationale behind this choice is to offer some flexibility if ever the work-cell needs to be reassigned (thus changing the task) or adapted to different robots (swapping the robot).

The following sections offer delve into the implementation of this architecture. To begin, an optimization-based approach for task-space tracking is proposed to implement the planning block. Afterwards, the motion generation (the planning block) is presented as the main contribution of this manuscript: a linear MPC implementation capable of constrained full pose planning (i.e. position and orientation). Finally, to offer some insights into the work-space analyzer, the final section introduces a practical experiment to showcase a safety-aware implementation.

## 3.2 Task-space Tracking ↑

Trajectory tracking refers to the ability of a robot to follow a predefined trajectory accurately. This trajectory is typically specified as a series of desired positions or configurations that the robot needs to reach over time. The robot objective is to adjust its control inputs (i.e. motor commands) in real-time to move along the trajectory as closely as possible.

Robots are controlled through actuators that define its control input space. Often times, there is one actuator per joint and so this space directly relates to the robot configuration space, which defines the state of each joint (commonly the joint angles and/or velocities). Consequently, a task defined as a configuration-space trajectory is often simpler to execute because the translation to the robot control input space is often also rather direct.
In a more general sense though, for a robot controlled through some decision variable<fnref n="2" /> $x \in \mathbb{R}^n$, a tracking task can be described as an optimization problem with a cost function $f(x) : \mathbb{R}^n \to \mathbb{R}$ such that [Joseph, 2018]:

$$ 
\begin{array}{l}
x^{\text{opt}} = \arg \min_{x} f(x) \\
\text{subject to} \quad h(x) \le 0
\end{array}
\qquad (3.1) $$

where $x^{\text{opt}}$ designates the optimal control value to perform said task $f(x)$ while subject to some constraints $h(x)$.

In this way, $f(x)$ relates the desired trajectory with the robot model. Here, trajectory is used in a generic way to designate a pre-defined series of interest points with an associated time. In this way, $x^{\text{opt}}$ actually denotes the optimal control input in order to achieve the next desired interest point $f(x)$.

As mentioned earlier, if the trajectory is defined in the actuation space as $x(t) \in \mathbb{R}^n$, then the unconstrained optimal control input $x^{\text{opt}}$ can be directly computed. However, formulating the task as an optimization like in (3.1) can still be convenient due to the ability to explicitly and generically relate the control input to constraints in $h(x)$ (for example, for the actuation limits). This results in a more generic way to consider the feasibility of the task, effectively allowing a “best effort” strategy, i.e. try to perform the task as best as possible.

Furthermore, one can define a position trajectory $p(t) \in \mathbb{R}^3$ that takes the form of a 3-vector in time; or alternatively, if a trajectory is defined for position and orientation simultaneously, then it can take the form of a homogeneous transformation matrix in time $\mathcal{X}(t) \in SE(3)$ (as in Section 2.3). The focus of this manuscript is on tracking pose trajectories, i.e. the tracking of a given position and orientation in time.

Actually, determining the appropriate input for a given task-space trajectory can be addressed differently depending on the type of control variable utilized by the robot. This defines two broad sets of problems: inverse kinematics problems, when the input variable of the robot is either the position or the joint velocities (addressed in this section); and inverse dynamics problems, when the control variable is of higher order and so requires exploiting the robot dynamics model (see Chapter 4), for example, joint accelerations, torques, etc.

Roughly speaking, this section focuses on formulating problem (3.1) as a constrained least squares problem similar to the control strategy proposed in [Salini et al., 2011] for humanoids:

$$ x^{\text{opt}} = \arg \min_{x} ||Ex - b||^2 \qquad (3.2) $$

$$ \text{subject to} \quad A_u \le Ax \le A_u \qquad (3.3) $$

where (3.2) denotes the task space trajectory tracking problem, that can be efficiently

<footnote n="2">²Here $x$ offers a generic way to define the control variable. Industrial robots often use joint angles, velocities and, in some rare cases, torques as an input.</footnote>
solved as a Quadratic Program (QP)<fnref n="3" /> (see the details on this reformulation on Appendix A). Additionally, (3.3) serves as a convenient way to formulate linear constraints with respect to the decision variable. The focus of the following sections is, using this form and some robot modeling foundations, to introduce a quadratic optimization-based controller that considers the actuation limits, for robots servoed at the kinematic level.

To skip the rationale and modeling aspects, a summary of the resulting controller is presented in Appendix C.

### 3.2.1 Kinematic Robot Model ↑

The tasks to be performed by a robot are naturally occurring in the 3D space, i.e. position and orientations. This is why trajectories are often defined in the same Cartesian or operational space as the task they parameterize. For example, a pick and place task requires the robot end-effector to move from some pose A towards some other pose B. Depending on the robot mechanical design, this same path from A to B may correspond to multiple configuration-space paths. Nevertheless, the robot still needs a correct joint-space control input to perform the task. This implies that some mathematical resolution is needed to articulate the transformation from one space to the other.

While determining the pose of a robot body given some configuration can be done in a rather direct fashion in the case of serial robots, the inverse problem requires more thought. The first mathematical problem is concerned with calculating the position and/or orientation of a robot body in the task-space for a given robot configuration (its joint angles) and is known as Forward Kinematics (FK)<fnref n="4" />. The inverse problem of finding the robot configuration that achieves a certain position and/or orientation is known as Inverse Kinematics (IK).

For a given configuration of an n-Degree of Freedom (DoF) manipulator represented as a vector $q \in \mathbb{R}^n$ containing the joint angles $q_i$ for $i = 1...n$, the pose of a robot body (commonly the end-effector) in Cartesian space can be represented as a rotational displacement from the world frame. This pose is described by a homogeneous transformation matrix $\mathcal{X}_{WB}$, a function of the robot configuration that combines both the translation and rotation information necessary to go from the world to the body frame:

$$ \mathcal{X}_{WB}(q) = \begin{bmatrix} R_{WB}(q) & p(q) \\ \mathbf{0}_{1 \times 3} & 1 \end{bmatrix} \qquad \mathcal{X} \in \mathbb{SE}(3) \qquad (3.4) \quad \text{revisited } \uparrow $$

This allows a more formal definition of FK and IK as:

$$ \begin{gathered} \mathrm{FK}(q) : \mathbb{R}^n \to \mathbb{SE}(3) \qquad \mathrm{IK}(\mathcal{X}) : \mathbb{SE}(3) \to \mathbb{R}^n \\ \mathrm{FK}(q) = \mathcal{X}_{WB} \qquad \mathrm{IK}(\mathcal{X}) = \{q \in \mathbb{R}^n, \mathcal{X} \in \mathbb{SE}(3) \mid \mathrm{FK}(q) = \mathcal{X}\} \end{gathered} \qquad (3.5) $$

<footnote n="3">³Quadratic programming (see Appendix A) is a mathematical optimization technique used to solve a wide range of problems in various fields. It deals with the optimization of a quadratic objective function subject to linear constraints.</footnote>

<footnote n="4">⁴Forward Kinematics refers to the geometric analysis of a robot body in space, conferring it a rather static view. Nevertheless, kinematics is also used to refer to the study of motions without considering the forces that cause them. Interestingly, in french, *cinématique* is mostly associated to the latter sense while *géométrique* is preferred when the differential aspect is not considered.</footnote>
While, for serial robots, the solution of FK is unique, the same cannot be said of IK. For redundant robots where the DoF of the robot is higher than the DoF of interest, IK has an infinite set of solutions. For example, such is the case of 7 joint serial manipulators used for pose tracking (6 DoF, to account for the position and orientation). Yet, in a more realistic scenario, the joint angles $q$ are constrained to the set of feasible configurations limited by the joint bounds.

Moreover, considering real robots in operation implies that only the configurations near the current one are actually realizable, which also has some interesting implications that can be analyzed by considering a differential analysis of the FK.

The Differential Kinematics or Forward Instantaneous Kinematics [Siciliano et al., 2008] problem deals with the problem of computing the infinitesimal rigid body movement associated with an infinitesimal increment in the robot configuration.

For an n-DoF robot at any point in time, the Jacobian of a robot is defined as a matrix $J$ that describes the relation between the Cartesian space end-effector pose rate of change and the rate of change of the joints $\dot{q} \in \mathbb{R}^n$ as [Lynch et al., 2017]:

$$ W \nu = W J(q) \dot{q} \quad J \in \mathbb{R}^{6 \times n} \quad q, \dot{q} \in \mathbb{R}^n \quad \nu \in \mathfrak{SE}(3) \quad (3.6) \quad \text{revisited } \downarrow $$

where $\nu$ corresponds to the rate of movement of the body, a spatial twist in the world frame. The frame in which the Jacobian is expressed will ultimately determine the resulting frame for the twist $\nu$. If the Jacobian is in a body frame, then the twist is referred to as a *body twist*.

A remarkable implication of (3.6) is that even if IK in (3.5) admits multiple solutions, given a robot in operation, its motion (i.e. the realized twists) might still be unique. For example, for a 6-DoF robot executing a task, given its current initial pose, the joint-space motion required to go to some other pose is still unique.

Nonetheless, in the more general case of robots with more than 6 DoF, not only does the IK admit infinite joint configurations, but it is also kinematically redundant in the motion sense, admitting infinitely many joint velocities that can achieve the same spatial twist.

Some final considerations for the robot model used in this chapter are the robot constraints. For the kinematic model of this section, the robot is considered up to the joint velocities level, implying that FK, IK and all the spatial twists $\nu$ are defined for $q \in Q$ and $\dot{q} \in \dot{Q}$ such that:

$$ \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 (3.7) \quad \text{revisited } \downarrow $$

which correspond to sets defined by the lower and upper limits for the joint configurations and velocities.

The following sections articulate the foundations presented here to formulate the control problem introduced in (3.1) while taking into consideration (3.7).
### 3.2.2 Pose Tracking as a Quadratic Program ↑

Combining the robot modeling foundations introduced in Section 3.2.1 allows formulating many of the challenges in robotics as mathematical problems. These problems can then be efficiently solved through optimization techniques, enabling for a systematic approach to robot control.

One of these problems constitutes a major focus of this work: pose trajectory tracking. In practical terms, this section shows how to formulate the cost function of the problem introduced in (3.1) in the constrained normed least squares form shown in (3.2) so that it can be solved through QP (to see how, refer to Appendix A).

The case in consideration assumes the robot is controlled at the kinematic level, i.e. through its joint velocities $\dot{q}$ and that the current robot configuration $q_k$ can be measured.

A given a reference pose trajectory $\mathcal{X}^r(t) \in \mathbb{SE}(3)$, once discretized, yields a reference pose and twist (see Section 2.3) to be achieved at the current time step k: $\mathcal{X}_k^r = \mathcal{X}^r(t_k)$ and $v_k^r = v^r(t_k) \in \mathfrak{sc}(3)$ (with $t_k = k\Delta t$ and $k \in \mathbb{N}$). The k sub-indexes are dropped for clarity in the following expressions, as they always refer to the current step. In an ideal world, this should be enough to plug right into (3.2) to relate it to the joint velocities:

$$ \dot{q}^{\text{opt}} = \arg \min_{\dot{q}} \quad \| J(q) \dot{q} - v^r \|^2 \qquad (3.8) $$

Nevertheless, due to unmodeled dynamics, a Proportional Derivative (PD) term might be necessary for task servoing.

In that case, one can obtain the current pose of the robot $\mathcal{X}(q)$ using FK and then compute the ideal reference error twist in the body frame $^B\mathfrak{e} \in \mathfrak{sc}(3)$ required to arrive at $\mathcal{X}^r$ as:

$$ {}^B\mathfrak{e} = \log(\mathcal{X}^{-1} \mathcal{X}^r) \qquad (3.9) $$

Then, (3.8) can be reformulated with:

$$ \dot{q}^{\text{opt}} = \arg \min_{\dot{q}} \quad \| {}^B J(q) \dot{q} - {}^B v^* \|^2 \qquad (3.10) $$

$$ {}^B v^* = K_p {}^B \mathfrak{e} + {}^B v^r \qquad (3.11) $$

where $v^*$ denotes the desired twist, that leverages the reference twist from the trajectory and a proportional term with $K_p$. Note that in this case, the equations are expressed in the body frame for convenience but it can be reformulated as needed (see Section 3.2.4).

In order to generalize this formulation, one final problem remains: for kinematically redundant robots, $q^{\text{opt}}$ will not be unique and this might result in non desirable vibrations in the redundant DoFs of the robot. To overcome this issue, one can add a “regularisation task” to the cost function with a very small weight value, so that it has a negligible effect on the main tracking task.
There is a wide variety of regularisation tasks [Joseph, 2018] that can be chosen depending on the application, ranging from projected mass minimization for safety purposes [Joseph et al., 2019] to posture tasks that try to maintain a configuration. For simplicity, in this manuscript, the later is adopted, yielding the final formulation of the cost function:

$$ \dot{\mathbf{q}}^{\text{opt}} = \arg \min_{\dot{\mathbf{q}}} \left( \left\| {}^{B}\mathbf{J}(\mathbf{q})\dot{\mathbf{q}} - {}^{B}\mathbf{v}^{*} \right\|^2 + w_{\text{reg}} \left\| \dot{\mathbf{q}}_{\text{reg}} - \dot{\mathbf{q}} \right\|_2^2 \right) \quad (3.12) $$

$$ {}^{B}\mathbf{v}^{*} = \mathbf{K}_{p} {}^{B}\mathbf{e} + {}^{B}\mathbf{v}^{r} \quad (3.13) $$

$$ \dot{\mathbf{q}}_{\text{reg}} = \mathbf{K}_{q}(\mathbf{q}_{\text{reg}} - \mathbf{q}) \quad (3.14) $$

$$ \mathbf{q}_{\text{reg}} = \frac{(\mathbf{q}_{\text{M}} - \mathbf{q}_{\text{m}})}{2} \quad (3.15) $$

where $K_q$ is a positive proportional term; $w_{reg} \ll 1$ and $\dot{\mathbf{q}}_{reg}$ are defined as in [Joseph et al., 2020], to maintain a configuration $\mathbf{q}_{reg}$ distant from the joint bounds.

This definition allows for a pose tracking cost function that effectively solves the IK problem through a QP. That is why this controller is also referred to as a constrained differential IK solver.

### 3.2.3 Constraints Formulation ↑

This section explains how the linear constraints in (3.3) can be used to formulate the actuation limits in (3.7). For a robot controlled through its joint velocity, $\dot{\mathcal{Q}}$ can be directly included into the linear constraints as:

$$ \dot{\mathbf{q}}_{m} \le \mathbf{I}_{n} \dot{\mathbf{q}} \le \dot{\mathbf{q}}_{M} \quad (3.16) $$

where $\mathbf{I}_n$ denotes an n-sized identity matrix (here, it is made explicit in order to show a linear inequalities form).

However, other type of constraints need to be reformulated as a function of the decision variable $\dot{\mathbf{q}}$. For instance, the set $\mathcal{Q}$ (from (3.7)) and $\ddot{\mathcal{Q}}$ (for the joint accelerations):

$$ \ddot{\mathcal{Q}} = \{ \ddot{\mathbf{q}} \in \mathbb{R}^n \mid \ddot{\mathbf{q}}_m \le \ddot{\mathbf{q}} \le \ddot{\mathbf{q}}_M \} \quad (3.17) $$

can be reformulated through a first order taylor expansion as:

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

$$ C_{\dot{\mathbf{q}}|\ddot{\mathbf{q}}} = \{ \dot{\mathbf{q}} \in \mathbb{R}^n \mid \dot{\mathbf{q}} \in [\dot{\mathbf{q}}_k + \ddot{\mathbf{q}}_m \Delta t, \dot{\mathbf{q}}_k + \ddot{\mathbf{q}}_M \Delta t] \} \quad (3.19) $$

Note that the intersection of $C_{\dot{\mathbf{q}}|\mathbf{q}}$, $C_{\dot{\mathbf{q}}|\ddot{\mathbf{q}}}$ may create a feasibility problem [Rubrecht et al., 2010]. Then expressed as a linear function of $\dot{\mathbf{q}}$:

$$ \left[ \frac{\mathbf{q}_m - \mathbf{q}_k}{\Delta t} \right] \le \left[ \mathbf{I}_n \right] \dot{\mathbf{q}} \le \left[ \dot{\mathbf{q}}_k + \ddot{\mathbf{q}}_M \Delta t \right] \quad (3.20) $$

This section complements the QP proposition for a constrained optimization-based controller approach to pose tracking at the kinematic level. An overview integrating
<figure><img src="image_22.png" /><figcaption>Figure 3.2: Depiction of three conventional frames used to describe quantities in robotics: S is the world or universal frame; L is a local or body frame; A corresponds to a combined frame, placed in the body but aligned with the world frame.</figcaption></figure>

all the equations is shown in Appendix C. The following section presents some concepts regarding reformulating this controller in a different reference frame but does not further expand the current definition of the controller.

### 3.2.4 On the Solver Reference Frame ↑

This work exploits spatial algebra to express rigid body motion quantities in a concise manner. All these quantities require a frame in which they are expressed. This means that the same instant body motion can be represented, for instance in a local frame and thus be referred to as a body twist or in any other arbitrary reference frame.

Three widely used frames in the robotics literature are defined either with respect to the body in question or some arbitrary universal world frame. This work adopts the notation used in [Carpentier et al., 2019]. They are depicted in Figure 3.2:

* **World**: A universal or world frame, often referred to as frame S or W.

* **Local**: Also referred to as body frame and designated as *L* or *B*, represents a frame placed and aligned with the designated body. Notably, this frame presents a performance advantage in some algorithms (see page 95, Section 5.3 in [Featherstone, 2008]).

* **Local World-Aligned**: This frame is a combination of the previous two. It expresses a quantity with the same translation as a local frame but aligned with the world frame, it will be referred to as *A*. Some authors refer to it as the *moving-base frame* (see page 16 in [Traversaro et al., 2019]).
**REMARK.** The Adjoint action map for SO(3) and SE(3) for changing Twist Frames The adjoint action operator Ad presented on (2.50) for general Lie groups and briefly introduced in (2.38) for SE(3), is defined for the pose and orientation groups as:

$$v \in \mathfrak{sc}(3), \chi = (R, p) \in \mathfrak{SE}(3)$$

$$\mathbf{Ad} : \mathfrak{SE}(3) \times \mathfrak{sc}(3) \rightarrow \mathfrak{sc}(3)$$

$$\mathbf{Ad}_{\chi} v = \chi v \chi^{-1}$$

$$\mathbf{Ad}_{\chi} = \begin{bmatrix} R & 0 \\ p^{\wedge} R & R \end{bmatrix}$$

$$w \in \mathfrak{so}(3), R \in \mathfrak{SO}(3)$$

$$\mathbf{Ad} : \mathfrak{SO}(3) \times \mathfrak{so}(3) \rightarrow \mathfrak{so}(3)$$

$$\mathbf{Ad}_{R} w = RwR^{-1} = Rw \quad (3.21)$$

$$\mathbf{Ad}_{R} = R$$

**Changing a Twist From the Local Frame to the World Frame** Given some frame C described by a $\chi_{WC} = (R_{WC}, p)$ and some twist expressed in this frame $Cv$, it is possible to change the twist coordinates frame with:

$$w_{\nu} = \mathbf{Ad}_{\chi_{WC}}^{C} \nu \quad (3.22)$$

$$\mathbf{Ad}_{\chi_{WC}} = \begin{bmatrix} R_{WC} & 0 \\ p^{\wedge} R_{WC} & R_{WC} \end{bmatrix}, \quad p^{\wedge} = \begin{bmatrix} 0 & -p_3 & p_2 \\ p_3 & 0 & -p_1 \\ -p_2 & p_1 & 0 \end{bmatrix}$$

**From Local to Local World-Aligned** For a given body B described by a pose $\chi_{WB} = (R_{WB}, p_{WB})$, the transformation matrix goin from the world frame to the local world-aligned frame $\chi_{WA}$ can be obtained as:

$$\chi_{WA} = \chi_{WB} \begin{bmatrix} R_{WB}^{T} & 0 \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} I_3 & p_{WB} \\ 0 & 1 \end{bmatrix} \quad (3.23)$$

The controller described in this section was formulated in the local or body frame. Special care was taken to specify the quantities to which this applies. Each frame offers some advantages either in intepretability of the equations and/or simplicity of the formulation. Some quantities are more naturally expressed in certain frames, like the error in a pose $e = \log(\chi_k^{-1} \chi_r)$ is intrinsically expressed in the local frame; in the same way, the velocities in reference trajectories (a twist for a rigid body motion including the orientation) are often formulated either in the world or the local world-aligned frames, which also make it easier to interpret the motion in the workspace.

Following the method described in (3.22), the twists can be transported to other frames. For instance, for a body twist $^Bv$ on a body whose pose is described by a pose $\chi$ (its roto-translation from the world frame), the same twist may be expressed in the world frame as:

$$^Sv = \mathbf{Ad}_{\chi} \mathbf{B}v \quad (3.24)$$
Then (3.13) in the world frame becomes:

$$ 
\begin{align} 
{}^S \boldsymbol{v}^* &= {}^S \boldsymbol{e} + {}^S \boldsymbol{v}^r & \boldsymbol{e}, \boldsymbol{v} \in \mathfrak{se}(3) \notag \\ 
{}^S \boldsymbol{e} &= \boldsymbol{K}_p \boldsymbol{Ad}_{\chi} {}^B \boldsymbol{e} & \chi \in \mathfrak{SE}(3) \tag{3.25} \\ 
{}^B \boldsymbol{e} &= \log(\chi_k^{-1} \chi^r) & \notag 
\end{align} 
$$

Note that a bigger proportional gain $K_p$ produces a scaling on the resulting error $^S e$ that forces the element out of the manifold. This is why a formulation in the local frame should be favored.

A similar method with (3.26) can be used to change the Jacobian reference frame and reformulate the cost function in (4.7).

**REMARK.** Changing the Jacobian frame As shown in (3.22), it is possible to change a twist frame with the adjoint operator. The same method can be applied to the Jacobian to obtain the resulting twist in the world frame:

$$ 
\begin{align} 
{}^S \boldsymbol{v} &= {}^S \boldsymbol{J}(\boldsymbol{q}) \dot{\boldsymbol{q}} & \boldsymbol{J} \in \mathbb{R}^{6 \times n} & \boldsymbol{q}, \dot{\boldsymbol{q}} \in \mathbb{R}^n & \boldsymbol{v} \in \mathfrak{se}(3) \tag{3.26} \\ 
\text{with } {}^S \boldsymbol{J}(\boldsymbol{q}) &= \boldsymbol{Ad}_{\chi} {}^B \boldsymbol{J} & \notag 
\end{align} 
$$

where $\chi \in \mathfrak{SE}(3)$ expresses transformation from the world to the Jacobian frame.

## 3.3 Constrained Motion Generation with Model Predictive Control

The focus of this section is to propose an *Model Predictive Control (MPC)* implementation on the $\mathfrak{SE}(3)$ manifold, allowing pose control. It does so by revisiting some concepts from Chapter 2.

The section begins with an overview of some essential aspects of MPC. Starting from the description of a system in a state space representation, the discussion delves into formulating a linear system propagation for a receding horizon. This approach hinges on the assumption of constant linearized dynamics over a short time, which is essential for optimization applications over estimated future states. Then, the time discrete dynamic model is presented in a recursive matrix form over the entire horizon. This provides the foundation that allows formulating motion generation as a linear optimal control problem in a receding horizon that can be solved through a *Quadratic Program (QP)*.

Finally, the section finishes by providing an overview on manifold optimization and on how to exploit these techniques for a linear MPC strategy formulated over the pose manifold for constrained motion generation.

For a more exhaustive explanation on optimal control and MPC fundamentals, the reader may consult [Rawlings et al., 2017]; [Borrelli et al., 2017] and [Rossiter, 2003], for a more practical approach.
### 3.3.1 Problem Statement: Motion Generation via MPC ↑

Constrained motion generation consists in the problem of generating viable motions that account for changing conditions in the dynamic environment of a collaborative scenario. Task-level Cartesian re-planning performing this action needs to overcome two main challenges:

* Optimal task and constraint transitions

* Computational complexity

The first challenge implies that the re-planner needs to be able to account for the effects of its current actions. Moreover, it needs to do so while transitioning between task objectives in an optimal manner and while maintaining a strict consideration of the constraints throughout time. While this is in itself an already complex problem, it builds up to the second challenge: it becomes intractable given the real-time urgency requirements of dynamic environment events.

In this context, linear MPC strikes a balance between continuously evaluating the environment conditions to achieve a task while achieving an acceptable optimality (compared to, for example, results of offline global planners). Indeed, receding horizon optimization is able to continuously re-consider the future actions of a task subject to work-space configurations that are evolving in time, producing some level of guarantee on the viability of the generated motions [Rubrecht et al., 2010] and also offering smooth transition between the task objectives.

Another main advantage of MPC as motion generator is obtaining demonstrably trustworthy (as in constraint-respecting) motions [Kress-Gazit et al., 2021] which cannot formally be obtained from learning approaches [Matschek et al., 2023]. Indeed, MPC being a constrained optimization-based approach either provides a coherent solution or fails if it is non-existent. Conversely, there isn't formal approaches capable of guaranteeing that all solutions provided by learning algorithms respect the imposed constraints.

Recent contributions highlight the MPC features mentioned above for different applications [Bednarczyk et al., 2020]. Using MPC for motion generation in a similar way is proposed in [Gold et al., 2023] but ultimately they approach the technique from a servoing point of view, employing joint-level models [Oleinikov et al., 2021] and, quite often, use non-linear solvers, both of which increase the complexity of the formulations and, depending on the type of method employed, are not able to explicitly guarantee constraints [Kleff et al., 2021].

In contrast, this chapter proposal looks to streamline the control architecture from a task-centric point of view, which is why it focuses on point to point motions in the Cartesian space. The originality of the proposal goes beyond a philosophical point of view, as it complements this vision by proposing full pose motion generation: it exploits a mathematical formalism to also account for the orientation in the same MPC.

While the same Lie group formalism that enables optimization on the pose manifold has been employed for rigid body motion modeling [Terze et al., 2015] in the recent literature, they often resort to non-linear solvers [Knyazev et al., 2015] or were applied in unrelated applications [Chang et al., 2020]. In contrast, the current proposal
is to model this propagation in a linear way, allowing a quadratic cost function, necessary for linear MPC but most importantly, leading to a reasonable computation time.

Overall, the contribution is an efficient way to formulate a full body pose point to point constrained motion, obtaining horizon-based trajectories that can be updated continuously to account for the dynamic changes in the environment.

### 3.3.2 Linear Model Predictive Control ↑

This section presents a some fundamentals on modeling systems in a State Space (SS) representation, which enables state estimation and optimization over a horizon. This kind of formulation is crucial for the later parameterization of the Cartesian pose dynamics for constrained online motion generation.

A given non-linear system with dynamics $f(x, u)$ is parameterized with state $x \in \mathbb{R}^d$ and input $u \in \mathbb{R}^m$ vectors (where $d$ is the size of the state and $m$ the size of the input variables). The state vector is the smallest subset of system variables that contains all the information required to describe the system at some time instant. The input variable corresponds to the decision variable used to drive it to some desired or target state. The system can be discretized under Linear Time-Invariant (LTI) simplifying assumptions as (see Appendix B for more details):

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

revisited ↓

$$y_n = Cx_n + Du_n \qquad C \in \mathbb{R}^{r \times d}, D \in \mathbb{R}^{r \times m} \qquad (3.28)$$

where $y$ designates an r-sized output or observed variable and the matrices $C, D$ express its dependency on the current states and inputs; meanwhile, $A, B$ are the state and input matrices that propagate the future states and inputs through a linearization.

This kind of parameterization allows generically representing a system over a receding horizon. Note that for the general case of a non-linear system, all these matrices also depend on the state. This means that, for the interest of horizon estimation, the system needs to be linearized at some time instant $t_n$ and then $A_n$ can be assumed constant for a limited time window (for the unknown states that are to be estimated). The same assumption can be applied to $B, C, D$.

This last assumption is often exploited in the control field for predictive approaches. Assuming the constant linearized dynamics for a short time allows performing an optimization over the estimated future states. While the later estimations become less precise when the horizon grows (the constant linearized dynamics assumption is less true), this is not a problem if the linearization and the horizon recalculation are performed often (thus updating the linearization approximation to the new state).

In order to exploit this representation for state estimation in a horizon, one must first discretize it. Discretizing a 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, ..., h$ and one can define the discretized state and input at each time-step $t_k$ within the horizon as $x(t_k) = x_{k|n}, u(t_k) = u_{k|n}$. The sub-index k indicates the horizon discretization (an estimated value in the horizon)
and the n refers to the current time of the system so that $x(t_0) = x_{0|n}$ refers to the initial state of the system at the start of the horizon.

One can formulate a linear receding horizon expression by extending (3.27) over all the states and inputs in this time period as:

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

revisited ↓

This time discrete linearized model expression can straightforwardly be written in a recursive matricial form over the whole horizon. Subsequent sections will drop the n subscript to simplify the notation, making $x_{k|n}$ equivalent to $x_k$.

Leveraging this linear propagation of the state in a horizon is central to the formulation of MPC as a constrained QP and can be packed into a single equation by first constructing a vector for the propagated states and inputs in a horizon:

$$X_{0\dots h} = \begin{bmatrix} x_k \\ x_{k+1} \\ \vdots \\ x_{k+h} \end{bmatrix} \qquad U_{0\dots h-1} = \begin{bmatrix} u_k \\ u_{k+1} \\ \vdots \\ u_{k+h-1} \end{bmatrix} \quad (3.30)$$

revisited ↓

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

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

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

This allows compressing (3.29) to a single algebraic formula:

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

revisited ↓

where $\bar{X}$ and $\underline{U}$ respectively correspond to the future states and control inputs of the system. Furthermore, $A', B'$ are matrices constructed from $A, B$ as:

$$A' = \underbrace{\begin{bmatrix} A \\ A \\ A \\ \vdots \\ A \end{bmatrix}}_{h \text{ times}} \qquad B' = \underbrace{\begin{bmatrix} B \\ B \\ B \\ \vdots \\ B \end{bmatrix}}_{h \text{ times}} \quad (3.32)$$

Finally, one can formulate the linear optimal control problem in a receding horizon as a QP with the following general quadratic cost function:

$$\min_{x,u,x_h} \frac{1}{2} \underline{U}^T R \underline{U} + \frac{1}{2} \underline{X}^T P \underline{X} + \frac{1}{2} x_h^T P_T x_h \quad (3.33)$$

subject to:

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

revisited ↓

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

revisited ↓

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

revisited ↓

where $R, P, P_T$ represent, respectively, weighting matrices for the input, state and the
terminal state. Meanwhile, $C_x, C_u$ allow formulating linear constraints<fnref n="5" /> with respect to the state and input; $x_m, x_M$ and $u_m, u_M$ respectively designate the state and input bounds. In a general sense this cost function minimizes the distance towards the desired state (giving an explicit treatment to the terminal state in the horizon $x_h$) while penalizing the effort (as in the control input $u$).

The choice of values for the weighting matrices $R, P$ and specially the terminal state weight $P_T$ have significant effects over the resulting form of the optimal solution. Some insights are provided in [Tan, 2016] (Section 5.2.3) on how varying weights can lead to under/over shooting. The same source also highlights the effects of varying the horizon length. In a broad sense, these parameters affect the convergence towards the solution of the optimization and thus influence the stability of the problem (specially the terminal state) [Mayne et al., 2000].

Note that (3.34) expresses the link between the propagated states, leveraging the equality constraints of a QP solver. This establishes the optimization scheme as a direct single shooting method [Bard, 1974].

Equation (3.33) under constraints (3.34) to (3.36) constitutes the general linear MPC formulation as a constrained quadratic optimization [Alexis et al., 2012]; [Bemporad et al., 2002].

### 3.3.3 Optimization on the Pose Manifold ↑

Using a receding horizon optimization scheme to generate a pose trajectory requires formulating the control problem of predicting the end-effector pose given a horizon of control inputs, here considered at the velocity level. This section quickly revisits concepts from Section 2.4 to re-introduce the integration scheme of the pose dynamics in the tangent space for MPC.

Given some system with an initial pose $\mathcal{X}_i \in \mathbb{SE}(3)$ subject to a body twist $v(t) \in \mathbb{SE}(3)$, its discretized pose trajectory $\mathcal{X}_k = \mathcal{X}(t)|_{t=k\Delta t} \in \mathbb{SE}(3)$ can be described as:

$$ \begin{aligned} \mathcal{X}_k &= \mathcal{X}_i e^{v_0 \Delta t} e^{v_1 \Delta t} \dots e^{v_{k-1} \Delta t} \\ &= \mathcal{X}_i \prod_{n=0}^{k-1} e^{v_n \Delta t} \end{aligned} \qquad (3.37) \qquad \text{revisited } \uparrow $$

This relation can be used incrementally to compute the desired twists at each time instant given some tracking error [Lynch et al., 2017]. Nevertheless, the pose trajectory in a horizon remains a non-linear function with respect to the twist. In other words, the system evolves as the successive Lie group actions, acting on its state (pose) to glide through the $\mathbb{SE}(3)$ manifold at each time point, as shown in (3.37). Hence, to be solved at a decent control rate for reactivity, the MPC should be expressed in a linear form like in (3.31) (Section 3.3.2).

This section presents how to link both forms while providing a rough explanation on the conceptual steps for optimizations on manifolds employed in this work (for more details see [Forster et al., 2015]; [Saccon et al., 2013]).

Consider the optimization problem of a variable belonging to a smooth manifold $\mathcal{X} \in \mathcal{M}$; hence there exists a map $f: \mathcal{M} \to \mathbb{R}^n$ (with its inverse denoted $f^{-1}$), transporting elements from the solution space into an optimization space $f: \mathcal{X} \to \mathcal{X}$,

<footnote n="5">⁵Linear constraints can be used to formulate convex polytopes as half planes.</footnote>
allowing the definition of the optimization problem with cost function C as:

$$x^{\text{opt}} = \arg \min_{x \in \mathbb{R}^n} C(x) \qquad (3.38)$$

$$\mathcal{X}^{\text{opt}} = f^{-1}(\mathcal{X}^{\text{opt}}) \qquad (3.39)$$

With a conveniently chosen lifted space (see Section 2.4.1), optimizing can take the form of a quadratic cost function like for linear MPC as in (3.33) and be solved as a QP.

This re-parameterization of the original space into an optimization space that offers some desirable properties is often referred to as *lifting* or *pushing* while the inverse operation is a *retract* or *pull*. Refer to Figure 2.7 for a visual representation. Then the solution can be recovered through a retraction (as in (3.38)), resulting in a lift, optimize and retract scheme. The lift and retract functions are respectively denoted with $\psi, \psi^{-1}$.

As explained in Chapter 2, the $SE(3)$ manifold can be mapped to $sc(3)$, through a differentiable map:

$$\begin{aligned} \log &: \quad SE(3) \to sc(3) \qquad \mathcal{X} \to \xi \\ \exp &: \quad sc(3) \to SE(3) \qquad \xi \to \mathcal{X} \end{aligned} \qquad (3.40) \quad \text{revisited } \uparrow \downarrow$$

The log function<fnref n="6" /> relates the Lie group $SE(3)$ to its Lie algebra $sc(3)$, its tangent space, at the origin. As explained in Section 2.4.1, the bijectivity properties are maintained<fnref n="7" /> for $|\phi| < \pi$ (where $\phi$ is the angular component of $\xi$).

The tangent space behaves like an euclidean space for optimization purposes: it allows representing a pose as its exponential coordinates (see Section 2.2.2.3), a 6-vector instead of an homogeneous transformation matrix.

Analyzing (3.37) for a single step yields (2.48), the relation between an infinitesimal increment in pose (body twist) and the resulting pose:

$$\mathcal{X}_{k+1} = \mathcal{X}_k e^{\Delta t v_k} \qquad (3.41) \quad \text{revisited } \uparrow \downarrow$$

This is a first-order approximation, analogous to the one presented in [Forster et al., 2015] for $SO(3)$ and [Solà et al., 2018] (for more general manifolds).

As introduced in Section 2.4.2, it can be reformulated the tangent space as:

$$\xi_{k+1} = \log(e^{\xi_k} e^{\Delta t v_k}) \approx \xi_k + \text{dlog}_{\xi_k} \Delta t v_k \qquad (3.42) \quad \text{revisited } \uparrow \downarrow$$

where $\text{dlog}_{\xi_k}$ is the right-trivialized log map derivative<fnref n="8" /> at $\xi_k$, that relates additive increments in the tangent space at the origin to the right-multiplied increments in

<footnote n="6">⁶Throughout this work, the log and exp operations adopt the vectors forms. This could be considered an abuse of notation but one can go back and forth between the matrix and vector forms through the Vee map and its inverse (see Sections 2.2.1.3 and 2.2.2.2) or [Solà et al., 2018]; [Torres Alberto et al., 2022a].</footnote>

<footnote n="7">⁷The one to one correspondence is maintained under the conditions detailed in [Forster et al., 2015] and [Torres Alberto et al., 2022a] (Section 2.1). Otherwise, the exp map is surjective.</footnote>

<footnote n="8">⁸Given the adopted vector form of the tangent space in this work, the $\text{dlog}_{\xi_k}$ takes the form of a 6 × 6 matrix.</footnote>
the pose space. In layman's terms<fnref n="9" />, this relates the body twist with an increment in the logarithm of the pose. This constitutes the push-forward operation that enables optimization in a manifold [Boumal, 2023], extending the concepts introduced in this section.

Finally, the missing link that allows the present work to formulate a pose propagation in a linear form (as in (3.29)) for a receding horizon is shown in (3.42), as required for the linear MPC in (3.33).

### 3.3.4 Geodesic Path in the Tangent Space ↑

As introduced in Section 3.3.3, this work aims at finding the pose and input trajectory that will drive the system to some target pose $\mathcal{X}_t \in \mathbb{SE}(3)$. In order to embed this in the MPC cost function, a distance metric formulated in the tangent space is necessary. In fact, $\mathfrak{se}(3)$ is a Riemannian manifold [Lee, 2019] equipped with the geodesic distance metric known as Log-Euclidean ([Jayasumana et al., 2014], Table 1):

$$e_k = \left\| \underbrace{\xi_k}_{\log(\mathcal{X}_k)} - \underbrace{\xi_t}_{\log(\mathcal{X}_t)} \right\|^2 \quad (3.43)$$

The last challenge to overcome is that there exist multiple paths that connect two extreme poses $\mathcal{X}_i, \mathcal{X}_t \in \mathbb{SE}(3)$. Given some $\alpha(t) \in [0, 1]$ the shortest path (geodesic) [Novelia et al., 2015]; [Marthinsen, 1999] can be interpolated by using the path that passes through the origin and computing its “distance” in tangent space $\delta$:

$$\mathcal{X}_k = \mathcal{X}_i e^{\alpha_k} \delta \qquad \delta = \log(\mathcal{X}_i^{-1} \mathcal{X}_t) \quad (3.44)$$

which is depicted in Figure 3.3. One way to ensure that the trajectory optimization in the lifted space corresponds to (3.44) is through the choice of the lift function (and consequently also the retract function):

$$\psi(\mathbf{x}) = \log(\mathcal{X}_l^{-1}\mathbf{x}) \qquad \psi^{-1}(\mathbf{x}) = \mathcal{X}_l \exp(\mathbf{x}) \quad (3.45) \qquad \text{revisited } \downarrow \\ \mathcal{X}_l = \mathcal{X}_t$$

where $\mathcal{X}_l$ denotes the lift pose. This way, the optimization can be “directed” from $\psi(\mathcal{X}_i) \to \psi(\mathcal{X}_t)$, which yields the geodesic. The choice of $\mathcal{X}_l = \mathcal{X}_t$ stems from the application: it allows the optimization to always converge to the origin in the tangent space $\xi_l$ because: $\psi(\mathcal{X}_t) = \xi_l$. In fact, the lift function in (3.45) implies that $\psi$ is in the tangent space at $\mathcal{X}_l^{-1}$.

### 3.3.5 Task-Space Linear MPC in the Tangent Space ↑

This section reformulates the problem introduced in Section 3.3.3 using the same notation as in (3.30).

The first step consists in defining the state and input variables:

$$\mathbf{x}_k = \psi(\mathcal{X}_k) \qquad \mathbf{x}_t = \psi(\mathcal{X}_t) \qquad \mathbf{u}_k = \mathbf{v}_k \quad (3.46) \qquad \text{revisited } \downarrow$$

<footnote n="9">⁹Plain English (layman's terms) is a mode of writing or speaking the English language intended to be easy to understand regardless of one's familiarity with a given topic. https://en.wikipedia.org/wiki/Plain_English</footnote>
<figure><img src="image_23.png" /><figcaption>Figure 3.3: There exist multiple paths that connect some initial and target poses $\mathcal{X}_i, \mathcal{X}_t \in \text{SE}(3)$. The image shows the successive directions of the red axis of a frame (3-axis) moving through the path. It illustrates the geodesic (in red) and an alternative path (in yellow). The yellow path can be interpolated in the tangent space and then transformed to poses as $\mathcal{X}_k = e^{(1-\alpha_k)\xi_0 + \alpha_k\xi_t}$ for some $\alpha(t) \in [0, 1]$.</figcaption></figure>

where $x_t$ is lifted the target state; $x_k = \xi_k$ is the lifted state and $v_k$ is the body twist.

Then it is possible to extend the distance metric in the tangent space (3.43) over a horizon to use it as a cost function:

$$ \bar{\mathbf{X}}^*, \underline{\mathbf{U}}^* = \arg\min_{x, u} ||\bar{\mathbf{X}} - \bar{\mathbf{X}}_t||_2^2 + \gamma ||\underline{\mathbf{U}}||_2^2 \quad (3.47) $$

revisited ↓

$$ \bar{\mathbf{X}}_t^T = \underbrace{[\psi(\mathcal{X}_t)^T \dots \psi(\mathcal{X}_t)^T]^T}_{\text{h-1 times}} \quad (3.48) $$

revisited ↓

$$ \begin{array}{ll} \text{s.t:} & \mathbf{x}_{k+1} = A\mathbf{x}_k + B\mathbf{u}_k & \text{for } k = 0, \dots, h-1 \\ & \mathbf{u}_m \le \mathbf{u}_k \le \mathbf{u}_M & \text{for } k = 0, \dots, h-1 \\ & \dot{\mathbf{u}}_m \le \frac{\mathbf{u}_{k+1} - \mathbf{u}_k}{\Delta t} \le \dot{\mathbf{u}}_M & \text{for } k = 0, \dots, h-2 \end{array} \quad (3.49) $$

where $\gamma$ designates a weighting term for the input throughout the horizon and $\psi(\mathcal{X}_t) = \xi_t$ corresponds to the lifted target pose while $\bar{\mathbf{X}}_t$ is its vector version throughout the horizon. The input constraints (for simplicity in the formulation, often defined as $\mathbf{u}_M = -\mathbf{u}_m$), establish the limits for the body twist and spatial body acceleration (through a finite difference).

Note that the definition in (3.48) is defined such that the MPC is in charge of generating the motion that minimizes the distance towards the target state while respecting the constraints. This is part of the reference-less approach adopted.

Furthermore, (3.47) shows the standard linear MPC formulation in (3.33). The state can be propagated throughout the horizon as in (3.29) by using the relationship shown in (3.42), which yields:

$$ A = I_6 \qquad B = \text{dlog}_{\xi_i} \Delta_h t \quad (3.50) $$

revisited ↓

where $A$ is an identity matrix and $B$ reflects the tangent space linearization at the initial robot pose, as $\xi_i = \psi(\mathcal{X}_i)$. This constitutes the central relation that enables tangent space linear MPC.
<figure><img src="image_24.png" /><figcaption>Figure 3.4: Safe workspaces in a collaborative environment cannot be defined with static zones as it depends on the robot-operator states and tasks.</figcaption></figure>

Finally, in order to recover the desired poses $\mathcal{X}_k^{\text{des}}$ from $\bar{\mathbf{X}}^*$:

$$ \mathcal{X}_k^{\text{des}} = \psi^{-1}(\mathbf{x}_k^*) \qquad (3.51) $$

This constitutes the linear MPC formulation that allows full pose and constrained trajectory generation in an online manner.

## 3.4 Experimental Validation: The Case of Velocity Modulation ↑

This section presents an implementation of the proposed Model Predictive Control (MPC) for a collaborative robotics scenario that emphasizes the safety features of the approach.

Safety in robotics has always been a concern, but modern industrial trends have specially exacerbated this in search for better integration of robots and humans in working environments. For collaborative robots, this is addressed through norms or technical specifications.

One of the most recent standards materialized in a Technical Specification (TS) [ISO/TS-15066, 2016]. Although innovative, it still tends to be a somewhat coarse method that frequently results in impractical configurations and installations that are challenging to certify. This leads to collaborative robots in the industry that are mostly used as standard robots, i.e. behind cages or inefficient and empty safety zones.

Addressing safety in an efficient way when considering shared workspaces is a complex problem that requires an online evaluation of multiple moving bodies. Nevertheless, just like with the TS, this problem is often simplified as a “workspace safety state machine” through discrete zones [Amaya-Mejía et al., 2022]; [Long et al., 2018].

Some recent promising methods focus on considering safety from an energetic level [Joseph et al., 2018a]; [Raiola et al., 2018]; [Meguenani et al., 2015], which, although more complex in implementation, allow for more flexible solutions.
<figure><img src="image_25.png" /><figcaption>Figure 3.5: Classical vs proposed control architecture. (Top) Constrained offline planning employed in classical control approaches cannot deal with dynamic environments that evolve over time. (Bottom) Proposes a cascade of closed-loop composed of task-space online trajectory re-planning over a receding horizon (MPC block) and a high-frequency lower-level task-to-joint-space controller (inverse velocity kinematics block). A third block (Workspace Analysis) feeds the changing environment information to the MPC to achieve safety-awareness.</figcaption></figure>

revisited ↑ ↓

Figure 3.4 aims to depict this situation: a dynamically safe area for the robot to evolve can be computed based on the state of both the operator and the robot as well as on their near future motions. However, the task of accurately calculating the potential area and making real-time control decisions based on this calculation, which is essential for facilitating close encounters as the one in Figure 3.4, remains an open research challenge [Long et al., 2017]; [Mainprice et al., 2016]; [Eckhoff et al., 2022].

In its most general form, it boils down to solving a global optimal control problem online that determines at each time a control trajectory that satisfactorily combines safety and efficiency.

Despite some research effort in this direction, this very general problem is far from being solved in a generic way and goes far beyond the scope of this work. Yet, a clear required feature emerges from the search for optimality: the need to predict the effect of control actions over a time horizon to account for the continuous changes to workspace constraints.

This last requirement is directly addressed by the control architecture proposed in Section 3.1.1 and this experiment showcases one implementation of this feature.

### 3.4.1 Implementing Online Re-planning ↑

Beyond safety, a close dynamic interaction between a human operator and a robot may also imply on-the-fly re-definitions of the task to be achieved by the robot. This cannot be solved by “classical control architectures” (such as the one depicted in the top of Figure 3.5) which generally consider offline planning solutions, incapable of