Keywords: pose propagation in linear form, 2, 11 Average error for different horizon lengths

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. . . . .

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 intervens 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. . . . . . . . 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. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.3 There exist multiple paths that connect some initial and target poses X i , X t ∈ 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 X k = e (1-α k )ξ 0 +α k ξ t for some α(t) ∈ [0, 1]. . . . . . . . . . . . 3.4 Safe workspaces in a collaborative environment cannot be defined with static zones as it depends on the robot-operator states and tasks. .

revisited ↑ ↓

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 (Model Predictive Control (MPC) block) and a high-frequency lower-level task-to-jointspace controller (inverse velocity kinematics block). A third block (Workspace Analysis) feeds the changing environment information to the MPC to achieve safety-awareness. . . . . . . . . . . . . . . . . . . 3.6 The planned horizon in time. The online MPC progressively "discovers" a path from the initial pose (on the left) towards the target pose (on the right). Given the MPC optimizes over a receding time window, the optimizing horizon does not cover the complete path until it is close enough to the target pose. . . . . . . . . . . . . . . . . . . . . . . 3.7 Numerical simulation of the replanning controller for 1-Degree of Freedom (DoF). This example shows the result of employing the MPC to replanify at 50Hz and apply the resulting twists for a Cartesian motion along one of the axis (x from the frame attached to the robot base). To compare optimality, the output is compared to the jerk-bounded algorithm in [Berscheid et al., 2021b]. . . . . . . . . . . . 3.8 Velocity modulation experimental setup. The photo shows the setup.

The robot is placed on a table to perform a task provided on-the-fly.

The distance to the operator d h used for safety-aware velocity modulation is measured via the laser sensor. . . . . . . . . . . . . . . . . . .

3.9 Shows the result of requesting the robot to move between 4 preset poses. While the robot moves, a laser sensor is used to capture the minimal distance to a human and modulate the maximum velocity (shown in red shade). The error curves show the distance between the current position and orientation (p k , R k ) and the preset target poses (p t , R t ). The linear error is e = p tp k , while the angular error is computed as φ = log(R -1 k R t ). The MPC (T=300ms, h = 10, ∆ h = 30ms, f MPC = 50Hz) modulates the body twist ν = [v T , ω T ] T respecting the safe limits imposed even when they become so low that the robot stops moving. . . . . . . . . . . . . . . . . . . . . . . . . 3.10 QR code for the video of the velocity modulation experiment. On PDF file readers, it is also a clickable hyperlink. . . . . . . . . . . . . . . . . .

revisited ↑ ↓

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. . . . . . . . . . . . . . . . . . . . . . .

revisited ↑ ↓

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. . . . . . . . . . . . . . . . . . Reflective markers are placed on a plastic brick. The camera array is used to detect the position and orientation of the plastic brick in the workspace. The robot is instructed to grab the brick without prior knowledge of its pose. The experiment consists of three stages of increasing complexity: 1-non-moving brick; 2-slowly moving brick; 3-the brick is held by the operator and tracked in real time. . . . . . . . 4.5 In the first phase, the robot is tasked to pick up the plastic brick positioned on an uneven surface to induce a non-trivial orientation. The brick pose, unknown beforehand, is identified through a vision system and remains stationary. Using this information, the robot moves to the brick as per the control scheme shown in Figure 4.1. After grasping, it holds the brick for a few moments and then releases it.

The tracking results indicate target poses set in real-time. In each graph, the red, green, and blue curves denote x, y, and z axis components. Actual robot velocities are shown with solid lines, while the desired velocities (calculated by the MPC and denoted ν mpc ) are dotted. Shaded regions mark the bounds set for the MPC. . . . . . . . . . 4.6 This showcases the third phase of the experiment. Certain key moments of the accompanying video have been captured in still images to provide a more immediate understanding. During the third phase of the experiment, the focus shifts to the robot real-time adaptability and its capability to re-plan its actions in response to dynamic changes. During this phase, a human operator holds the plastic brick, moving it in an unpredictable manner at a moderate pace. As the brick moves, the robot diligently tracks its position and orientation in real-time. Adding to the complexity, the operator intervenes by restraining the robot movement for a short period (shown in (d) and (e)), serving as an external disturbance. This interruption aims to highlight the robot proficiency in rapidly recalculating and adjusting its strategy. The images depict three elements: on the left, the real robot as seen during the test is shown; on the bottom right, a virtual counterpart displays both the target pose (the brick pose) and the horizon trajectory calculated in real-time; meanwhile, the top right presents graphs illustrating the tracking error and planned velocities. . . . . . . 4.7 QR code for the video of the high-frequency acceleration-based Model Predictive Controller experiment for full-pose online constrained motion generation. On PDF file readers, it is also a clickable hyperlink. . .

revisited ↑ ↓

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. . . . . . . . . . . . . . . . . . . . .

revisited ↑ ↓

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. . . . . . . . . . . 5.3 The translational velocity polyhedron. It visualizes the feasible velocity set P ṗ| Q as defined in (5.8) once transformed into the "equivalently occupied space". Multiplying the set by a small time interval ∆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. . . . . . . . . .

5.4

The translational convex reachable space approximation polyhedron. This illustrates the attainable space for two different configurations.

It presents P p|Q obtained from (5.21) with the multiplication of the Jacobian J l by a scaling factor of 0.15. This scaling is essential for visualization since the approximation becomes coarse in extreme cases, particularly those farthest from the joint boundaries. The first configuration, 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. . . . . . . . . . . . . . . . . . . . . . . . . . 5.5 Example horizon with zonotope constraints and zero initial conditions: The simulation begins at the origin with zero velocity, illustrating the system response to constraints. The trajectory demonstrates the shortest path towards the nearest feasible point within the constraints, since the target position is unreachable due to the imposed limitations. Notably, the trajectory closely resembles an optimal planning algorithm utilizing a trapezoidal acceleration profile, showcasing efficient utilization of available kinematic resources. . . . . . . . . . 5.6 Example horizon with zonotope constraints and non-zero initial velocity: The simulation highlights the impact of the initial velocity on the trajectory. Due to limited kinematic capacities, the optimal trajectory takes on a curved path towards the target. This observation is particularly relevant in online re-planning scenarios, where the system must continuously compute new trajectories based on evolving initial conditions, demonstrating the capability for smooth transitions that consider the starting point in a loop. . . . . . . . . . . . . . . . . . 5.7 Example horizon with polyhedral constraints, non-zero initial velocity: This third simulation implements non-trivial polyhedral velocity constraints. This combines elements from the previous scenarios while mimicking the constraints derived from joint capabilities. The resulting trajectory exhibits a curved path that gradually transitions toward the target position. It is worth noting that the trajectory saturates the available velocity capacity by closely following the edges of the polyhedron, demonstrating how the system optimally utilizes constrained kinematic resources to approach the goal. . . . . . . . . . .

revisited ↑ ↓

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. . . . . . . . . . . . . . . . . . 5.9 An example of the back and forth scenario (as defined in Section 5.4.3) used for the experiments, showing the starting and ending poses on the real setup. This scenario is also used in simualtions but the video shows one of the experiments being executed on the robot. . . . . . . .

5.10

Showcases the interactive scenario (as defined in Section 5.4.3) used for the experiments, showing the robot tracking the pose of a plastic piece in real time. The video shows the experiment. . . . . . . . . . . . 5.11 This image showcases the results of a joint-bounds aware task-space planning addressed in Section 5.4.6. The experiment corresponds to a simulation of a motionless planning scenario (as defined in Section 5.4.3) in which the robot is driven to a configuration near its joint bounds so that it cannot move towards the target cube. When the joint bounds limits are disabled (in (a), (b) and (c)), the robot plans a horizon path through the geodesic towards the target. Conversely, once the joint bounds limits are employed in (d) and (e), the horizon becomes very small and tries to find a path that minimizes the distance to the target without violating the joint bounds constraints. . . . . . . . 5.12 Real-world Experiment: Showcases a back and forth task scenario between two poses that have the same orientation. The objective is to show the robot behavior while the allowed joint range is artificially reduced through a factor R q beween 0 and 1 (also shown in the plot) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints. The passage from R q = 0.6 to R q = 1 is highlighted with a dashed vertical line. . . . . . . . . . . . 5.13 Simulation: Showcases a back and forth task scenario between two poses without changing the orientation, while changing the maximum allowed q (showed with R q) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.14 Simulation: Showcases a back and forth task scenario between two poses that have different orientations, while changing the maximum allowed q (showed with R q) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.15 Real-world experiment: Showcases a back and forth task scenario between two poses that have the same orientation, while changing the maximum allowed q (showed with R q) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.16 Real-world experiment: Showcases an interactive task scenario where the robot tracks a pose captured in real-time while changing the maximum allowed q (showed with R q) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints in an online fashion. The corresponding video is shown in Figure 5.10. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.17 A smooth jerk-bounded (TAP) stopping trajectory example for a worst-case scenario: v 0 = -0.7v M , a 0 = -a M (acceleration starts at maximum value and not inversed to velocity while velocity starts near its maximum value). . Polytope constraints from compatibilized joint-space constraints. . . 2 MPC parameters used in this implementation. In the case of w T , the terminal weight is homogeneized to account for the dimensional difference in the state (i.e. positions and velocities) as defined in (5.60). . . 5.3 Franka Emika Cartesian trajectory requirements, as specified on their site: https://frankaemika.github.io/docs/control _ parameters.html. 5.4 Franka Emika joint trajectory requirements, as specified on their site: Chapter 1 Introduction "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

List of Tables

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 humancentric 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.

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 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 repported a net revenu 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 [START_REF] Beauville | Démarche de conception d'un système de production industriel reconfigurable, dans un contexte de fortes variations de marché en volume et en diversité[END_REF]; [START_REF] Voilqué | Formalisation du problème de conception d'un exosquelette industriel et application à l'assistance aux efforts des membres supérieurs[END_REF].

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. 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 [START_REF] Ghobakhloo | Industry 4.0, digitization, and opportunities for sustainability[END_REF]. 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" [START_REF] Gunasekaran | Agile manufacturing: A framework for research and development[END_REF].

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, reasigning 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 [START_REF] Beauville | Démarche de conception d'un système de production industriel reconfigurable, dans un contexte de fortes variations de marché en volume et en diversité[END_REF].

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 [START_REF] Demir | Industry 5.0 and Human-Robot Co-working[END_REF]; [START_REF] Lu | Outlook on human-centric manufacturing towards Industry 5.0[END_REF]. 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 of manufacturing facilities, just like before they were removed in favor of automation 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 strategies 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 original promises and transition into "Agile Robotics".

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 crystallized 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 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 workcells [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]; [START_REF] Fryman | Updating the industrial robot safety standard[END_REF]; [START_REF] Chemweno | Orienting safety assurance with outcomes of hazard analysis and risk assessment: A review of the ISO 15066 standard for collaborative robot systems[END_REF], 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 humanrobot relation [L. [START_REF] Wang | Symbiotic human-robot collaborative assembly[END_REF]. 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 [START_REF] Tsarouchi | Human-robot interaction review and challenges on task planning and programming[END_REF].

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).

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 reasignable. 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.

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 [START_REF] Skuric | On-line force capability evaluation based on efficient polytope vertex search[END_REF]. 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 asses 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 operatorcentric) 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 deployed 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.

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 framework 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 envisioned 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 [START_REF] Raiola | Development of a Safety-and Energy-Aware Impedance Controller for Collaborative Robots[END_REF]; [START_REF] Meguenani | Control of robots sharing their workspace with humans: An energetic approach to safety[END_REF]. Furthermore, its certification is complex and leads to cobots that are used in noncollaborative 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 aspects of the architecture immediately points the approach towards a modeling strategy for the "external events" and for the robot capacities: constraints. Some approaches contribute towards the safety aspects while considering these capacities in joint space [START_REF] Joseph | Online velocity constraint adaptation for safe and efficient human-robot workspace sharing[END_REF]. However, a method to consider workspace constraints 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 [START_REF] Rubrecht | Constraints Compliant Control: constraints compatibility and the displaced configuration approach[END_REF]; [START_REF] Prete | Joint Position and Velocity Bounds in Discrete-Time Acceleration/Torque Control of Robot Manipulators[END_REF]. While some approaches do consider the real capacities and are capable of formulating workspace constraints [START_REF] Kleff | High-Frequency Nonlinear Model Predictive Control of a Manipulator[END_REF], 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 single architecture is to employ an optimization-based approach. The idea is to translate 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 [START_REF] Gold | Model Predictive Interaction Control for Robotic Manipulation Tasks[END_REF]. 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 [START_REF] Forster | On-Manifold Preintegration Theory for Fast and Accurate Visual-Inertial Navigation[END_REF], the methodologies are often employed for state estimation in mobile robots [START_REF] Solà | A micro Lie theory for state estimation in robotics[END_REF]. 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.

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 taskspace 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 [START_REF] Skuric | Online task-space trajectory planning using realtime estimations of robot motion capabilities[END_REF] Antun 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 selfcontained. 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

Navigation and Notation Aspects in this Manuscript

relevant to bring it to the reader's attention, it appears on the manuscript margins like this:

revisited ↓ p ∈ R 3 (1.1)
where the arrow on the right is a reference to the where it is revisited, right below:

revisited ↑ p = [p x p y p z ] (1.2)
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 explicitely saying it corresponds to an equation), to facilitate reading. However, when referring to tables, figures or sections, it is explicitely 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 approach resides in its capacity to streamline predictive and optimizationbased 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 movements 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 circumventing the multiplicative propagation scheme. As a final point, the integration 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.

Introduction ↑

The rise of collaborative robotics has led to a surge in the need for intricate control mechanisms [START_REF] Ajoudani | Progress and prospects of the human-robot collaboration[END_REF]. Over time, the focus on safety in human-robot interactions has intensified [START_REF] Proia | A Literature Review on Control Techniques for Collaborative Robotics in Industrial Applications[END_REF]. 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 [START_REF] Escande | Hierarchical quadratic programming: Fast online humanoid-robot motion generation[END_REF], 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 [START_REF] Ibanez | Optimization-Based Control Approaches to Humanoid Balancing[END_REF]. 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 [START_REF] Müller | Screw and Lie group theory in multibody dynamics: Recursive algorithms and equations of motion of tree-topology systems[END_REF] for online trajectory generation [START_REF] Huber | An Online Trajectory Generator on SE(3) for Human-Robot Collaboration[END_REF]. While some recent studies [START_REF] Kleff | High-Frequency Nonlinear Model Predictive Control of a Manipulator[END_REF] 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 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) [START_REF] Casas | Explicit Magnus expansions for nonlinear equations[END_REF]) 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 1 Typically of the robot end-effector. 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.

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 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 [START_REF] Lynch | Modern Robotics: Mechanics, Planning, and Control[END_REF], Section 2.2 of [Siciliano et al., 2008], Chapter 2 of [START_REF] Craig | Introduction to Robotics : Mechanics & Control. Includes bibliographies and index[END_REF]; [START_REF] Murray | A Mathematical Introduction to Robotic Manipulation[END_REF].

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 ∈ 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 [START_REF] Brockett | Robotic manipulators and the product of exponentials formula[END_REF].

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. To construct a rotation matrix, one can imagine a frame denoted by 3 orthonormal vectors expressed a global frame x, y, z ∈ R 3 representing 3 points attached to a body. Re-orienting this body results in 3 new coordinates for these points x ′ , y ′ , z ′ ∈ 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 det R = 1 R ∈ R 3×3 (2.1) ⇒ R ∈ SO(3)
which can perform rotations on the Euclidean space without modifying distances: given some vector e ∈ R 3 , it can be rotated to obtain a new vector with the same length e ′ ∈ 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 [START_REF] Merker | Theory of Transformation Groups[END_REF] 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 G with a binary operation denoted • such that any combination of two elements of G yields a new one: a • b = c where a, b, c ∈ G. It must also respect the group axioms [START_REF] Lang | Algebra[END_REF] such that:

• Associativity: for a, b, c ∈ G, then:

(a • b) • c = a • (b • c).
• Identity element: there exists an identity element e ∈ G such that for a ∈ G:

e • a = a • e = a. • Inverse element: for each element a ∈ G there exists b ∈ G such that: a • b = b • a = e.

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 necesssary to go from the orientation of body 1 to the frame S is the inverse: R 1S = R -1 S1 , which, from the properties of SO(3) in (2.1), corresponds to its transpose: R -1

S1 = R T S1 .
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 -1 S1 R S2 = R 1 ¡ S R ¡ S2 (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 -1 2 = R 2S . In contrast, if a rotation was defined with respect to some other body, then it needs to be explicitely 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 R S1 , R 12 , R 2 ∈ SO(3) (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 ≤ 2 (which includes orientations in 2D) is that they are abelian groups, meaning they are commutative:

R a R b = R b R a R a , R b ∈ SO(n) for n ≤ 2 (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.

Axis-angle representation ↑

One implication of Euler's rotation theorem [START_REF] Euler | Theoria motus corporum solidorum seu rigidorum (etc.)(Cum tabulis aeneis[END_REF] 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 [START_REF] Rodrigues | Des lois géométriques qui régissent les déplacements d'un système solide dans l'espace, et de la variation des coordonnées provenant de ces déplacements considérés indépendamment des causes qui peuvent les produire[END_REF]. The axis-angle representation allows describing a rotation by an angle φ about a unitary axis denoted φ ∈ R 3 . These parameters can be used to form a single vector φ = φ φ (note that φ = ||φ||) from which an equivalent rotation matrix R can be deducted by employing the Rodrigues' formula:

R(φ, φ) = I 3 + sin φK + (1 -cos φ)K 2 φ = [φ 1 φ 2 φ 3 ] T K =    0 -φ 3 φ 2 φ 3 0 -φ 1 -φ 2 φ 1 0    (2.5)
While not crucial to this chapter, this formula presents a closed-form for the exponential to map 3-vectors to rotation matrices [START_REF] Gogu | Représentation du mouvement des corps solides[END_REF]. The link with the exponential is further analyzed in the following sections.

Angular velocity ↑

The angular velocity is the a 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 [START_REF] Hamano | Derivative of Rotation Matrix Direct Matrix Derivation of Well Known Formula[END_REF]; [START_REF] Zhao | Time Derivative of Rotation Matrices: A Tutorial[END_REF].

The physical notion can be simply constructed from the motion of a point attached to a body. In particularly, Figure 2.3(a) depicts a point attached to a rigid body frame moving at an angular velocity denoted ω ∈ R 3 (a respresentation introduced in the previous section). If p(t) ∈ R 3 denotes the position of this point in time, then ṗ(t) ∈ R 3 denotes its time derivative, i.e. its velocity, then:

ṗ(t) = ω(t) × p(t) (2.6)
where • × • denotes the vector cross-product and ω(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:

ṗ = ω ∧ p (2.7)
where the time dependency was dropped for a clearer notation. Here, ω ∧ denotes the Hat operation, described in (2.8).

REMARK. The Hat/Vee map for so (3) The Hat map allows re-parameterizing a 3vector as a 3 × 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:

Hat: • ∧ : R 3 → R 3×3 Vee: • ∨ : R 3×3 → R 3 (2.8) ω ∧ =    0 -ω 3 ω 2 ω 3 0 -ω 1 -ω 2 ω 1 0    • ∨ -⇀ ↽ - • ∧ ω =    ω 1 ω 2 ω 3    (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 ω, p ∈ R 3 :

ω ∧ p = ω × p (2.10)
This same process can be applied for points attached to each of the body frame axes x(t), y(t), z(t) ∈ R 3 moving at the same angular velocity, yielding three velocity vectors: ẋ(t), ẏ(t), ż(t) ∈ R 3 that can be used to construct the angular displacement rate as a rotation matrix time derivative Ṙ:

Ṙ(t) = ẋ(t) ẏ(t) ż(t) (2.11) = ω(t) × x(t) ω(t) × y(t) ω(t) × z(t) (2.12)
which can in turn be factorized as:

Ṙ(t) = W (t) x(t) y(t) z(t) W = ω ∧ (2.13)
where ω ∧ denotes the skew-symmetric matrix of the angular velocity ω.

Given both R, Ṙ belong to SO(3) (and so they respect the properties presented in (2.1)), then it can be shown that 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 R 3 . Such that:

Hat ω ∧ : R 3 → so(3) Vee W ∨ : so(3) → R 3 (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 [•, •] satisfying the Jacobi identity [START_REF] Solà | A micro Lie theory for state estimation in robotics[END_REF].

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

Axis-angle representation as the rotation Matrix exponential parameterization ↑

Section 2.2.1.2 briefly introduces the Rodrigues formula that maps an axisangle representation into a rotation matrix. More recent works [START_REF] Grassia | Practical Parameterization of Rotations Using the Exponential Map[END_REF][Grassia, ] (1994)), [START_REF] Chirikjian | Rigid-Body Kinematics[END_REF]] (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 [START_REF] Murray | A Mathematical Introduction to Robotic Manipulation[END_REF].

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 φ 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:

Ṙ(t) = W (t)R(t) (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:

Ṙ(t) = W R(t) (2.16)
Thanks to some well-known results [START_REF] Hairer | Geometric numerical integration[END_REF] (see Section 3.2.3.1 in [START_REF] Lynch | Modern Robotics: Mechanics, Planning, and Control[END_REF]), this linear differential equation can be solved through:

R(t) = e W t R 0 (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 W = ω ∧ (2.18)
showing that it is possible to find an element W ∈ so(3) such that its exponential represents an element R ∈ SO(3).

Remembering that elements of so(3) can be mapped to R 3 through the Vee map (see (2.8)), then W = ω ∧ 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 ||ω|| around an axis ω for a unit of time. This vector ω 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 ω : so(3) → SO(3) log R : SO(3) → so(3) (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 ∈ so(3) may be referred to as an "angular velocity" because W = w ∧ and the following forms are treated as equivalent:

exp ω ≡ exp W log R = w ≡ log R = W (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 ω ∧ from its vector form ω [START_REF] Rodrigues | Des lois géométriques qui régissent les déplacements d'un système solide dans l'espace, et de la variation des coordonnées provenant de ces déplacements considérés indépendamment des causes qui peuvent les produire[END_REF], see (2.5).

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 matrix that goes from the universal frame to the body frame B:

revisited ↓ T B = R p 0 1×3 1 (2.21) with: p ∈ R 3 R ∈ SO(3) T B ∈ R 4×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 SE(3) and any element X ∈ SE(3) can be denoted as: X = (R, p).

Pose as a frame displacement ↑

Just as with the rotation matrices (see Section 2.2.1.1), the elements of 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 T S1 , T 12 , T 2 ∈ SE(3) (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 -1 S1 T S2 (2.23)
REMARK. Commutativity of SE(3) Just like SO(3), the SE(3) group is noncommutative (see (2.4)).

Pose derivative ↑

Similarly to how the angular velocity relates to a rate of change in the orientation, linking the orientation manifold SO(3) to its tangent space 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 ν ∈ se(3) (see Section 3.3 of [Lynch et al., 2017]) and are composed of an angular component ω ∈ so(3) (a rotational velocity, that belongs to the Lie algebra of SO(3)) and a linear component v ∈ R 3 (analog to the linear velocity).

To see how ν relates to the pose time derivative Ẋ , it is possible to extend (2.15) for pose elements in SE(3):

revisited ↓ Ẋ (t) = H(t)X (t) (2.24)
such that Ẋ , X ∈ SE(3) are elements of the pose manifold and H ∈ 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 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 R 6 and R 4×4 as:

ν ∧ = ω ∧ v 0 1×3 0 • ∨ -⇀ ↽ - • ∧ ν = v ω (2.25) with: v =    v 1 v 2 v 3    ω =    ω 1 ω 2 ω 3    (2.26)
where ν ∧ ∈ R 4×4 and ν ∈ R 6 .

Screw motion as the pose matrix exponential parameterization ↑

Thanks to the Mozzy-Chasles theorem [START_REF] Chasles | Note sur les propriétés génerales du système de deux corps semblables entr'eux et placés d'une manière quelconque dans l'espace; et sur le déplacement fini ou infiniment petit d'un corps solide libre [A note on the general properties of a system of two similar bodies arbitrarily positioned in space; and on the finite or infinitely small displacement of an unconstrained solid body[END_REF]; [Davidson et al., 2004], the concepts from Euler's Rotation theorem can be extended to the more general rototranslational 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, on can start by revisiting (2.24), assuming H remains constant to simplify the expression Ẋ (t) = HX (t) (2.27) and utilize some known ordinary linear differential solutions [START_REF] Hairer | Geometric numerical integration[END_REF] (see Section 3.2.3.1 in [Lynch et al., 2017]):

X (t) = e Ht X 0 (2.28) where the exponential refers to the matrix operation and X 0 = 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:

X = e H H = ξ ∧ (2.29)
which shows that it is possible to map elements from the SE(3) manifold to elements of its tangent space se(3) through the exponential operation. This vector ξ is referred to as the exponential coordinates of X . Furthermore, the reciprocal operation (the logarithm map) allows going back and forth between both representations:

exp H : se(3) → SE(3) log X : SE(3) → se(3) (2.30)
For simplicity, the same notation showed in (2.20) is extended to (2.30). This means that H ∈ se(3) may be referred to as a "spatial twist" because H = ξ ∧ and the following expressions are treated as equivalent:

exp ξ ≡ exp H log X = ξ ≡ log X = H (2.31)
The following sections exploit the representations introduced previously to describe rigid body trajectories in space as time-varying pose elements in time.

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. O 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.

X 1 X 2 X 3 x y z ∆X 1 ∆X 2 X (s)
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 3 . 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) ∈ SE(3). A representation of such path is depicted in Figure 2.6 and can be parameterized as:

X (s) = R(s) p(s) 0 1×3 1 (2.32) with: p(s) ∈ R 3 R(s) ∈ SO(3) X (s) ∈ SE(3)
where R(s) does for the orientation what p(s) does for the position: it parameterizes an orientation path as a function of some parameter s.

Discretizing a pose path ↑

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

∆X k = X k,k+1 = X -1 k X k+1 (2.33)
subsequently, the pose at each discretized step can be retrieved through:

X k+1 = X k ∆X k (2.34)
Considering 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 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: 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:

∆X k = e S k ∆X k ∈ SE(3) S k ∈ se(3) (2.35) yielding: X k+1 = X k e S k (
X k+1 = ∆X ′ k X k ∆X ′ k = X k+1 X -1 k X k , X k+1 , ∆X ′ k ∈ SE(3) ∆X ′ k = e H k H k ∈ se(3) (2.37)
It can be shown that 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 ∆X ′ k refers to a pose variation expressed in the universal frame.

REMARK. World vs Body Twist

The relation between a body twist B H and a twist expressed in the universal frame S H is (see Section 5.1 of [Traversaro et al., 2019]): 2.38) This also constitutes the adjoint action map of SE(3) over se (3). One property of this map is that:

S H = X B B HX -1 B X B ∈ SE(3) H ∈ se(3) (
e X HX -1 = X e H X -

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. where 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 [START_REF] Lynch | Modern Robotics: Mechanics, Planning, and Control[END_REF]). In contrast, the right-multiplied version of this differential equation would be:

Ẋ (t) = X (t)S(t) (2.41)
where 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 [START_REF] Forster | On-Manifold Preintegration Theory for Fast and Accurate Visual-Inertial Navigation[END_REF]; [START_REF] Solà | A micro Lie theory for state estimation in robotics[END_REF] that is central to the linearization strategy that is presented in the following sections.

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 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, . . . , N, then:

X k+1 = X 0 ∆X 1 ∆X 2 . . . ∆X k (2.42)
that can be further generalized as:

X k+1 = X 0 k ∏ i=0 ∆X i ∆X i = X -1 k X k+1 (2.43)
Remembering (2.35), each pose increment can be replaced for its equivalent exponential coordinates:

X k+1 = X 0 k ∏ i=0 e S i S i = log(∆X i ) (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) ∈ [0, 1] s k = s(t k ) t k = k ∆t (2.
↓ X k+1 = X 0 k ∏ i=0 e ∆tV i V i = S i ∆t (2.47)
This formula shows one more application of the product of exponentials introduced by Brockett in [START_REF] Brockett | Robotic manipulators and the product of exponentials formula[END_REF] 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):

revisited ↓ X k+1 = X k e ∆tV k (2.48)

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 ∆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 X = R and ∆X = ∆R.

Integration on a Lie Manifold ↑

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).

Manifold

Tangent space 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 [START_REF] Solà | A micro Lie theory for state estimation in robotics[END_REF].

Relation between a Lie group and its algebra ↑

For a given Lie group G, there exists a tangent space at the identity that defines a Lie algebra 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:

exp : T e G → G log : G → T e G (2.49)
where T e G denotes the tangent space of G at the identity element e ∈ 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 se(3) (see Section 2.2.2.3).

Note that the exponential map surjectively covers the SO(3) manifold. This means that, for instance, given φ ∈ so (3), it yields a one to one correspondence to R ∈ SO(3) through the logarithm map as long as ||φ|| < π. This condition in turn extends for the angular component of an element of the tangent space of the pose manifold se(3) [START_REF] Forster | On-Manifold Preintegration Theory for Fast and Accurate Visual-Inertial Navigation[END_REF]; [Torres Alberto et al., 2022a].

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 [START_REF] Solà | A micro Lie theory for state estimation in robotics[END_REF]). 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 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 ∈ g, y ∈ G:

Ad : G × g → g (2.50) Ad y h = yhy -1 (2.51)
This constitutes the general definition of the operation defined in (2.38) for SE(3).

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

At the heart of this propagation scheme on a group element X ∈ G, is the choice of the push-pull function ψ. As previously introduced, a convenient choice for this is the logarithm and exponential maps [Lee, 2013] such that:

revisited ↓ ψ : G → g log(X ) → ξ ψ -1 : g → G e ξ → X (2.52)
where ξ represents the analogous variable in the tangent space.

As shown in [START_REF] Forster | On-Manifold Preintegration Theory for Fast and Accurate Visual-Inertial Navigation[END_REF] 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 [START_REF] Solà | A micro Lie theory for state estimation in robotics[END_REF], an infinitesimal variation in the manifold dξ can be approximated as:

e ξ+dξ ≈ e ξ e J R (ξ)dξ (2.53)
which, for small variations, is shown to be equivalent to these other approximations:

e ξ e dξ ≈ e ξ+J -1 R (ξ)dξ (2.54) log(e ξ e dξ ) ≈ ξ + J -1 R (ξ)dξ (2.55)
where J R maps variations in ξ to the right multiplicative increments in the exponential map. This is in fact the exponential map derivative dexp and J -1 R 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 -1 R 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 [START_REF] Forster | On-Manifold Preintegration Theory for Fast and Accurate Visual-Inertial Navigation[END_REF] analogous to (2.36) with a twist but in the tangent space:

ξ k+1 ≈ ξ k + ∆tJ -1 R (ξ k ) dξ k dt (2.56)
where dξ k dt = V ∨ and the corresponding element X k+1 can be retrieved through the pull:

X k+1 = ψ -1 (ξ k+1 ) (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 -1 R 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.

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:

Ẋ (t) = X (t)U(t) with X (t 0 ) = X 0 (2.58)
with X (t) ∈ G and U(t) ∈ g (as proposed in [Zanna, 1999]). It is possible to find a solution in the form:

X (t) = e Ω(t) Ω(t 0 ) = log(X 0 ) (for X (t 0 ) = X 0 ) (2.59)
where Ω(t) incorporates the cumulative effects of the infinitesimal element U(t) in time over the initial element X 0 . Note that X (t) and U(t) do not necessarily commute: this means that X 0 U 0 U 1 ̸ = X 0 U 1 U 0 (or for any other order for that matter). This is why Ω(t) cannot be found via a simple integration and why the Magnus expansion is required [START_REF] Hairer | Geometric numerical integration[END_REF].

A consequence of (2.59), is that Ẋ (t) = e Ω(t) dexp Ω(t) Ω ′ (t). One can see the resemblance of this expression with the derivative of the exponential map 4 at x:

d dt e x(t) = dexp x(t) x ′ (t) (2.60)
From which, based on (2.58), one can identify U(t) as

U(t) = dexp Ω(t) Ω ′ (t) (2.61)
where dexp : g × g → g. Then, one can retrieve Ω ′ (t) from ( 2.61) by inverting dexp:

Ω ′ = dexp -1 Ω U(t) = ∞ ∑ n=0 B n n! ad n Ω U(t) (2.62)
where B n are the Bernoulli numbers and ad is the adjoint operator defined a series of Lie brackets (see (2.65) for the case of se(3) and so( 3)):

ad n Ω = [Ω(t), [. . . , [Ω(t) n-times , U(t)]]] (2.63)
Actually, the main interest lies in finding Ω(t) that satisfies (2.58), which can be obtained through the repeated commutation and integration of 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 Ω(t) = t t o U(x)dx iff U is constant or commutes with its primitive (i.e. [U(t), U(t)dt] = 0). Otherwise, Ω(t) is an infinite series known as the Magnus expansion [Magnus, 1954]; [Blanes et al., 2008]:

Ω(t) = t 0 U(t)dt + 1 2 t 0 t 1 0 U(t 2 )dt 2 , U(t 1 ) + . . . ( 2
.64) 4 The derivative of the exponential map is often presented as d dt e x(t) = e x (1-e ad x ) ad x dx dt or, with a different notation but equivalently:

d dt e x(t) = dexp x(t) x ′ (t)

REMARK. The Lie bracket and the adjoint operator

Lie algebras such as g are equipped with the commutation operator:

[X, Y] = XY -Y X = ad X Y X, Y ∈ g (2.65)
This is also referred to as the adjoint action endomorphism, as it is a linear map onto itself such that ad g • : [g, g] → g. For the specific case of the algebras so(3) and se(3), is defined as [START_REF] Solà | A micro Lie theory for state estimation in robotics[END_REF]: for φ ∈ so(3): 3): (with a linear component ρ ∈ R 3 and angular component φ ∈ so( 3))

ad φ 1 φ 2 = [φ 1 , φ 2 ] = φ 1 × φ 2 = φ 1 ∧ φ 2 (2.66) with: ad φ 1 = φ 1 ∧ ∈ R 3×3 (2.67) for ξ = [ρ T , φ T ] T ∈ se(
ad ξ 1 ξ 2 = [ξ 1 , ξ 2 ] = φ 1 × ρ 2 + ρ 1 × φ 2 φ 1 × φ 2 (2.68) with: ad ξ 1 = φ 1 ∧ ρ 1 ∧ 0 φ 1 ∧ ∈ R 6×6 (2.69)
Note that ad φ 1 φ 2 ∈ so( 3) and ad ξ 1 ξ 2 ∈ se(3).

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 ≡ ME2). This allows approximating (2.62) in a linear form with respect to U(t):

Ω ′ (t) ≈ U(t) + [Ω(t), U(t)] 2 + [Ω(t), [Ω(t), U(t)]] 12 (2.70) Ω ′ (t) ≈ U(t) + ad Ω(t k ) U(t) 2 + ad 2 Ω(t k ) U(t) 12 (2.71) Ω ′ (t) ≈ KU(t) (2.72) with: K(t) = I + ad Ω(t k ) 2 + ad 2 Ω(t k ) 12 (2.73)
Notice that K allows approximating dexp -1 by truncating the series. In fact, K ≈ J -1 R 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.

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 derivatives5 :

X -1 Ẋ = ν ∧ ⇒ Ẋ = X ν ∧ X ∈ SE(3) ν ∈ se(3) (2.74) R -1 Ṙ = ω ∧ ⇒ Ṙ = Rω ∧ R ∈ SO(3) ω ∈ so(3) (2.75)
which, under the assumption that U(t) remains constant throughout an integration step (meaning U(t) = U, that is necessary for (2.36)), means that U is in fact the twist or the angular velocity applied, for each respective case.

Furthermore, noting that: log(X (t)) = log(e Ω(t) ) = Ω(t) (2.76) then it becomes evident that Ω(t) is in fact the desired logarithm of the pose ξ(t).

Using a first-order Taylor expansion of the discretized Ω(t):

Ω(t k+1 ) ≈ Ω(t k ) + ∆tΩ ′ (t k ) (2.77)
where

Ω ′ (t k ) can be replaced with (2.73) for U = ν k and Ω(t k ) = ξ k : ξ k+1 ≈ ξ k + ∆tK(t k )ν k (2.78)
In this expression, K only depends on Ω(t k ) = ξ 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.

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.

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.

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 (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 = Ax 0 + B(u 0 + u 1 + • • • + u k ) (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.

Discrete pose estimation in a horizon ↑

Assuming the pose X k = e ξ k 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 replaced with their chosen quantities ξ, ν ∈ se(3), respectively:

X k+1 = e ξ k+1 (2.81) with: ξ k+1 ≈ ξ 0 + ∆tK(t 0 )(ν 0 + ν 1 + . . . + ν k )
where ξ 0 = log(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.

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.

Simulation results ↑

Let the initial and target poses of a robot end-effector path be X i , X t ∈ 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 fp 0 || = 0.71m and ||ang(δ)|| = 0.94rad .

• For trajectory 2: ||p fp 0 || = 0.32m and ||ang(δ)|| = 0.90rad .

The rotational displacement (the norm of the angular component ang(δ

)) is com- puted from δ = log(X -1 0 X f ). (a) s(t) (b) ṡ(t) (c) s(t)
Figure 2.9: Normalized s(t) for a time-optimal trajectory generated with a trapezoidal profile (in velocity).

Defining s ∈ [0, 1] to discretize the path between both poses as: X * k = X 0 e s k δ yields a ground truth reference velocity twist

ν * k = log(X * -1 k X * k+1 ) ∆t .
This implies that the reference trajectory can be reconstructed with:

X k+1 = X k e ∆tν * k (2.82) where: log(X k ) = ξ k (2.83)
The Ruckig library [Berscheid et al., 2021a] is used6 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 ± 6.1mm and 0.048 ± 0.27 • (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.

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.

Contents

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-ofthe-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.

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 [START_REF] Haddadin | Physical Human-Robot Interaction[END_REF], which are often analyzed from the application point of view to reduce complexity [START_REF] Gualtieri | Emerging research fields in safety and ergonomics in industrial collaborative robotics: A systematic literature review[END_REF]]. 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. [START_REF] Liu | Generalized hierarchical control[END_REF]; [START_REF] Salini | Synthesis of Complex Humanoid Whole-Body Behavior: a Focus on Sequencing and Tasks Transitions[END_REF] 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 [START_REF] Kurniawati | Global Motion Planning under Uncertain Motion, Sensing, and Environment Map[END_REF]. Nevertheless, such strategy is inefficient due to two reasons: for one, it might get computationally nonviable 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 [START_REF] Rubrecht | Constraints Compliant Control: constraints compatibility and the displaced configuration approach[END_REF]; [START_REF] Prete | Joint Position and Velocity Bounds in Discrete-Time Acceleration/Torque Control of Robot Manipulators[END_REF] and even consider their effects in the workspace motions [START_REF] Rubrecht | Motion safety and constraints compatibility for multibody robots[END_REF]. Some of these strategies resort to linking workspace and joint constraints with "interfacing" quantities like the minimal motion stopping time [START_REF] Joseph | Online velocity constraint adaptation for safe and efficient human-robot workspace sharing[END_REF]. 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 revisited ↓ 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 intervens 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. adopting a strategy that performs optimizations on a receding horizon, effectively providing computationally efficient solutions that often yield approximately global optimal strategies.

Control Architecture Proposal ↑

The traditional control architecture1 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 approriate 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 asses 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

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 planner could define a task that exploits the ergonomics of the HRI [START_REF] Spaa | Predicting and Optimizing Ergonomics in Physical Human-Robot Cooperation Tasks[END_REF] 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 [START_REF] Du | Active collision avoidance for human-robot interaction with ukf, expert system, and artificial potential field method[END_REF]; [START_REF] Mohammed | Active collision avoidance for human-robot collaboration driven by vision sensors[END_REF]; [START_REF] Safeea | On-line collision avoidance for collaborative robot manipulators by adjusting off-line generated paths: An industrial use case[END_REF] 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 [START_REF] Halme | Review of vision-based safety systems for humanrobot collaboration[END_REF], 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 [START_REF] Krämer | Model predictive control of a collaborative manipulator considering dynamic obstacles[END_REF]; [START_REF] Spahn | Dynamic Optimization Fabrics for Motion Generation[END_REF] or predicting human motions in the workspace [START_REF] Bajcsy | A Robust Control Framework for Human Motion Prediction[END_REF]; [START_REF] Weitschat | Safe and Efficient Human-Robot Collaboration Part II: Optimal Generalized Human-in-the-Loop Real-Time Motion Generation[END_REF].

Added to this problem is the fact that environmental conditions are susceptible to sensor noise and sudden changes, though to circumvent this problem, some special considerations have been proposed that avoid discontinuous changes that could result in instantly incompatible problems [START_REF] Tan | Minimization of the rate of change in torques during motion and force control under discontinuous constraints[END_REF].

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 [START_REF] Benzi | An optimization approach for a robust and flexible control in collaborative applications[END_REF]; [START_REF] Wang | Vision-guided active collision avoidance for human-robot collaborations[END_REF]; [START_REF] Scalera | Enhancing fluency and productivity in human-robot collaboration through online scaling of dynamic safety zones[END_REF].

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 function subject to linear constraints. While similar architecture approaches have been employed in humanoid control [START_REF] Wieber | Trajectory Free Linear Model Predictive Control for Stable Walking in the Presence of Strong Perturbations[END_REF]; [START_REF] Lober | Task Feasibility Maximization Using Model-Free Policy Search and Model-Based Whole-Body Control[END_REF], 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 convex 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 workcell 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.

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. Oftentimes, 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 2 x ∈ R n , a tracking task can be described as an optimization problem with a cost function f (x) : R n → R such that [START_REF] Joseph | An energetic approach to safety in robotic manipulation[END_REF]:

x opt = arg min x f (x) subject to h(x) ≤ 0 (3.1)
where x 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 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) ∈ R n , then the unconstrained optimal control input x 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) ∈ 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 X (t) ∈ 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 [START_REF] Salini | Synthesis of Complex Humanoid Whole-Body Behavior: a Focus on Sequencing and Tasks Transitions[END_REF] for humanoids:

x opt = arg min x ||Ex -b|| 2 (3.2) subject to A u ≤ Ax ≤ A u (3.3)
where (3.2) denotes the task space trajectory tracking problem, that can be efficiently solved as a Quadratic Program (QP) 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.

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 tranformation 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) 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 ∈ 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 rototranslational displacement from the world frame. This pose is described by a homogeneous transformation matrix 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:

revisited ↑ X WB (q) = R WB (q) p(q) 0 1×3 1 X ∈ SE(3) (3.4)
This allows a more formal definition of FK and IK as:

FK(q) : R n → SE(3) IK(X ) : SE(3) → R n FK(q) = X WB IK(X ) = {q ∈ R n , X ∈ SE(3) | FK(q) = X } (3.5)
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 q ∈ R n as [START_REF] Lynch | Modern Robotics: Mechanics, Planning, and Control[END_REF]:

revisited ↓ W ν = W J(q) q J ∈ R 6×n q, q ∈ R n ν ∈ se(3) (3.6)
where ν 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 ν. 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 ν are defined for q ∈ Q and q ∈ Q such that:

revisited ↓ Q = {q ∈ R n | q m ≤ q ≤ q M } Q = { q ∈ R n | qm ≤ q ≤ qM } (3.7)
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).

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 q and that the current robot configuration q k can be measured.

A given a reference pose trajectory X r (t) ∈ SE(3), once discretized, yields a reference pose and twist (see Section 2.3) to be achieved at the current time

step k: X r k = X r (t k ) and ν r k = ν r (t k ) ∈ se(3) (with t k = k∆t and k ∈ 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:

qopt = arg min q ||J(q) q -ν r || 2 (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 X (q) using FK and then compute the ideal reference error twist in the body frame B e ∈ se(3) required to arrive at X r as:

B e = log(X -1 X r ) (3.9)
Then, (3.8) can be reformulated with:

qopt = arg min q || B J(q) q -B ν * || 2 (3.10) B ν * = K p B e + B ν r (3.11)
where ν * 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 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 [START_REF] Joseph | An energetic approach to safety in robotic manipulation[END_REF] that can be chosen depending on the application, ranging from projected mass minimization for safety purposes [START_REF] Joseph | Online minimization of the projected mass of a robot for safe workspace sharing with a human[END_REF] 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:

qopt = arg min q || B J(q) q -B ν * || 2 + w reg || qreg -q|| 2 2 (3.12) B ν * = K p B e + B ν r (3.13) qreg = K q (q reg -q) (3.14) q reg = (q M -q m ) 2 (3.15)
where K q is a positive proportional term; w reg ≪ 1 and qreg are defined as in [START_REF] Joseph | Online velocity constraint adaptation for safe and efficient human-robot workspace sharing[END_REF], to maintain a configuration 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.

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, Q can be directly included into the linear constraints as:

qm ≤ I n q ≤ qM (3.16)
where 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 q. For instance, the set Q (from (3.7)) and Q (for the joint accelerations):

Q = { q ∈ R n | qm ≤ q ≤ qM } (3.17)
can be reformulated through a first order taylor expansion as:

C q|q = q ∈ R n | q ∈ q m -q k ∆t , q M -q k ∆t (3.18) C q| q = { q ∈ R n | q ∈ [ qk + qm ∆t, qk + qM ∆t]} (3.19)
Note that the intersection of C q|q , C q| q may create a feasibility problem [START_REF] Rubrecht | Constraints Compliant Control: constraints compatibility and the displaced configuration approach[END_REF]. Then expressed as a linear function of q:

q m -q k ∆t qk + qm ∆t ≤ I n I n q ≤ q M -q k ∆t qk + qM ∆t (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 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.

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 [START_REF] Carpentier | The Pinocchio C++ library, a fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives[END_REF]. 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 [START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF]).

• 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:

ν ∈ se(3), X = (R, p) ∈ SE(3) w ∈ so(3), R ∈ SO(3) Ad : SE(3) × se(3) → se(3) Ad : SO(3) × so(3) → so(3) Ad X ν = X νX -1 Ad R w = RwR -1 = Rw Ad X = R 0 p ∧ R R Ad R = R (3.21)
Changing a Twist From the Local Frame to the World Frame Given some frame C described by a X WC = (R WC , p) and some twist expressed in this frame C ν, it is possible to change the twist coordinates frame with:

W ν = Ad X WC C ν (3.22) Ad X WC = R WC 0 p ∧ R WC R WC p ∧ =    0 -p 3 p 2 p 3 0 -p 1 -p 2 p 1 0   
From Local to Local World-Aligned For a given body B described by a pose X WB = (R WB , p WB ), the transformation matrix goin from the world frame to the local world-aligned frame X WA can be obtained as:

X WA = X WB R T WB 0 0 1 = I 3 p WB 0 1 (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(X -1 k X 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 worldaligned 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 B ν on a body whose pose is described by a pose X (its roto-translation from the world frame), the same twist may be expressed in the world frame as:

S ν = Ad X B ν (3.24)
Then (3.13) in the world frame becomes:

S ν * = S e + S ν r e, ν ∈ se(3) S e = K p Ad X B e X ∈ SE(3) B e = log(X -1 k X r ) (3.25)
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:

S ν = S J(q) q J ∈ R 6×n q, q ∈ R n ν ∈ se(3) with S J(q) = Ad X B J (3.26)
where X ∈ SE(3) expresses transformation from the world to the Jacobian frame.

Constrained Motion Generation with Model Predictive Control ↑

The focus of this section is to propose an Model Predictive Control (MPC) implementation on the 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 [START_REF] Rawlings | Model Predictive Control: Theory, Computation, and Design[END_REF]; [START_REF] Borrelli | Predictive Control for Linear and Hybrid Systems[END_REF] and [START_REF] Rossiter | Model-Based Predictive Control: A Practical Approach[END_REF], for a more practical approach.

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 on the viability of the generated motions [START_REF] Rubrecht | Constraints Compliant Control: constraints compatibility and the displaced configuration approach[END_REF] 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 [START_REF] Kress-Gazit | Formalizing and guaranteeing human-robot interaction[END_REF] 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 [START_REF] Gold | Model Predictive Interaction Control for Robotic Manipulation Tasks[END_REF] 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 [START_REF] Kleff | High-Frequency Nonlinear Model Predictive Control of a Manipulator[END_REF].

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.

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 ∈ R d and input u ∈ 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):

revisited ↓ x n+1 = Ax n + Bu n A ∈ R d×d , B ∈ R d×m (3.27) y n = Cx n + Du n C ∈ R r×d , D ∈ R r×m (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: revisited ↓

x k+1|n = Ax k|n + Bu k|n for k = 0, 1, . . . , h -1 (3.29) 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:

revisited ↓ X 0...h =       x k x k+1 . . . x k+h       U 0...h-1 =       u k u k+1 . . . u k+h-1       (3.30) X = X 1...h X = X 0...h-1 U = U 0...h-1
This allows compressing (3.29) to a single algebraic formula:

revisited ↓ X = A ′ X + B ′ U (3.31)
where X and U respectively correspond to the future states and control inputs of the system. Furthermore, A ′ , B ′ are matrices constructed from A, B as:

A ′ =          A A A . . . A          h times B ′ =          B B B . . . B          h times (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 1 2 U T RU + 1 2 X T PX + 1 2 x h T P T x h (3.33) subject to: revisited ↓ X = A ′ X + B ′ U (3.34) revisited ↓ C x m ≤ C x x k ≤ C x M (3.35) revisited ↓ C u m ≤ C u u k ≤ C u M (3.36)
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 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)

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 X i ∈ SE(3) subject to a body twist ν(t) ∈ se(3), its discretized pose trajectory X k = X (t) t=k∆t ∈ SE(3) can be described as: 3.37) This relation can be used incrementally to compute the desired twists at each time instant given some tracking error [START_REF] Lynch | Modern Robotics: Mechanics, Planning, and Control[END_REF]. 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 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).

revisited ↑ X k = X i e ν 0 ∆t e ν 1 ∆t . . . e ν k-1 ∆t = X i ∏ k-1 n=0 e ν n ∆t (
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 [START_REF] Forster | On-Manifold Preintegration Theory for Fast and Accurate Visual-Inertial Navigation[END_REF]; [Saccon et al., 2013]).

Consider the optimization problem of a variable belonging to a smooth manifold X ∈ M; hence there exists a map f : M → R n (with its inverse denoted f -1 ), transporting elements from the solution space into an optimization space f : X → x, allowing the definition of the optimization problem with cost function C as:

x opt = arg min x∈R n C(x) (3.38) X opt = f -1 (x opt ) (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 ψ, ψ -1 .

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

revisited ↑ ↓ log : SE(3) → se(3) X → ξ exp : se(3) → SE(3) ξ → X (3.40)
The log function6 relates the Lie group SE(3) to its Lie algebra se(3), its tangent space, at the origin. As explained in Section 2.4.1, the bijectivity properties are maintained 7 for |φ| < π (where φ is the angular component of ξ).

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 6vector 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:

revisited ↑ ↓ X k+1 = X k e ∆tν k (3.41)
This is a first-order approximation, analogous to the one presented in [START_REF] Forster | On-Manifold Preintegration Theory for Fast and Accurate Visual-Inertial Navigation[END_REF] for SO(3) and [START_REF] Solà | A micro Lie theory for state estimation in robotics[END_REF] (for more general manifolds).

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

revisited ↑ ↓ ξ k+1 = log(e ξ k e ∆tν k ) ≈ ξ k + dlog ξ k ∆tν k (3.42)
where dlog ξ k is the right-trivialized log map derivative 8 at ξ k , that relates additive increments in the tangent space at the origin to the right-multiplied increments in the pose space. In layman's terms9 , 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).

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 X t ∈ SE(3). In order to embed this in the MPC cost function, a distance metric formulated in the tangent space is necessary. In fact, 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 = || ξ k log(X k ) -ξ t log(X t ) || 2 2 (3.43)
The last challenge to overcome is that there exist multiple paths that connect two extreme poses X i , X t ∈ SE(3). Given some α(t) ∈ [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 δ:

X k = X i e α k δ δ = log(X -1 i X t ) (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):

revisited ↓ ψ(x) = log(X -1 l x) ψ -1 (x) = X l exp(x) (3.45) X l = X t
where X l denotes the lift pose. This way, the optimization can be "directed" from ψ(X i ) → ψ(X t ), which yields the geodesic. The choice of X l = X t stems from the application: it allows the optimization to always converge to the origin in the tangent space ξ I because: ψ(X t ) = ξ I . In fact, the lift function in (3.45) implies that ψ is in the tangent space at X -1 l .

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:

revisited ↓ x k = ψ(X k ) x t = ψ(X t ) u k = ν k (3.46)
X k = e (1-α k )ξ 0 +α k ξ t for some α(t) ∈ [0, 1].
where x t is lifted the target state; x k = ξ k is the lifted state and ν 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:

revisited ↓ X * , U * = arg min x,u ||X -X t || 2 2 + γ||U|| 2 2 (3.47) revisited ↓ X T t = [ψ(X t ) T . . . ψ(X t ) T h-1 times ] T (3.48) s.t: x k+1 = Ax k + Bu k for k = 0, . . . , h -1 u m ≤ u k ≤ u M for k = 0, . . . , h -1 um ≤ u k+1 -u k ∆t ≤ uM for k = 0, . . . , h -2 (3.49)
where γ designates a weighting term for the input throughout the horizon and ψ(X t ) = ξ t corresponds to the lifted target pose while X t is its vector version throughout the horizon. The input constraints (for simplicity in the formulation, often defined as u M = -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:

revisited ↓ A = I 6 B = dlog ξ i ∆ h t (3.50)
where A is an identity matrix and B reflects the tangent space linearization at the initial robot pose, as ξ i = ψ(X i ). This constitutes the central relation that enables tangent space linear MPC. Finally, in order to recover the desired poses X des k from X * :

X des k = ψ -1 (x * k ) (3.51)
This constitutes the linear MPC formulation that allows full pose and constrained trajectory generation in an online manner.

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]; [START_REF] Raiola | Development of a Safety-and Energy-Aware Impedance Controller for Collaborative Robots[END_REF]; [START_REF] Meguenani | Control of robots sharing their workspace with humans: An energetic approach to safety[END_REF], which, although more complex in implementation, allow for more flexible solutions. revisited ↑ ↓ 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 taskspace online trajectory re-planning over a receding horizon (MPC block) and a highfrequency 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. 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 requirements is directly addressed by the control architecture proposed in Section 3.1.1 and this experiment showcases one implementation of this feature.

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 reacting to dynamic events implied in the considered collaborative scenarios. Instead one needs to consider online re-planning at the task level.

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

As a consequence, this chapters implements a hierarchical cascade of closed-loops controllers in a similar way to [START_REF] Lober | Task Feasibility Maximization Using Model-Free Policy Search and Model-Based Whole-Body Control[END_REF], combining both online trajectory re-planning over a receding horizon and task space-control in two stages: a highlevel safety-aware operational space MPC and a high-frequency lower-level task-tojoint-space controller.

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

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

• often use non-linear solvers [START_REF] Kleff | High-Frequency Nonlinear Model Predictive Control of a Manipulator[END_REF]; [Sathya et al., 2020] which may not be appropriate for frequent re-planning;

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

• mostly disregard orientation.

By leveraging the relation between the pose space (the Lie Group SE(3)) and its tangents spaces (in the Lie algebras se( 3)) [Saccon et al., 2013]; [Torres Alberto et al., 2022a]; [START_REF] Forster | On-Manifold Preintegration Theory for Fast and Accurate Visual-Inertial Navigation[END_REF], this work proposes a Linear MPC capable of estimating the evolution of the position and orientation with fast Quadratic Program (QP) solvers.

Numerical Simulation ↑

To showcase the result of a task-space MPC employed for online re-planning, a numerical simulation was constructed for a simple Cartesian motion along one of the axis of the reference frame attached to the robot base (x in this case). While the experience might seem trivial, it emulates the operation of the MPC by re-planning at a constant rate. It was executed using the full implementation of the algorithm even though only the axis in question actually performs a motion (as intended by the design of the experiment).

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

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

Experimental Scenario & Safety Constraints ↑

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

2. adapt to changing constraints online.

To do so, a 7-DoF Panda robot from Franka Emika is requested to move to four different targets. These targets are not known in advance by the MPC but rather given on-the-fly. For this experiment, a cascade controller as shown in Figure 3.5 is implemented. This schema also shows the working frequency of each controller and the feedback flow. The MPC implemented uses OSQP [Stellato et al., 2020] as a QP solver. The joint velocity controller used for this experiments is available online 10 and uses qpOASES [Ferreau et al., 2014]. The robot model is computed using the Pinocchio library [START_REF] Carpentier | The Pinocchio C++ library, a fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives[END_REF]. The robot control architecture is implemented using the Robot Operating System (ROS) framework and ran in real-time at a frequency of 1kHz using the franka_ros library. 10 The code used for the controller is hosted at gitlab.inria.fr/auctus-team/components/robots/panda/panda_qp_control. That said, future versions of the controller will be integrated in the qontrol library at gitlab.inria.fr/auctus-team/components/control/qontrol. When solving the MPC problem for a new target pose, computation times vary a lot depending on the amount of steps and constraints in the horizon. Moreover, the first solving step (cold start) always takes more time. For a typical horizon of h = 10, ∆t h = 50ms OSQP solves the problem in under 10ms. The solution of the previous QP is used as a first guess for the next one (hot start). By assuming that the optimal solution between two resolutions closely resembles the previous one, the resolution process can be accelerated. This leads to computation times under 1ms with a warm start.

Safety constraints

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

The constraint modulation in this scenario is chosen for its simplicity but it showcases how to potentially integrate arbitrarily complex safety behaviours as velocity modulation functions, such as, for example, dynamic risk assesment strategies based on probabilistic scenarios [Z. Liu et al., 2020]; [Mayyas et al., 2020], the robot stopping distance [START_REF] Joseph | Online velocity constraint adaptation for safe and efficient human-robot workspace sharing[END_REF] or damage-based maximum velocities [Mansfeld et al., 2017].

The distance between the end-effector and the human is calculated as d h = ||p eep h || 2 (where p ee , p h respectively refer to the end-effector and the human position). A simple piece-wise function is employed for each component of u M designated u M,i for i = 1, . . . , 6 (to modulate both the linear and angular velocities):

u M,i (d h ) =      V m d h ≤ d m d h -d m d M -d m V M d m < d h < d M V M d h ≥ d M (3.52)
that has user defined extreme values for the minimal and maximal distances d m , d M and velocities V m , V M ; all greater than zero.

This function ensures a linear relation between maximum velocity and the distance inside the distance interval; outside of this range u M is assigned either the minimal or the maximum velocity. The minimum velocity V m is chosen to be close to zero to completely stop the robot motion when the distance is below the threshold d m .

The robot Cartesian velocity is constrained using (3.52). The distance between the operator and the robot is determined by a Hokuyo Laser range finder placed at the robot base, as shown in Figures 3.4

V m,lin = 0.01m.s -1 V m,ang = 0.01s -1 d m = 0.2m V M,lin = 1m.s -1 V M,ang = 1.5s -1 d M = 1m (3.53)
In an industrial scenario, these values would be determined by the type of humanrobot collaboration and technical specification used, such as the ISO TS 15066.

Results ↑

This section shows the results from periodically requesting the robot to move to a different pose. Figure 3.9 presents the experiments performed on the Panda robot, including 1. the error between the current robot pose and the target pose;

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

Result 1) shows the error between the current position p k ∈ R 3 and orientation of the robot R k ∈ SO(3) and the target ones. There is a spike every time a new pose is requested. It is important to highlight that this is not the resulting tracking error of following the trajectory planned with the MPC but rather the "distance" between the current pose and the objective.

The following formula was used to compute this error:

e = p k -p t φ = log(R -1 k R t ) (3.54)
where p t , R t denote the target position and orientation, respectively; e ∈ R 3 reflects the position distance and φ ∈ so( 3) is the orientation error.

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

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

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

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

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

Conclusion ↑

This Chapter proposes an efficient linear Model Predictive Control (MPC) approach that can deal with the online planning of SE(3) motion in task space. The main features of the proposed control approach lie in its ability to generate optimal Cartesian motion which dynamically accounts both for targets updated on-the-fly and evolving motion constraints. This receding horizon approach endows the robot with the ability to interactively adapt to its environment. More particularly the safety of a human operator sharing its workspace with the robot can be accounted for through the sensor-based adaptation of maximum velocity constraints. While the robot moves, a laser sensor is used to capture the minimal distance to a human and modulate the maximum velocity (shown in red shade). The error curves show the distance between the current position and orientation (p k , R k ) and the preset target poses (p t , R t ). The linear error is e = p tp k , while the angular error is computed as φ = log(R -1 k R t ). The MPC (T=300ms, h = 10, ∆ h = 30ms, f MPC = 50Hz) modulates the body twist ν = [v T , ω T ] T respecting the safe limits imposed even when they become so low that the robot stops moving.

The chapter proposes a velocity-based MPC formulation for acceleration-bounded trajectory planning. This allows formulating constraints up to the acceleration level but presents two big limitations: the resulting trajectories is not smooth on the acceleration level; the MPC cost function is very sensitive to the regularisation task.

The next chapters of this work will focus on a acceleration-based formulation that allows incorporating jerk bounds to obtain smoother trajectories. At the same time, they focus on formulating the linear constraints in the MPC for the Cartesian pose state, to limit the effective position and orientation space. Furthermore, the current experiment was implemented with explicit equality constraints in the Quadratic Program (QP) solver. This adds a computation complexity to the solver that results in slower convergence. The following chapters show an improved version that employs a compressed matricial, speeding up resolution times.

Figure 3.10: QR code for the video of the velocity modulation experiment. On PDF file readers, it is also a clickable hyperlink.

Introduction ↑

The following sections present the rationale behind the task adaptation in interactive and collaborative scenarios from a dynamics modeling point of view. The proposal is to make a slight adjustment to the control architecture presented in Chapter 3, exploiting a more efficient mathematical formulation of the constrained motion replanning problem. Section 4.2 focuses on the modeling and control assumptions of the robot. In order to start considering motion dynamics for collaborative tasks, these aspects must also be embedded in the robot model. With this objective in mind, the kinematics-based approach presented before is extended to control a robot through the joint torques. This model is then used for task-space tracking via a Quadratic Program (QP).

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

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

Collaborative robotics has increasingly gained interest over the last decades. From an application point of view, it crystallises early expectations [START_REF] Makinson | Research and development prototype for machine augmentation of human strength and endurance: Hardiman I project[END_REF]; [J Edward [START_REF] Colgate | Intelligent assist devices in industrial applications: a review[END_REF] around the improvement of human working conditions [START_REF] Maurice | Human-oriented design of collaborative robots[END_REF]; [START_REF] Schoose | Evolution of the biomechanical dimension of the professional gestures of grinders when using a collaborative robot[END_REF]. From a research stand point, it raises several essential questions, for example regarding the prediction of human motion [START_REF] Mainprice | Predicting human reaching motion in collaborative tasks using Inverse Optimal Control and iterative replanning[END_REF] or the underlying cognitive principles behind the concept of collaboration [START_REF] Chen | Handbook Of Human Factors And Ergonomics[END_REF]. Among these research questions, the more general one of controlling robots in dynamic environments 1 is a recurring yet unsolved problem. A large amount of literature has been dedicated to this problem but the most recent works in the domain of collaborative robotics have focused on human avoidance [Eckhoff et al., 2022]; [START_REF] Merckaert | Real-time motion control of robotic manipulators for safe human-robot coexistence[END_REF]. Another essential feature when considering dynamic environments is the general ability to autonomously, continuously and smoothly adapt to unplanned task updates. While Chapter 3 focuses on a control approach for continuous task adaptation, this Chapter proposes an extension that more comprehensively tackles the problem to achieve smoother reactive behaviors from the robot.

1 as opposed to static ones where robots are physically separated from anything that could provoke a change in either the task or the surrounding objects during execution

Robots in Changing Environments for Dynamic Tasks ↑

Given the existence of mechanisms for updating the robot current state in relation to its environment and task objective (i.e. perception) in real-time, the most straightforward approach to grant robots with the ability to adapt is through real-time adjustments in their movement planning to reach the desired goal [START_REF] Palleschi | Fast and Safe Trajectory Planning: Solving the Cobot Performance/Safety Trade-Off in Human-Robot Shared Environments[END_REF].

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

Indeed, it is unlikely for the entire pre-computed trajectory to be successfully executed in the presence of unforeseen changes to the task. Additionally, despite advancements in motion planning [START_REF] Coleman | Reducing the barrier to entry of complex robotic software: a moveit! case study[END_REF] and optimal trajectory computation [START_REF] Pham | A New Approach to Time-Optimal Path Parameterization Based on Reachability Analysis[END_REF], the online adjustment of both the path and the time profile throughout the entire motion remains an extremely challenging problem [START_REF] Palleschi | Fast and Safe Trajectory Planning: Solving the Cobot Performance/Safety Trade-Off in Human-Robot Shared Environments[END_REF]. This difficulty becomes even more pronounced when the planning approach involves computing a trajectory for control inputs.

Re-planning over a receding horizon presents a viable alternative [START_REF] Ghazaei Ardakani | Model Predictive Control for Real-Time Point-to-Point Trajectory Generation[END_REF] to ensure online computational feasibility of the control problem while striving for optimal task performance. This approach represents a compromise between methods that compute an optimal trajectory once offline and those that adapt to environmental changes without fully revising the whole trajectory [START_REF] Joseph | Online velocity constraint adaptation for safe and efficient human-robot workspace sharing[END_REF].

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

As a consequence, situations may arise where emergency stops become the sole recourse to prevent accidents. To circumvent such control exceptions, controllers need to be designed as optimization problems that explicitly incorporate constraints in the resolution [M. [START_REF] Liu | Generalized hierarchical control[END_REF]]. MPC appears as a good candidate due to its capability to perform optimal motion re-planning based on the current system state (i.e. in closed-loop fashion) while taking into account some constraints. In fact reasoning over a receding horizon not only achieves local optimality but also implicitly ensures compatibility among the set of constraints. This approach is more likely to succeed compared to methods that attempt to address constraint compatibility offline [START_REF] Rubrecht | Constraints Compliant Control: constraints compatibility and the displaced configuration approach[END_REF]; [START_REF] Meguenani | Safe control of robotic manipulators in dynamic contexts[END_REF]; [START_REF] Prete | Joint Position and Velocity Bounds in Discrete-Time Acceleration/Torque Control of Robot Manipulators[END_REF].

Assuming that MPC can keep up with the robot control rate [D. [START_REF] Kouzoupis | Towards proper assessment of QP algorithms for embedded model predictive control[END_REF]; [START_REF] Kouzoupis | Recent advances in quadratic programming algorithms for nonlinear model predictive control[END_REF], one can rely on a control architecture where the joint velocity or torque control input horizon is re-computed at each control time step and fed to the robot, based on a given operational space target. revisited ↑ ↓ 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.

While Chapter 3 proposed a kinematic approach to the online re-planning problematic, the interactive nature of collaborative tasks enforce the need for forces to be considered in the architecture. Yet, to do so, this approach must be extended to account for the dynamics. This is proposed in [START_REF] Kleff | High-Frequency Nonlinear Model Predictive Control of a Manipulator[END_REF] where MPC is performed through Differential Dynamic Programming which can be solved efficiently [Mastalli et al., 2020] at each control step over a receding horizon. This approach couples two underlying sub-problems and solves them together: 1) the computation of an operational space trajectory towards the target and 2) the resolution of the operational to joint space mapping. Even though constraints are implemented in the cost function and cannot be formally guaranteed, this approach presents the advantage of considering joint and actuation constraints directly at the MPC problem level. As a consequence, except for the joint torque servoing performed at the actuation level, no inner control loop is required.

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

Control Architecture ↑

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

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

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

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

Furthermore, the asynchronous design choice of the proposed architecture in Chapter 3 is a workaround to the heavy computing requirements of motion planning. Nevertheless, when controlling a motion at the dynamics level, a faster and more reactive controller is desired. This directly connects to the second challenge: the computational demands of considering more complex models that account for dynamics. In fact, the faster the control architecture the better: the closed feedback loop is able to better adapt to what is happening at a higher frequency.

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

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

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

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

Acceleration-based Task-space Tracking ↑

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

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

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

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

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

Dynamic Robot Model ↑

The dynamical model of a manipulator is one that describes the interaction between the joint torques and external contact forces to produce a joint acceleration q ∈ R n (see the Forward Dynamics Section 8.5 of [START_REF] Lynch | Modern Robotics: Mechanics, Planning, and Control[END_REF] for the joint-space formulation and Section 3.3 of [Siciliano et al., 2008], for the task-space one): q = M(q) -1 (τ -J T (q) f -B(q, q)) B(q, q) = C(q, q) + G(q) (4.1)

where τ ∈ R n denotes the torques generated by each joint; J T (q) f corresponds to the joint torques resulting of applying an external wrench f ∈ R 6 ; M(q) ∈ R n×n corresponds to the joint space inertia matrix; and the B(q, q) ∈ R n term is referred to as the bias forces, comprised of the effects of the Coriolis and centrifugal forces in C(q, q) and the gravity in G(q).

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

revisited ↑ ↓ W ν = W J(q) q (4.2)
W ν = W J(q) q + W J(q, q) q (4.3)

and now q can be replaced with (4.1). The kinematic model assumes actuation limits on the joint configuration bounds and joint velocities:

revisited ↑ ↓ Q = {q ∈ R n | q m ≤ q ≤ q M } Q = { q ∈ R n | qm ≤ q ≤ qM } (4.4)
These need to be extended to account for the joint torques:

T = {τ ∈ R n | τ m ≤ τ ≤ τ M } (4.5)
which can be even further extended in case one needs to consider the torque variation limits:

Ṫ = { τ ∈ R n | τm ≤ τ ≤ τM } (4.6)
This would effectively impose varying limits on the joint jerks; as can be seen by taking the time derivative of (4.1).

Inverse Dynamics Controller as a Quadratic Program ↑

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

The desired motion for an end-effector is parameterized with reference pose trajectory X r (t) ∈ SE(3). Once discretized, this yields a reference pose, twist and twist derivative (also referred to as spatial acceleration) to be achieved at the current time step k:

X r k = X r (t k ), ν r k = ν r (t k ), νr k = νr (t k ) ∈ se(3) (with t k = k∆t and k ∈ N).
The controller presented here is similar to the one in [Joseph et al., 2018b]; [START_REF] Prete | Joint Position and Velocity Bounds in Discrete-Time Acceleration/Torque Control of Robot Manipulators[END_REF]. The objective is to devise a strategy to control manipulators based on joint torque. For this, one needs to link the desired Cartesian spatial acceleration from the reference trajectory with the joint torques. First, the spatial acceleration can be expressed as a function of the joint accelerations with (4.3) using the Jacobian and its derivative. Then, by exploiting the robot dynamics model (see Section 4.2.1), one can formulate the joint accelerations as a function of the joint torques. This enables a QP formulation (as in (3.2)) of the task-space tracking problem, just like the kinematic formulation in (3.8):

τ opt = arg min τ || B ν(τ) -B ν * k || 2 + f reg (τ) (4.7) B ν(τ) = B J(q k ) q(τ) + B J(q k , qk ) qk (4.8)
where the sub-index k indicates measured states of the robot (for instance, the Jacobian J is computed at the measured robot configuration q k ); and f reg defines a regularisation task used to optimize over the redundant Degree of Freedom (DoF)s of the robot and B ν * denotes the desired twist derivative expressed in the body frame (spatial acceleration).

Additionally, the term J(q k ) qk can be computed from the current measured state and be treated as a constant bias in the optimization. Thus, the spatial acceleration ν(τ) is a linear function of the optimization variable (the joint torque τ).

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

B ν * = K p B e + K d (ν r k -ν k ) + B νr (4.9) B e = log(X -1 k X r k ) (4.10)
where the reference spatial acceleration B νr acts as a feed forward term.

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

f reg (τ) = w reg ||M -1 (τ reg -τ)|| 2 2 (4.11)
with w reg ≪ 1, ensuring it will not affect the main tracking task; and τ reg is defined depending on the desired use of the redundant DoFs. The inclusion of the inertia matrix M helps modulate the importance of each joint according to the configuration.

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

q reg = (q M -q m ) 2 (4.13)
where K τ , K q are positive proportional terms and q M , q m respectively denote the upper and lower joint bounds.

Inverse Dynamics Constraints ↑

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

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

τ m ≤ I n τ ≤ τ M (4.14)
where I n denotes an n-sized identity matrix.

The kinematic constraints (3.7) need to be reformulated as a function of the control input. To achieve this, one can perform a taylor expansion of future joint configuration and velocities (assuming the current ones can be measured, i.e. q k , qk ∈ R n ):

q k+1 = q k + ∆t qk + ∆t 2 2 q(τ) (4.15) qk+1 = qk + ∆t q(τ) (4.16)
where q is explicitely dependent on the decision variable, the joint torques τ, as seen in ( 4.1). Then, knowing that q k+1 ∈ Q and qk+1 ∈ Q, one can formulate the kinematic constraints as:

q M ≥ q k + ∆t qk + ∆t 2 2 q(τ) q m ≤ q k + ∆t qk + ∆t 2 2 q(τ) qM ≥ qk + ∆t q(τ) qm ≤ qk + ∆t q(τ) (4.17)
where ∆t is chosen large enough to cover a few control periods, as a safe margin to avoid entering the limits [START_REF] Rubrecht | Constraints Compliant Control: constraints compatibility and the displaced configuration approach[END_REF]. These equations allow formulating the constraints as a function of the joint accelerations:

C τ|q = q(τ) ∈ R n | q(τ) ∈ 2 (q m -q k -qk ∆t) ∆t 2 , 2 (q M -q k -qk ∆t) ∆t 2 (4.18) C τ| q = q(τ) ∈ R n | q(τ) ∈ ( qm -qk ) ∆t , ( qM -qk ) ∆t (4.19)
which is possible thanks to the definition of q in (4.1), under the assumption that there are no external forces applied ( f = 0).

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

C τ| τ = {τ ∈ R n | τ ∈ [τ k + ∆t τm , τ k + ∆t τM ]} (4.20)

MPC-based motion re-planning over a receding horizon ↑

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

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

x n+1 = Ax n + Bu n A ∈ R d×d , B ∈ R d×m (4.21)
where A and B represent its state and input matrices whereas x and u are the state and input vectors. Smooth point to point pose motion generation can be achieved by generating trajectories at the acceleration level. Choosing the control input at the task level to be the acceleration, allows predicting the evolution of the end effector (system) state -pose and twist -over a short period of time to generate a jerkbounded motion.

Discretizing a receding time window in h time steps of duration T starting at time t n (total duration hT) yields the discretized time steps inside the horizon, defined as t k = t n + kT for k = 0, 1, . . . , h and one can obtain x(t k ) = x k|n , u(t k ) = u k|n .

One can formulate a linear receding horizon expression as:

revisited ↑ ↓ x k+1|n = Ax k|n + Bu k|n for k = 0, 1, . . . , h -1 (4.22)
Subsequent sections drop the n subscript to simplify the notation, making x k|n equivalent to x k .

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

revisited ↑ X = A ′ X + B ′ U (4.23)
where A ′ , B ′ are matrices constructed from A, B via the recursive propagation of (3.29) over the time horizon. Moreover, X and U respectively correspond to the future states and control inputs of the system, as described in:

revisited ↑ X 0...h =       x k x k+1 . . . x k+h       U 0...h-1 =       u k u k+1 . . . u k+h-1       (4.24) X = X 1...h X = X 0...h-1 U = U 0...h-1
From these propagated linear dynamics, one can formulate the problem of finding a trajectory over a gliding time horizon. In a generic way, this concerns the problem of generating a constrained and time discrete trajectory over the system state. Point to point trajectories in a system state minimize the distance between the starting and desired states of a system. This problem can be mathematically expressed as a the linear optimal control problem over a receding horizon as a Quadratic Program (QP) with the following cost function:

revisited ↑ ↓ min x,u X t -X 2 P + U 2 R (4.25) subject to: revisited ↑ X = A ′ X + B ′ U (4.26) revisited ↑ ↓ C x m ≤ C x x k ≤ C x M (4.27) revisited ↑ ↓ C u m ≤ C u u k ≤ C u M (4.28)
where X t is the target state x t vectorized over the horizon. This is an important detail and one of the original features of the proposed approach: no pre-interpolation or a priori trajectory generation is required. The resulting motion is solely shaped by the considered constraints on the admissible task space motions. This implicitly yields a time optimal trajectory over the horizon.

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

P =          P k P k+1 . . . P k+h-1 P T          h times R =          R k R k+1 . . . R k+h-1 R k+h          h times (4.29)
For a typical usage, the weights employed remain constant throughout the horizon and these matrices are defined as:

P k+i = I d×d for i = 0, . . . , h -1 (4.30) P T = I d×d w T (4.31) R k+i = I d×d γ for i = 0, . . . , h (4.32)
where w T , γ denote the weight for the terminal state and the control inputs in the horizon, while the weight of the remaining states is set to 1. The control input weight γ shown in (3.47) is the vector version that contains the weight values for each step (assuming they are not the same).

Finally, C x , C u allow formulating linear constraints with respect to the state and input, while x m , x M and u m , u M respectively designate the state and input bounds.

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

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

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

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

Reduced Formulation ↑

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

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

However, the key consideration is that this assumption allows formulating the future state in a horizon as a function of the initial state x 0 and the control horizon U: more explicitly, a function X(x 0 , U).

To do so, one may develop (3.29) for every state x k (for k = 1 . . . h) as a function of the initial state x 0 and the successive control inputs u 0 , u 1 . . . u h :

x 1 = Ax 0 + Bu 0 (4.33)

x 2 = Ax 1 + Bu 1 (4.34) = A(Ax 0 + Bu 0 ) + Bu 1 = A 2 x 0 + ABu 0 + Bu 1 x 3 = Ax 2 + Bu 2 (4.35) = A(A 2 x 0 + ABu 0 + Bu 1 ) + Bu 2 = A 3 x 0 + A 2 Bu 0 + ABu 1 + Bu 2 . . . x H = A H x 0 + A H-1 Bu 0 + A H-2 Bu 1 . . . + ABu h-2 + Bu h-1 (4.36)
arriving at a matricial form:

revisited ↓          x 1 x 2 x 3 . . . x h          X =          B AB B A 2 B AB B . . . A h-1 B A h-2 B . . . AB B          Â          u 0 u 1 u 2 . . . u h-1          U +          A A 2 A 3 . . . A h          B x 0 (4.37)
that can be synthesized into a simple algebraic expression:

X(x 0 , U) = ÂU + Bx 0 (4.38) that can be directly embedded into (3.47): min u X t -X(x 0 , U)

2 P + U 2 R (4.39)
but optimizing only over the control inputs u. Nevertheless, it can still be subject to linear constraints:

revisited ↑ ↓ C x m ≤ C x x k ≤ C x M (4.40) revisited ↑ ↓ C u m ≤ C u u k ≤ C u M (4.41) C um ≤ C u uk ≤ C uM with: uk = (u k+1 -u k ) T (4.42)
where the control input constraints in (4.41) are kept the same as in (3.35) and (3.36). Meanwhile, the state constraints need to be expressed with respect to the optimization variable with (4.38). In the case of simple state bounds (where C x is an identity matrix), it can be done straightforwardly with:

X m -Bx 0 ≤ ÂU ≤ X M -Bx 0 (4.43)
where the term Bx 0 can be treated as constant, because x 0 is the measured initial state of the system (at the start of the horizon) and:

X m =       x m|k+1 x m|k+2 . . . x m|k+h       X M =       x M|k+1 x M|k+2 . . . x M|k+h       (4.44)
express the limits for the state at each discretized step in the horizon. Here, they are chosen constant to coincide with those in (3.35), but the expression can accommodate potentially evolving limits.

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

Dynamics-based MPC in the Tangent Space ↑

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

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

revisited ↑ ↓ X k+1 = X k e Tν k + T 2 2 νk (4.45) revisited ↓ ν k+1 = ν k + T νk (4.46)
where (4.46) is added to relate the twist to a spatial acceleration ν integration step for the same time period T. The objective is to include the pose and the twist into the state x of the system, and use the spatial acceleration as a decision variable or control input u. This way, (4.45) and ( 4.46) can be expressed in the form of (3.27) to be used for linear MPC with (4.39).

The term e Tν k in (4.45) is the equivalent homogeneous matrix to a floating body moving at ν k for a duration of T. It is computed using the matrix exponential, that surjectively maps the Lie algebra se( 3) onto the Lie group SE(3). Its inverse operation is known as the logarithmic map.

This relation can be exploited to represent X in a vector form. In fact, given some initial pose X 0 , one can devise a function ψ (referred to as the lift function) that maps X k to a vector ξ k . The general principles behind this approach are further explained in Section 2.4 or, more briefly, in [START_REF] Forster | On-Manifold Preintegration Theory for Fast and Accurate Visual-Inertial Navigation[END_REF]; [START_REF] Solà | A micro Lie theory for state estimation in robotics[END_REF]; [Torres Alberto et al., 2022a]. Such function is defined as:

revisited ↑ ψ(X k ) = log(X -1 0 X k ) ψ -1 (ξ k ) = X 0 e ξ k (4.47) revisited ↑ ψ : SE(3) → se(3) log(X -1 0 X k ) → ξ k ψ -1 : se(3) → SE(3) X 0 e ξ k → X k (4.48)
The lift function maps elements from the SE(3) manifold M to its tangent space at X 0 : ψ : M → T X 0 M. Notice that the roto-translational displacement ∆X k ∈ SE(3) (or equivalent homogeneous matrix) needed to move from X 0 to X k can be computed with ∆X k = e ψ(X k ) . The function ψ -1 is referred to as the retract function.

With the MPC being formulated in se( 3), the lift operator ψ is used to map the current state and the desired pose into se(3) to perform the optimization. Meanwhile the retract operator ψ -1 is used to map the computed trajectory back to SE(3). This enables directly embedding the lifted pose ξ = ψ(X ) into the discretized state of the system x, along with the body twist ν. Then, choosing the spatial acceleration ν as the input:

revisited ↑ ↓ x k = ξ k ν k u k = νk (4.49)
where the reader is reminded that the k sub-index symbolizes the discretized step within the horizon. In other words, with this notation, x k corresponds to the state at the step k in the horizon starting at time t n , also denoted as x k|n . The same notation applies to the other variables. Then, once the MPC optimization is solved, it is straightforward to obtain the desired twists and spatial accelerations from x and u. Yet, for the desired pose X , the retract function is required:

X k = ψ -1 (ξ k ) (4.50)
Furthermore, in order to link (4.45) and ( 4.46) with (3.27) (with linear MPC in mind), it needs to be linearized. Computing ψ(X k+1 ) yields:

ψ(X k+1 ) = log(∆X k e Tν k + T 2 2 νk ) (4.51)
This expression has a first-order approximation in its Lie algebra (see Equations 68-74 in [START_REF] Solà | A micro Lie theory for state estimation in robotics[END_REF]):

revisited ↑ ↓ ξ k+1 = log(e ξ k e Tν k + T 2 2 νk ) ≈ ξ k + J log R (ξ k )(Tν k + T 2 2 νk ) (4.52)
where J log R (ξ k ) is the right-trivialized Jacobian of the logarithmic map, evaluated at ξ k . A closed-form expression can be found in [Barfoot et al., 2014]; [START_REF] Solà | A micro Lie theory for state estimation in robotics[END_REF] and is implemented in the Pinocchio library [START_REF] Carpentier | The Pinocchio C++ library, a fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives[END_REF]. It relates the additive increments in T 0 M (the tangent map at the origin) to the rightmultiplied increments in SE(3). Equation ( 4.52) offers the missing piece to have a concrete definition for (3.27) when considering the acceleration νk as the control input:

revisited ↑ ↓ A k = I 6 TJ log R (ξ k ) 0 6 I 6 B k = T 2 2 J log R (ξ k ) TI 6 (4.53)
finally offering a concrete definition for the propagation of the state of the system as in (4.38).

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

X (α) = ψ -1 (αψ(X t )) = X 0 e αψ(X t ) (4.54) It is straightforward to notice that X (0) = X 0 and X (1) = X t .
This geodesic path is obtained by minimizing the Log-Euclidean ([Jayasumana et al., 2014], Table 1) distance metric between the lifted pose ξ k and the lifted target pose ξ t = ψ (X t ):

ξ t -ξ k 2 2 (4.55)
Then one can define a metric for the state space that calculates the target-related cost in (4.39). Assuming the target state x t contains the lifted target pose and a null (zero) body twist:

revisited ↑ ↓ x t = ξ t 0 (4.56)
for each step in the discretized horizon, one can use the metric:

x t -x k (x 0 , u 0 , . . . , u k-1 ) 2 2
(4.57)

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

Interpolating the Horizon Trajectory ↑

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

In the case of the twist and input, their value at some desired time can be obtained by a simple linear interpolation. Consider a desired time within two steps in the horizon t d ∈ [t k , t k+1 ] and some variable discretized within a horizon y, then the linear interpolation function f lerp (y, t d ) is defined as:

y(t d ) ≈ f lerp (y, t d ) = y k + (t d -t k ) T (y k+1 -y k ) (4.58)
such that y(t k ) = y k and y(t k+1 ) = y k+1 . Then the desired trajectory can be interpolated from the body twist and spatial acceleration with f lerp (ν, t d ) and f lerp ( ν, t d ), respectively.

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

X (t d ) ≈ X k e αδ δ = log(X -1 k X k+1 ) α = (t d -t k ) T ( 4

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

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

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

1. constraints compliance.

2. the ability to manage instantaneously switching targets.

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

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

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

Experimental Procedure & Setup ↑

A three phase experiment is proposed, each progressively increasing the complexity of the task. The setup and phases are illustrated in Figure 4.4. After completing each phase, the robot returns to its "home pose" before embarking on the subsequent phase. The specifics of the phases are as follows: 1. Non moving target: The simplest task is presented in the first phase. The robot is requested to pick-up the plastic brick placed in the workspace on top of an irregular surface (to induce non-trivial orientations). The pose of the brick is not known a priori and is detected via a vision system. The plastic piece is not moved during this phase. Given this target pose information X t , the robot starts its motion towards the brick using the control scheme shown in Figure 4.1. Once the object is reached, the robot grasps it and releases it after a few seconds. The tracking results of this phase are shown in Figure 4.5.

Slowly moving target:

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

Operator moving target:

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

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

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

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

2.

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

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

The same Quadratic Program (QP) solver is used to solve the constrained task space inverse dynamics. The values of gains and weights for this inner QP controller are summarized in Table 4.1. It is just recalled here that a larger cost is assigned to the terminal step of the horizon to favour convergence towards the target. Implementation details may be found in the source code 3 . Details on the resulting motion from one run of this experiment are provided in Figure 4.5 for the first of the described phases. While the brick stays in a fixed pose during this phase, it serves to showcase two behaviors: first, it shows the online re-planning algorithm uses all the acceleration available to attain pseudo-optimal trajectories; secondly, it does so even when the provided target pose is discontinuous (at the start of the phase).

Gains / Weights K p K d K q K q w reg Value 150 24.5 100 10 10 -5 Units s -2 s -1 N.m.s -2 N.m.s -1 N -2 .m 2
The nuances of the other two experiments are best understood through the video accompanying this study, which can be referenced in Figure 4.7. The dynamic nature of the moving target produces an adaptive behavior that is most effectively interpreted within the video context. Nonetheless, snapshots from the video can be seen in Figure 4.6. These captures illustrate the experiment third phase where an operator holds the brick. The objective here is to demonstrate the robot real-time adaptability, even when faced with external disturbances, such as being manipulated by an operator, as highlighted in Figures 4.6(d) and 4.6(e). 4.5(a) shows the error between the current and the target poses. The discontinuity seen at the beginning corresponds to the start of the experiment sequence, where the target pose switches from the "home pose" to the plastic brick pose. This

Analysis ↑

ν max νmax νmax linear 0.2 (m/s) 2 (m/s 2 )
1000 (m/s 3 ) angular 0.8 (rad/s) 5 (rad/s 2 ) 3000 (rad/s 3 ) highlights the convergence towards the requested pose without any a priori knowledge on the trajectory to follow. Moreover, it shows that even in the presence of a discontinuous target (a task change), the approach succeeds in providing a smooth transition that, while respecting constraints, progressively adapts the motion.

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

The final error is not exactly zero (2.1mm and 10.4 × 10 -2 rad) due to the relatively low gains of the Proportional Derivative (PD) controller and the absence of an integral term to reject disturbances due to dry friction at the joint level. Yet, the robot is able to grasp the targeted object at the end of the motion as can be seen in the accompanying video (see Figure 4.7). The first thing to notice is that the constraints on the planned trajectory are met by the MPC, as can be seen on the velocity and jerk curves.

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

Finally, one can notice that the robot end-effector velocity surpasses the established limits. This is discussed in Section 4.5.

(c) -0.2 0 0.2 ν (m/s) -0.8 0 0.8 ν (rad/s) (d) -1 0 1 νmpc (m/s 2 ) -2 0 2 νmpc (rad/s 2 ) (e) 0 0.5 1 1.5 2 2.5 3 -1,000 0 1,000 Time (s) νmpc (m/s 3 ) 0 0.5 1 1.5 2 2.5 3 -2,000 0 2,000 Time (s)
νmpc (rad/s 3 ) Figure 4.5: In the first phase, the robot is tasked to pick up the plastic brick positioned on an uneven surface to induce a non-trivial orientation. The brick pose, unknown beforehand, is identified through a vision system and remains stationary. Using this information, the robot moves to the brick as per the control scheme shown in Figure 4.1. After grasping, it holds the brick for a few moments and then releases it. The tracking results indicate target poses set in real-time. In each graph, the red, green, and blue curves denote x, y, and z axis components.

Actual robot velocities are shown with solid lines, while the desired velocities (calculated by the MPC and denoted ν mpc ) are dotted. Shaded regions mark the bounds set for the MPC.

(a) (b) (c) (d) (e) (f) (g) (h) (i)
Figure 4.6: This showcases the third phase of the experiment. Certain key moments of the accompanying video have been captured in still images to provide a more immediate understanding. During the third phase of the experiment, the focus shifts to the robot real-time adaptability and its capability to re-plan its actions in response to dynamic changes. During this phase, a human operator holds the plastic brick, moving it in an unpredictable manner at a moderate pace. As the brick moves, the robot diligently tracks its position and orientation in real-time. Adding to the complexity, the operator intervenes by restraining the robot movement for a short period (shown in (d) and (e)), serving as an external disturbance. This interruption aims to highlight the robot proficiency in rapidly recalculating and adjusting its strategy. The images depict three elements: on the left, the real robot as seen during the test is shown; on the bottom right, a virtual counterpart displays both the target pose (the brick pose) and the horizon trajectory calculated in real-time; meanwhile, the top right presents graphs illustrating the tracking error and planned velocities.

Discussion ↑

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

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

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

Furthermore, the controller's interactive capabilities demonstrate its ability to adjust to the physical attributes of the operator. For instance, in a pick-and-place scenario where the robot retrieves an item from a human, it will approach the human. This allows the operator to choose a comfortable height or location for interaction. This example is comparable to the scenario shown in the experimental interactive tracking task. However, more complex tasks where the robot needs to assist the human have the potential to showcase a stronger effect. This is further enabled by decoupling the task-related definition and constraints from the joint-level controller. Indeed, some constraints are intrinsically task related and somewhat independent of the robot performing the task while others are strongly related to the whole robot motion. It is for example the case for safetyaware constraints [Joseph et al., 2018a] or the exploitation of the robot redundancy to perform secondary tasks without compromising performance. Examples of the later case are shown for safety purposes in [START_REF] Joseph | Online minimization of the projected mass of a robot for safe workspace sharing with a human[END_REF]; [Mansfeld et al., 2017] and for situation awareness in [Camblor et al., 2022]. Figure 4.5(c) shows that the real end-effector velocity loosely follows the planned velocity, going beyond the established limits in some cases. The measured velocity is being used as the initial state of the MPC at each time step, causing the associated Quadratic Program (QP) to become infeasible. This characteristic is intrinsic to any QP-based architecture and it can be considered both a problem and an advantage: for one, it allows easily detecting ill-conditioned initial states that could result in undefined behaviors (for example, to trigger an emergency stop mode); on the other hand, it demands that a proper strategy is put in-place to deal with this situation.

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

Nevertheless, this opens one fundamental question that naturally arises from this type of cascade control architectures: the compatibility between outputs of each stage. For instance, the MPC could plan a trajectory that respects the Cartesian constraints even if it is instantaneously unfeasible in a control step or may lead to an inevitable joint-level constraint violation in the near future [START_REF] Rubrecht | Constraints Compliant Control: constraints compatibility and the displaced configuration approach[END_REF].

To solve this problem, joint-level constraints [START_REF] Prete | Joint Position and Velocity Bounds in Discrete-Time Acceleration/Torque Control of Robot Manipulators[END_REF] should be implicitly considered at the MPC level.

There exist efficient methods to effectively formulate the actual robot capacities as Cartesian constraints [START_REF] Skuric | On-line force capability evaluation based on efficient polytope vertex search[END_REF]. This type of constraints goes beyond classical box-like bounds and is a step towards more complex adaptation of the robot behaviour based on its motion capabilities. Integrating these into the MPC is addressed in the next chapter.

Conclusion ↑

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

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

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

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

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

Introduction ↑

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

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

Rationale: Feasible Online Motion Generation ↑

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

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

Indeed, often manufacturers offer constant Cartesian limits that can be used to solve planning problems. This constitutes a simplifying strategy that allows modeling actuation capabilities with an acceptable approximation as a constant task space limit.

The tradeoff is two-sided: for one, sometimes this corresponds to undervaluing the capabilities, provoking a performance loss but remaining robust from a feasibility decoupling task-related behaviors and constraints from the actual execution in the robot, providing at least some level of independence from the robot architecture.

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

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

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

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

From Joint-space to Task-space Constraints ↑

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

The proposal in this work is to exploit the linear inequality mechanisms present in Quadratic Program (QP) optimization. This way, if the Joint-space Constraints (JSC) can somehow be represented as linear inequalities of task-space variables (i.e. those present in the Model Predictive Control (MPC)), they can be directly embedded into the problem constraints.

CONTENTS

Workspace Analysis

Convexification

Task-space Projection

P p|Q Measurements q k , qk , qk , τ k Workspace Constraints P p|W , P ṗ|W Robot Capacities Q, Q, Q, ... Q T , Ṫ Workspace Configuration Pp , P ṗ, P p, P ... p Robot Constraints C τ|Q , C τ| Q C τ|T , C τ| Ṫ Figure 5.2:
revisited ↑ ↓ Zoom into the Workspace Analysis Block. It allows transforming userdefined 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.

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

Task-space Feasible Sets as Convex Polyhedrons ↑

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

P x|y = {x ∈ R n | x = Ay + b, y ∈ [y m , y M ]} (5.1)
where P defines a convex polyhedron1 over x due to the bounds on y [Fukuda et al., 2004]. This definition implies that the interval domain of the y variable defines a projected constrained space on the x variable through the linear transformation operator A. This constrained space is shifted from the origin by b. The shape of the projected space is only affected by A and b determines affine spaces through a "bias" or displacement in the domain of x.

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

C x x ≤ d x (x ∈ P x|y ) (5.2)
A concrete application of this is the main focus of this chapter. Task-centric motion planning focuses on the Cartesian space. Meanwhile, the actuation capabilities of a robot belong in the configuration space. The formula in (5.1) allows the expression of the actuation capabilities as convex sets in the Cartesian space.

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

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

A comprehensive introduction to polyhedral sets applied to robotics and biomechanics can be found in Antun Skuric's Thesis2 , including a way to approximate the robot reachable space [Skuric et al., 2022c] and human-robot collaboration scenarios based on an estimate of the human wrench capabilities [Skuric et al., 2021b]; [START_REF] Skuric | On-line force capability evaluation based on efficient polytope vertex search[END_REF].

Robot Actuation Capacities ↑

The interest of this work lies in generating "smooth" and feasible trajectories. From a mathematical standpoint, this translates to generating C 2 -continuous joint trajectories q(t) ∈ R n (for an n-dof serial robot) such that:

revisited ↑ Q = {q(t) ∈ R n | q ∈ [q m , q M ]} Q = { q(t) ∈ R n | q ∈ [ qm , qM ]} Q = { q(t) ∈ R n | q ∈ [ qm , qM ]} ... Q = { ... q (t) ∈ R n | ... q ∈ [ ... q m , ... q M ]} (5.3)
where Q designates the set of feasible joint configurations; Q, Q, ... Q respectively refer to velocity, acceleration and jerk constraints, that form the kinematic JSC. More specifically, the desired q(t) is jerk bounded and is smooth up to the second derivative q(t).

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

Nevertheless, this work assumes constant joint acceleration and jerk capacities represented in Q, ... Q respectively. These are conveniently announced by the manufacturer [Franka Emika, n.d.] Even though this constitutes an approximation, it provides an acceptable framework that improves feasibility by going up to a higher order [Gallant et al., 2018]. Multiplying the set by a small time interval ∆t = 0.15 corresponds to the space the endeffector 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. sets P ṗ| Q, P p| Q, P ... p | ... Q respectively designate the feasible velocities, accelerations and jerks due to the joint-space capacities in velocities, accelerations and jerks in each case. The dependencies on the measured state were explicitly added to show that some terms do not affect the projection of the joint capacities onto the Cartesian space but rather act as a constant bias or displacement over the origin. These biases are denoted as B p, B ... p for the acceleration and jerk, respectively.

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

B p(q k , qk ) ≈ 0 B ... p (q k , qk , qk ) ≈ 0 (5.11)
which a relatively good assumption at low to moderate speeds. When operating at higher velocities, eliminating these biases also allows to always include the origin in the intersection set, increasing the chances of obtaining a compatible QP problem.

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

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

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

revisited ↑ ↓ L ṗ| Q(q k ) = ṗ ∈ R 3 | ṗ = J l (q k ) q, J a (q k ) q = 0 3 , q ∈ Q (5.12) L p| Q(q k , qk ) = p ∈ R 3 | p = J l (q k ) q + B p, J a (q k ) q = 0 3 , q ∈ Q (5.13) L ... p | ... Q (q k , qk , qk ) = ... p ∈ R 3 | ... p = J l (q k ) ... q + B ... p , J a (q k ) ... q = 0 3 , ... q ∈ Q (5.14)
where 0 3 designates a zero-valued 3-vector; the bias terms B p, B ... p coincide with their previous definitions and their dependency on the measured states q k , qk , qk was dropped for clarity. The added equality constraint on the angular component effectively restricts the joint motions to those that act on the kernel of the angular Jacobians and its derivatives J a , Ja , Ja . This way, the sets designated with L serve to clearly denote the purely translational feasible motions.

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

revisited ↑ ↓ P ν| Q(q k ) = ν ∈ se(3) | ν = J(q k ) q, q ∈ Q (5.15) P ν| Q(q k , qk ) = ν ∈ se(3) | ν = J(q k ) q + B ν(q k , qk ), q ∈ Q (5.16) B ν(q k , qk ) = J(q k , qk ) qk P ν| ... Q (q k , qk , qk ) = ν ∈ R 3 | ν = J(q k ) ... q + B ν(q k , qk , qk ), ... q ∈ Q (5.17) B ν(q k , qk , qk ) = 2 J(q k , qk ) qk + J(q k , qk , qk ) qk
where most of the previous conclusions still stand but the notation should make it clear now that the full Jacobian matrix and its derivatives are used. This way, the full pose derivative variables can be constrained (i.e. the twist ν, spatial acceleration ν and jerk ν).

From Joint Bounds to Convex Reachable Workspace ↑

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

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

P p|Q (q k ) = p ∈ R 3 | p = FK(q), q ∈ [q m , q M ] (5.18)
which, unlike the definitions from the previous section, employs a non-linear Forward Kinematics (FK) function 3 . This provokes that no convexity characteristics can be inferred from the set P p|Q , which is a problem for the intended use in this work (to be embedded into optimization algorithms).

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

p k+1 = p k + dp k dp k = J l dq k (5.19)
where the main interest lies in computing p k+1 given a configuration differential dq.

Then, redefining this infinitely small variation to evaluate the available joint motion (signifiying it is no longer assumed to be small) yields a configuration delta ∆q. It can be computed to be the distance towards each of the joint bounds to define a set with respect to the current configuration:

∆q k ∈ [∆q m (q k ), ∆q M (q k )] ∆q m (q k ) = q m -q k ∆q M (q k ) = q M -q k (5.20)
Finally, integrating these expressions allows defining a convex reachable space set as:

revisited ↓ Pp|Q (q k ) = p ∈ R 3 | p(∆q), ∆q ∈ [∆q m , ∆q M ] p(∆q) ∼ p k + J l (q k )∆q p k = FK(q k ) (5.21)
where this time around, the term p k provides the constant bias for the convex set and can be computed by using the FK on the measured robot configuration q k . The definitions for ∆q m , ∆q M are provided in (5.20).

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

lim ∆q→0 Pp|Q (q k ) ≈ P p|Q (q k ) (5.22)
Moreover, bigger ∆q values imply that the joint bounds are far enough that even if the approximation is bad, their consideration is not urgent. In a worst case scenario that the robot motion tends towards the joint bounds, the increasingly better instantaneous precision of the approximation helps appropriately consider the joint bounds, provided that the convex set is re-computed frequently (which is an acceptable assumption in collaborative robotics scenarios that require reactivity).

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

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

revisited ↑ ↓ L p|Q (q k ) = p ∈ R 3 | p(∆q), J a (q k )∆q = 0 3 , ∆q ∈ [∆q m , ∆q M ] (5.23)
where ∆q m , ∆q M are defined in (5.20) and p(∆q) is defined in (5.21).

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

P X |Q (q k ) = {X ∈ SE(3) | X = FK(q), q ∈ [q m , q M ]} (
To do so, the set is to be defined on to the logarithm of the pose ξ = log(X ). Then, revisiting the first-order algebraic expression in the tangent space one obtains:

revisited ↑ ξ k+1 = log(e ξ k e ∆tν k ) ≈ ξ k + dlog ξ k ∆tν k (5.25)
that relates the pose logarithm ξ with the twist ν. Now, applying the same logic as in (5.21) is possible: .26) where P ξ|Q defines a convex set over the pose logarithm. This set is defined on the tangent space of the poses, se (3). The interpretability of this expression is not directly translated for the space of the poses, SE(3). However, for optimization purposes, as the proposed approach in this work is based on using the pose tangent space, this effectively provides the necessary means to limit the solution space. Then, once the optimization is finished, the actual pose can be recovered by computing the exponential of the solution.

revisited ↑ ↓ Pξ|Q (q k ) = {ξ ∈ se(3) | ξ(∆q), ∆q ∈ [q m -q k , q M -q k ]} ξ(∆q) ∼ ξ k + dlog ξ k J∆q ξ k = log(X k ) ( 5

Actuation-aware Task-space Re-planning ↑

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

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

Some MPC notions and notations ↑

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

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

x n+1 = Ax n + Bu n A ∈ R d×d , B ∈ R d×m (5.27)
where A and B represent its state and input matrices whereas x and u are the state and input vectors. Smooth point to point pose motion generation can be achieved by generating trajectories at the acceleration level. Choosing the control input at the task level to be the acceleration, allows predicting the evolution of the end effector (system) state -pose and twist -over a short period of time to generate a jerkbounded motion.

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

x k+1|n = Ax k|n + Bu k|n for k = 0, 1, . . . , h -1 (5.28)

Subsequent sections drop the n subscript to simplify the notation, making x k|n equivalent to x k . This propagation can be synthesized into a simple linear algebraic expression: revisited ↑ X(x 0 , U) = ÂU + Bx 0 (5.29) where:

revisited ↑          x 1 x 2 x 3 . . . x h          X =          B AB B A 2 B AB B . . . A h-1 B A h-2 B . . . AB B          Â          u 0 u 1 u 2 . . . u h-1          U +          A A 2 A 3 . . . A h          B
x 0 (5.30) This formulation can be employed to define a pose motion generation problem as an optimization problem over the control input that minimizes the distance towards a target state. Because the state can be computed from a control input horizon, the optimization maintains the capability of recovering the state throughout the horizon, allowing to also consider state constraints while reducing the optimization variables (as it only needs to consider the control inputs), effectively reducing resolution times.

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

revisited ↑ min u X t -X(x 0 , U)

2 P + U 2 R (5.31) revisited ↑ C x m ≤ C x x k ≤ C x M (5.32) revisited ↑ C u m ≤ C u u k ≤ C u M (5.33) revisited ↑ C um ≤ C u uk ≤ C uM (5.34)
with: uk = (u k+1u k ) T optimizing over the control inputs u. Where P, R designate semi-positive definite matrices containing weighting terms for the input throughout the horizon and X t corresponds to the target state vector repeated throughout the horizon.

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

About the Linear Inequality Constraints ↑

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

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

In order for these to be readily integrated into the task-space re-planning scheme proposed as an MPC, they need to be expressed as linear inequality constraints over either the state or the input of the system, as introduced in the most generic way that a QP admits in (5.32) to (5.34). Yet, they can also be used to formulate simple variable bounds or zonotopes [Bouchard et al., 2009]. For instance, the interval set of a variable defined as: 5.35) where y generically represents the variable constrained within some set Y with limits y m , y M (its upper and lower bounds), also admits a linear inequality representation as:

P y|Y = {y ∈ R 3 | ∀y ∈ [y m , y M ]} (
C l ≤ C y y ≤ C u (5.36) or equivalently:

C y -C y y ≤ C u -C l (5.37)
where C y is an identity and C l , C u contains the variable bounds. This last form shows that the linear inequality constraints can be used to embed complex constraints in the form of (5.2), because (5.35) is in fact a special case of (5.1).

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

Acceleration-based Position Re-planning ↑

For a pure translational motion re-planning (i.e. position control), the generic MPC formulations presented in Section 5.3.1 need to be specialized. More concretely, the system state and input need to be defined. Based on a second order expansion of the position: 5.39) one can define a system based on the acceleration as the control input: 5.40) where p, ṗ respectively designate the position and velocity and p denotes the acceleration. Meanwhile, x t contains the target state: the target position and a null velocity. Then, to complete the state-space system definition, the expressions for CONTENTS the A, B matrices are required:

p k+1 = p k + T ṗk + T 2 2 pk (5.38) ṗk+1 = ṗk + T pk (
x k = p k ṗk u k = pk x t = p t 0 (
A k = I 3 T I 3 0 3 I 3 B k = T 2
2 I 3 T I 3 (5.41) which provide all the necessary elements to complete the acceleration-based definition of the MPC for reactive position re-planning.

Position Re-planning Constraints ↑

In the interest of safety-aware planning, a multitude of approaches can be embedded as constraints. For instance, some strategies to increase safety through velocity modulation employ damage risks mitigation measures [START_REF] Palleschi | Fast and Safe Trajectory Planning: Solving the Cobot Performance/Safety Trade-Off in Human-Robot Shared Environments[END_REF]; furthermore the technical specification for collaborative robotics contemplates velocitylimited operation zones [ISO/ TS-15066, 2016]. In terms of position control, safetyrelated zones and velocity limits can be denoted with an S as: 5.42) 5.43) where P p|S designates the space deemed safe for the robot to operate in; while P v|S designates the maximum operating velocity.

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

Meanwhile, manufacturers also provide task-space related maximum velocities, accelerations and jerks, which can be denoted with an M as: 5.44) 5.46) These kind of constraints make sense from a usability point of view as they are easy to interpret and practical from a task-centric design proposed in this work. Yet, the linear inequalities form show that they could potentially be used to express more complex convex sets like the ones introduced in Section 5.2.1 that admit the same form as (5.1). For instance, this means that the shape of the allowed geometrical space in P p|S does not need to be rectangular (as in virtual walls parallel to the world frame planes). These arbitrary and application-dependent sets are denoted with A.

P ṗ|M = {v ∈ R 3 | ∀ ṗ ∈ [ ṗm , ṗM ]} (
P p|M = {v ∈ R 3 | ∀ p ∈ [ pm , pM ]} (5.45) P ... p |M = {v ∈ R 3 | ∀ ... p ∈ [ ... p m , ... p M ]} (
Altogether, the intersection of all set of CSC related to the task-space, are denoted as workspace constraints with a W: P p|W = P p|S ∩ P p|M ∩ P p|A (5.47) P ṗ|W = P ṗ|S ∩ P ṗ|M ∩ P ṗ|A (5.48) P p|W = P p|S ∩ P p|M ∩ P p|A (5.49) P ... p |W = P ... p |S ∩ P ... p |M ∩ P ... p |A (5.50) Finally, the focus of this section is on how to add actuation awareness onto the taskspace re-planning, while also considering the workspace constraints. To complete the position control MPC problem that contemplates all these restrictions, this definition is necessary:

p k = FK(q k ) (5.51) ṗk = J l q k (5.52)
p ∈ P p P p = P p|Q (q k ) ∩ P p|W (5.53) ṗ ∈ P ṗ P ṗ = P ṗ| Q(q k ) ∩ P ṗ|W (5.54) p ∈ P p P p = P p| Q(q k , qk ) ∩ P p|W (5.55) ... p ∈ P ... p P ... p = P ... p | ... Q (q k , qk , qk ) ∩ P ... p |W (5.56) where p k , ṗk respectively designate the initial position and velocity. These equations express the convex sets that need to be to be formulated as linear constraints as in (4.40) and (4.41). If fixed orientation is required along with position re-planning, the feasible sets may be replaced with their pure translation equivalent based on L, as defined in Sections 5.2.3 and 5.2.4.

Position Re-planning Constraints Simulations ↑

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

1. Zonotope constraints with zero initial conditions shown in Figure 5. In all cases, a single horizon with a fixed target position was computed and visualized. The horizon duration is hT=4s (as shown in the trajectories), with a configuration h=100 and T=40ms. The input weight is γ = 5 T 2 2 , and the terminal state weight is: (5.57) as defined in (4.29) and (4.30).

P T = I 3×3 100 0 3×3 0 3×3 I 3×3 100T
Furthermore, in all the scenarios, the target position is outside of the position constraints P p to showcase what happens when the target is unreachable. In such case, the distance-minimizing cost function as formulated in the MPC drives the system towards it until it reaches the closest point within the constraints.

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

Another significant result that can be appreciated across all scenarios is the resulting acceleration curve: it approximately resembles an optimal planning algorithm based on a trapezoidal acceleration profile. This showcases that a close to optimal use of the available capacities (kinematic constraints in velocity and acceleration).

In the case of polyhedral constraints, the same conclusion can be drawn regarding the saturation of the available capabilities. However, given the irregular shape of the polyhedron, the curves no longer seem to align with a trapezoidal acceleration profile. Yet, as can be appreciated in the 3D plots, the resulting horizon uses the maximum available capacities by aligning with the faces of the polyhedron.

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

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

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

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

The relevance of this last result becomes even more prominent in an online replanning scenario like the collaborative tasks proposed in this work. In such case, the MPC would has to continuously compute new trajectories based on evolving initial conditions, closing the feedback loop. Furthermore, this also demonstrates that in a scenario where the task (cost function) is switched or time-varying, the obtained trajectories remain coherent with the available kinematic capacities, showcasing a smooth transition that takes into account the starting point.

Finally, Figure 5.7 shows simulation 3. The objective is to showcase the combined situations of the previous scenarios with a non trivial velocity convex set. For instance, this is analogous to employing the feasible velocity sets devised from the joint capacities. The result shows the same conclusions as before can be extrapolating, keeping in mind that the velocity capacities are now heavily restricted by a polyhedron: the resulting position shows a curvy path that smoothly transitions towards the ending position; while the velocity path saturates the available capacity by "sticking " to the faces of the polyhedron.

Acceleration-based Pose Re-planning ↑

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

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

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

By using the lift function introduced in (4.47), one can map X k to a vector ξ k . The lift function maps elements from the SE(3) manifold M to its tangent space at X 0 : ψ : M → T X 0 M. The function ψ -1 is referred to as the retract function.

With the MPC being formulated in se(3), the lift operator ψ is used to map the current state and the desired pose into se(3) to perform the optimization. Meanwhile the retract operator ψ -1 is used to map the computed trajectory back to SE(3). This enables directly embedding the lifted pose ξ = ψ(X ) into the discretized state of the system x, along with the body twist ν. Then, choosing the spatial acceleration ν as the input:

revisited ↑ x k = ξ k ν k u k = νk (5.60)
where the reader is reminded that the k sub-index symbolizes the discretized step within the horizon. In other words, with this notation, x k corresponds to the state at the step k in the horizon starting at time t n , or equivalently, denoted as x k|n . This expression has a first-order approximation in its Lie algebra (see Equations 68-74 in [START_REF] Solà | A micro Lie theory for state estimation in robotics[END_REF]):

revisited ↑ ξ k+1 = log(e ξ k e Tν k + T 2 2 νk ) ≈ ξ k + J log R (ξ k )(Tν k + T 2 2 νk ) (5.61)
where J log R (ξ k ) is the right-trivialized Jacobian of the logarithmic map, evaluated at ξ k . A closed-form expression can be found in [Barfoot et al., 2014]; [START_REF] Solà | A micro Lie theory for state estimation in robotics[END_REF] and is implemented in the Pinocchio library [START_REF] Carpentier | The Pinocchio C++ library, a fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives[END_REF]. It relates the additive increments in T 0 M (the tangent map at the origin) to the rightmultiplied increments in SE(3). Equation (4.52) offers the missing piece to have a concrete definition for (3.27) when considering the acceleration νk as the control input: (5.62) finally offering a concrete definition for the propagation of the state of the system as in (5.29).

revisited ↑ A k = I 6 TJ log R (ξ k ) 0 6 I 6 B k = T 2 2 J log R (ξ k ) TI 6
Assuming the target state x t contains the lifted target pose and a null (zero) body twist:

revisited ↑

x t = ξ t 0 (5.63)

Pose Re-planning Constraints ↑

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

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

The formulation can be constructed by analogy from a position displacement zonotope constraint: 5.64) that can also be formulated as a polylhedron: 5.65) where C i denotes the first row of some linear inequality constraint matrix C. For the zonotope case, it is implied that

p k + ∆p ≤ p M → ∆p ≤ ∆p M ∆p M = p M -p k (
C i ∆p ≤ C i ∆p M (
C i = [1 1 1] T .
In that case, the variable ∆p and C i are both in the same frame (implicitly, the local world-aligned frame). Assuming the constrained variable ∆p is now in the body frame (i.e. rotated), the inequality needs to be brought into the same frame, by applying the current orientation R k :

R T k C i ∆p ≤ R T k C i ∆p M (5.66)
The same procedure can be followed for the case of pose constraints with: (5.67) where C l , C u , that respectively denote the original lower and upper bounds, must be continuously updated to account for the movement of the end-effector, just like with ∆p M . The matrix Ad X k -1 corresponds to the adjoint map that allows changing

Ad X k -1 C i C l ≤ Ad X k -1 C i dlog ξ k ξ ≤ Ad X k -1 C i C u

Experimental Validation ↑

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

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

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

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

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

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

• The trajectory also takes into account extra workspace constraints.

• It retains the features highlighted in previous chapters.

Control Architecture Implementation ↑

The control architecture resembles the experimental setup in Chapter 4. However, the control architecture is extended to include the feasibility sets. The differences are highlighted in Figure 5.8. The main changes lie in the workspace analysis features, which now considers both the workspace constraints with the joint actuation limits. The geometric workspace available is not included though, as they are analyzed broadly in the position control MPC whose example simulations are shown in Section 5. for online pose re-planning while a second inverse dynamic solver executes the resulting trajectories. This loop runs at a 1kHz in sync with the actual robot control Both setups are based on the ROS [START_REF] Quigley | ROS: an open-source Robot Operating System[END_REF] framework for the general communication and visualization. In order to compute the robot modeling aspects, the Pinocchio library [START_REF] Carpentier | The Pinocchio C++ library, a fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives[END_REF] is employed.

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

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

• ProxQP [START_REF] Bambade | PROX-QP: Yet another Quadratic Programming Solver for Robotics and beyond[END_REF]: A relatively recent library optimized with performance in mind. In these tests, the computation time was not up to the level of other alternative. However, more importantly, at the time of writing, it does not feature a computation time limit, necessary for the real time requirements of the controller.

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

• qpMAD 5 : This library implements the Goldfarb-Idnani dual active set algorithm for QP [START_REF] Goldfarb | A numerically stable dual method for solving strictly convex quadratic programs[END_REF] with some added optimizations to improve computation times. It often yielded the fastest resolution times. However, in some rare cases, it seemed to have a hard time dealing with too many linear inequalities (a consequence of polyhedral constraints in a horizon), leading to computation times over the control rate period. It also does not feature a computation time limit and so it was discarded in favor of the last one.

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

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

Though it remains an experimental implementation (not intended for production 5 https://github.com/asherikov/qpmad setups), more details may be found in the open-source implementation library6 . A workspace is also readily available for experimentation purposes7 .

Experimental Scenarios ↑

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

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

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

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

Interactive scenario:

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

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

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

Experimental Design & Objectives ↑

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

As stated before, the objective is to showcase the effect of task-pace planning while accounting for joint level constraints. To maintain a certain level of simplicity and intuitiveness in the experiences and results, only the joint bounds and velocities are taken into account for these objectives, leaving the JSC for the acceleration and jerk joint activated but not shown in the results. Concretely, this entails obtaining full pose Cartesian trajectories while considering joint level constraints Q, Q introduced in (5.3).

In order to test these constraints independently, two types of experiments were devised:

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

Joint velocity modulation experiments:

In this case, the experiment looks to showcase the effects of P ν| Q introduced in Section 5.2.3.

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

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

About the Measured Result Quantities ↑

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

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

• Position Error: Refers to the error between the end-effector position and the target position (the translation component of the target pose X t shown in ). Not to be confused with the "desired error" which would correspond to the position error between the one planned by the MPC (the position component of X mpc ) and the current position of the end-effector (the current pose is denoted X ). This is wrongfully called desired because it represents the servoed trajectory that actually accounts for the planning. The way to compute this

Results: Joint Bounds Aware Planning ↑

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

Such setting is shown in Figure 5.11. The robot is instructed to follow the pose of a gray cube floating in space. The images show the robot in a configuration near its limits. The horizon trajectory is also showed. However, because horizon tries to drive the robot in an impossible direction, the robot is stuck at a local minima. The horizon corresponds to the geodesic path towards the target pose, as shown in Figures 5.11(a) to 5.11(c). Once the convex reachable space constraints are activated, the horizon no longer tries to take this path. In fact, it stays near the end-effector, as it corresponds to the closest possible configuration achievable from this configuration, as shown in Figures 5.11(d) and 5.11(e). A consequence of using a linearized approximation of the reachable space is that these constraints is that the MPC, while still able to consider them, is also prone to local minima (as the target poses are actually reachable by the robot in alternative configurations).

CONTENTS

Experimental Validation on the Robot

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

revisited ↑ Pξ|Q ′ (q k ) = {ξ ∈ se(3) | ξ(∆q), ∆q ∈ [q ′ m -q k , q ′ M -q k ]} ξ(∆q) ∼ ξ k + dlog ξ k J∆q ξ k = log(X k ) (5.89) revisited ↑ Q ′ = {q(t) ∈ R n | q ∈ [q ′ m , q ′ M ]}
where the artificial joint bounds are provided by q ′ m , q ′ M and computed using a factor R q ∈ [0, 1]. Each element of these bounds (for each joint) is defined as:

q ′ m,1 = R q q m,1 q ′ M,1 = R q q M,1 q ′ m,2 = R q q m,2 q ′ M,2 = R q q M,2 q ′ m,3 = R q q m,3 q ′ M,3 = R q q M,3 q ′ m,4 = R q q m,4 q ′ M,4 = q M,4 (5.90)
q ′ m,5 = R q q m,5 q ′ M,5 = R q q M,5 q ′ m,6 = q m,6 q ′ M,6 = R q q M,6 q ′ m,7 = R q q m,7 q ′ M,7 = R q q M,7

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

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

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

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

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

Results: Joint Velocity Modulation Task-space Planning ↑

These experiments entail a combination of back and forth and interactive scenarios.

In order to showcase the velocity modulation capabilities of the feasible velocities constraint using Pξ| Q as indicated in Figure 5.8, a new feasible set with modulated capacities was defined:

revisited ↑ P ν| Q′ (q) = ν ∈ se(3) | ν = J(q) q, q ∈ Q′ (5.91) revisited ↑ Q′ = { q ∈ R n | q ∈ [R q qm , R q qM ]} (5.92) R q ∈ [0, 1]
where R q designates a factor between 0 and 1 that reduces the maximum joint velocity allowed.

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

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

To showcase the effects in a dynamic setting while modulating R q, four experiments were performed:

1. Simulation without reorientation: A back and forth between poses without changing the targets orientation is shown in Figure 5.13.

Simulation with reorientation:

A back and forth between poses with change in the target orientations is shown in Figure 5.14.

Real Robot without reorientation:

A back and forth between poses with change in the target orientations is shown in Figure 5.15 and executed on the real hardware.

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

In all these settings the robot begins at a neutral pose and then is instructed to begin.

In all experiments, while the robot is executing the task, the value of R q is changed to modulate the available joint velocities. The effects are highlighted in the "nonkernel" normalized joint velocities. The values of R q are also plotted in these graphs, allowing for a direct comparison (thanks to the normalized velocities).

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

The general results help make an argument for the consideration of JSC into the MPC, as the joint velocities roughly follow the same pattern as the modulation set with R q.

Furthermore, the interactive scenario with the real robot showcases the capability of the approach to adapt to completely unknown situation in an online fashion, a crucial requisite for collaborative tasks.

CONTENTS

q 1 q 2 q 3 q 4 q 5 q 6 q 7 -2 .13: Simulation: Showcases a back and forth task scenario between two poses without changing the orientation, while changing the maximum allowed q (showed with R q) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints.

q 1 q 2 q 3 q 4 q 5 q 6 q 7 -2 .14: Simulation: Showcases a back and forth task scenario between two poses that have different orientations, while changing the maximum allowed q (showed with R q) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints.

q 1 q 2 q 3 q 4 q 5 q 6 q 7 -2 .15: Real-world experiment: Showcases a back and forth task scenario between two poses that have the same orientation, while changing the maximum allowed q (showed with R q) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints.

q 1 q 2 q 3 q 4 q 5 q 6 q 7 .16: Real-world experiment: Showcases an interactive task scenario where the robot tracks a pose captured in real-time while changing the maximum allowed q (showed with R q) to demonstrate the MPC capability to achieve a taskspace trajectory that takes into account joint level constraints in an online fashion. The corresponding video is shown in Figure 5.10.

Discussion ↑

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

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

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

Finally, these results highlight the potential of a modular architecture separating tasks-space and joint-space aspects: for one it is generic enough for the application to other robot architectures; secondly, it separates the motion problem formulation while still allowing some aspects of the robot model to be considered at the task level. One potential field that could benefit of this approach is humanoid robot control, where the trajectory planning is often made at the task-space level [START_REF] Lober | Task Feasibility Maximization Using Model-Free Policy Search and Model-Based Whole-Body Control[END_REF] and considering the actual actuators capabilities is crucial for stable motions like walking.

Towards Viable Joint Constraints ↑

As it was briefly mentioned in this chapter, an unresolved issue regarding constraints is on the future states compatibility. As referenced in [START_REF] Rubrecht | Motion safety and constraints compatibility for multibody robots[END_REF], a set of constraints may be instantly feasible but incompatible in the short-medium term, as they might lead to inevitable constraint violations. The assumption of bounded jerk imposes non instantaneous changes to the accelerations. Then, the assumptions in (5.3) imply that there is a minimal joint displacement required for the robot to decelerate completely (to reach a stopped state q = 0, q = 0) before hitting the joint limits described by Q (the jerk is not part of the stopped state because, under the current assumptions of not necessarily continuous jerk, it can be instantly set to zero). The set of states that do not lead to inevitable violations can be referred to as viable states [START_REF] Prete | Joint Position and Velocity Bounds in Discrete-Time Acceleration/Torque Control of Robot Manipulators[END_REF].

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

A worst-case scenario 8 is shown in Figure 5.17 assuming the robot starts braking at the start of the trajectory: when the acceleration is at a maximum value and in the same sense as the current velocity (so it is driving the velocity towards its limit). Because of the smooth stopping requirements imposed, instant changes in acceleration are forbidden and will require reverting the acceleration at maximum jerk (for the acceleration to become opposite and start reducing the velocity) for a duration that depends on the jerk limits and the starting acceleration; during this time, the velocity will continue to increase towards its limit. This phenomenon highlights an implicit incompatibility problem between constraints because they hide a potentially risky situation that may arrive from edge-case scenarios. The initial velocity shown in Figure 5.17 should not have been allowed from the start to avoid this situation. This implies that a contraction on the maximum velocity constraints caused by the higher order dynamics can help avoid inevitable violation states.

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

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

The proposal for this kind of situation is analyzed in Figure 5.19: avoiding instant 8 not the worst case because the initial velocity is not maxed out, in which case the trajectory quickly surpasses the constraints 9 As in long term compatibility, so it is rather a viability analysis. changes in acceleration imposes a 2-stage stopping trajectory: an acceleration reversing stage; and a deceleration stage that continuously reduces the velocity up to a point where both acceleration and velocity reach zero, as is proposed in [START_REF] Joseph | Online velocity constraint adaptation for safe and efficient human-robot workspace sharing[END_REF]. In order to find the viable set of constraints (that will avoid inevitable violation states [START_REF] Rubrecht | Constraints Compliant Control: constraints compatibility and the displaced configuration approach[END_REF]), the smooth (as in jerk-bounded) stopping trajectories need to be computed. The way to do this is described in Appendix E. Applying this enables computing the worst-case scenario displacement for q and q:

CONTENTS

0 t v(t R ) Acceleration Reversing -a k -a M a M t R t 1 t 2 t 3 t stop j M - j M Deceleration t R ≤ t s -v k ≤ 0 -a k ≤ 0
∆q stop = ∆q R + ∆q D (5.93)

∆ qstop = ∆ qR + ∆ qD (5.94) where the subdindices R, D refer to the reversing and deceleration stages, respectively. Furthermore, ∆q R = 0, ∆ qR = 0 for the stopping trajectories that require only a deceleration stage, as identified in Table 5.6 according to the initial velocity and acceleration. Extending the assumptions mentioned in (5.3) Constraints (JSC) [START_REF] Prete | Joint Position and Velocity Bounds in Discrete-Time Acceleration/Torque Control of Robot Manipulators[END_REF][START_REF] Rubrecht | Motion safety and constraints compatibility for multibody robots[END_REF]:

Q V = {q(t) + ∆q stop ∈ R n | q ∈ [q m , q M ]} QV = { q(t) + ∆ qstop ∈ R n | q ∈ [ qm , qM ]} (5.95)
Finally, these new sets may be used to extend the feasible sets defined in Section 5.2.3. Having ensured that the joint level constraints are viable, theoretically ensures that the polyhedral feasible sets are also viable. So, these new sets can be defined by replacing Q V , QV with Q, Q, respectively. 5.6: Worst-case scenario stages required for the stopping trajectory according to the initial velocity v 0 and acceleration a 0 .

Conclusion ↑

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

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

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

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

Chapter 6

Conclusion

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

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

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

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

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

Perspectives ↑

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

Mathematical Formulation

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

While the approach shows a promising potential, the algebra employed by the algorithms can be generalized to a formulation based on geometric algebra. This approach offers the mechanisms for potentially more interpretable expressions with some efficiency gains [START_REF] Löw | Geometric Algebra for Optimal Control With Applications in Manipulation Tasks[END_REF].

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

Control Architecture

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

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

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

Another aspect unexplored in this work that merits exploration is the treatment of tasks that require manipulation [START_REF] Gold | Model Predictive Interaction Control for Robotic Manipulation Tasks[END_REF] and contact forces [START_REF] Kleff | Introducing Force Feedback in Model Predictive Control[END_REF] from the same perspective of decoupling task and joint spaces. From a performance point of view, it presents an interesting line of questioning. Beyond that, it would better align to the modularity properties desired for collaborative robots. Furthermore, from an application standpoint, this would enable evaluating the presented actuation awareness approach while exploiting other aspects of the collaborative task, like the wrench capacities of the robot and the human [Skuric et al., 2021b]; [START_REF] Skuric | On-line force capability evaluation based on efficient polytope vertex search[END_REF].

Finally, the current manuscript focuses on collaborative robotics and more specifically, for the case of serial manipulators. However, many of the principles and results can be straightforwardly extended to other robot architectures. For instance, some control approaches for humanoid robots employ optimization methods to generate task-space reference trajectories [START_REF] Lober | Task Feasibility Maximization Using Model-Free Policy Search and Model-Based Whole-Body Control[END_REF] because doing so in the actuator space is too computationally intensive. This application could benefit from the actuation-aware task-space planning proposal to generate a-priori feasible trajectories.

Hybrid Learning Algorithms

On a more long term perspective, MPC algorithms in general offer a deterministic approach to modulating systems behaviors. However, the same mathematical principles that render them deterministic also limit their flexibility in terms of evolving behaviors: once deployed, these algorithms behave consistently and do not offer improving or fine tuning to the situation. This offers an opportunity for hybrid learning approaches that can leverage the model-based knowledge with adaptive algorithms [START_REF] Bechtle | Leveraging Forward Model Prediction Error for Learning Control[END_REF] or equip the inherently non-deterministic control of learning approaches with some safety awareness [START_REF] Fisac | A General Safety Framework for Learning-Based Control in Uncertain Robotic Systems[END_REF].

Furthermore, the tasks enabled by optimization-based controllers are limited by the design of the cost functions. In other words, the behaviors that are achievable by these approaches are limited to what can be described mathematically. Sometimes, finding a formulation that performs consistently proves to be a complex endeavor. This ultimately hinders the complexity of the tasks that can be obtained and opens doors for the exploration of learning algorithms that can better adjust their behaviors with more finely tuned tasks [START_REF] Bogdanovic | Learning to Explore in Motion and Interaction Tasks[END_REF]; [START_REF] Calinon | Gaussians on Riemannian manifolds: Applications for robot learning and adaptive control[END_REF].

Human Factor Considerations

Finally, a fundamental aspect of collaborative robotics is the human-centric perspective. This means that the human factor must be accounted for within the control schemes. Integrating these kind of aspects remains an open scientific question. However, the proposed control architecture offers the infrastructure for the inclusion of generic and human-aware constraints that can potentially express complex information about the human state such as the fatigue level [START_REF] Savin | Movement Variability and Digital Human Models: Development of a Demonstrator Taking the Effects of Muscular Fatigue into Account[END_REF].

Other human aspects that offer opportunities for richer human-robot collaboration aspects stem from evaluating posture stress in real time (ergonomic aspects), the cognitive state [Camblor et al., 2022] and the expertise of the operators [Benhabib et al., 2020].

In the same way, the MPC can straightforwardly be formulated to follow a previously generated trajectory instead of a target pose. This would allow for path planning algorithms to be plugged right before the controller, integrating unexplored behaviors like, for example, human-aware collision avoidance.

Closing Remarks ↑

Collaborative robots constitute a fundamental piece in the ongoing modern industrial transformation towards human-centric and sustainable manufacturing. As such, the market demand for cobots has seen an increasing growth trend recently in market cap and is projected to even accelerate development for the future [Gambao, 2023]. Even though the cobot industry represents a small part of the overall robotics market cap, it still constitutes a young market with immense potential in the world and European market. Notably, Europe is the second main consumer of cobots in the world (after Asia) and is home to many of the biggest manufacturers.

Remarkably, the versatility and rapid adoption of cobots (as opposed to industrial robots) shows the potential to accelerate the development of multiple industrial domains. In this context, the continued investment in innovative ways to integrate cobots into every industry should be considered a strategic priority for industrial development.

Since their inception, cobots marked a hiatus from previous robot design trends to prioritize the new and (thus far) unconceived scenario of collaboration between humans and robots. These promises, while ambitious and enticing, have been ahead of the control strategies required to actually enable true collaboration.

The present work constitutes an effort towards showcasing and extending cobots flexibility in collaborative and dynamic environments. It shows that the promises of true human-robot collaboration demand a significant design effort. From the hardware point of view, robots are required to evolve (as introduced with cobots). From a control perspective, human-centric strategies demand an equally significant departure from classical control approaches. In the end, these are fundamental steps towards the true collaboration that will enable augmenting the human-robot synergy.

• Finally, the variable bounds express the decision variable lower b l and upper b u bounds. These can be included into the inequality constraints by adding an identity matrix to C.

Embedded Constraints

Often, Quadratic Program (QP) solvers offer only the linear inequality constraints, as the others can be embedded in them for simplification:

   b l A l d    ≤    I n A C e    x ≤    b u A u d    (A.3)

Simplified formulation

Then, it is possible to arrive at a simplified canonical formulation:

x opt = arg min Note that for the general case of a non-linear system, all these matrices also depend on the state. This means that, the system needs to be linearized at some time instant t n and then A d,n can be assumed constant for a limited time window. The same conclusions can be applied for B, C, D.

Using (B.4) with a sampling period of T s , we arrive at the exact solutions: q(τ) = M(q k ) -1 (τ + b(q k , qk )) (D.7)

A d = e A c
C q = q(τ) ∈ R n | q(τ) ∈ 2 (q mq k -qk ∆t) ∆t 2 , 2 (q Mq k -qk ∆t) ∆t 2 (D.8)

C q = q(τ) ∈ R n | q(τ) ∈ ( qm -qk ) ∆t ,
( qMqk ) ∆t (D.9) where B implies the corresponding quantity is expressed in the body frame (see Section 3.2.4). Additionally:

C τ = {τ ∈ R n | τ ∈ [τ k + τm ∆t, τ k + τM ∆t]} (D.
• τ opt corresponds to the optimal τ that minimizes the cost function; this implies it will be applied in the next step as τ k+1 . On the other hand, the current states of the robot referred to as X k , q k , qk , τ k and are used in (D.7), (D.8), (D.10), (D.11) and (D.12).

• f main (τ) in (D.11) designates the main task cost function. In this case, it cor- responds to a pose tracking task that minimizes the distance between some desired twist derivative ν * (analogous to a rigid body motion acceleration, see (D.12) explained below) and the robot current spatial acceleration, which is computed as a function of the optimizing variable (the joint torques) through the robot dynamics model in (D.7).

• ν * from (D.11) is defined in (D.12) as a Proportional Derivative (PD) term from a trajectory providing a reference pose X r , a twist ν r and a spatial acceleration νr (the latter used as a feed forward term). The proportional gain K p provides a way to modulate the correction from the error term e, which is constructed as the desired twist displacement required to correct the robot pose from X to X r ; in the same way, the derivative gain K d modulates the twist error ė.

• f reg (τ) is a regularisation task or function that helps uniquely determine an optimal solution in redudant robots by acting on the extra degrees of freedom.

In this case, the task tries to maintain a configuration close to the robot mean position relative to its bounds without affecting the main task (because w reg is chosen small enough not to affect it). Then τ reg can be chosen in different ways depending on the targeted application; here, it is chosen to ensure the convergence of the robot configuration to a neutral configuration q reg . The servoing of this configuration can be expressed at the joint torque level using a PD controller such that:

τ reg = K q (q regq) -K q q (D.14) with K q and K q positive proportional and derivative gains.

• C q in (D.8) offers one way to implement (D.3) via the second order taylor expansion on the current robot configuration q k , establishing a relation between the joint bound limits (q m , q M ) and the joint accelerations (and indirectly, the joint torques).

• Similarly, C τ in (D.10) is one possible implementation of (D.5); in this case, it exploits the first order taylor expansion of the current joint torque τ k to incorporate the joint torque derivative limits ( τm , τM ) as a function of the decision variable, the joint torque.

• Finally, (D.6) offers a way to incorporate linear constraints. For instance, it can be used to express workspace-related limits like the geometric space or, for example, the spatial acceleration constraints can be formulated as:

νb l ≤ J q(τ) ≤ νb u (D.15)
Given the interpolated first step of the trajectory computed by the Model Predictive Control (MPC), task space inverse dynamics has to be performed to compute the input joint torque for the robot.

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

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

E.2 Deceleration Stage ↑

When the initial acceleration is zero or in the opposite sens of the initial velocity. Note that if an acceleration reversing stage was necessary, then a k = 0, v k = v R (t R ).

j D (t) =                0 t = 0 j M 0 < t <

4 . 3 A

 43 linear MPC is used to compute a position and orientation trajectory towards a target (plastic brick) in a receding horizon. (a) and (b) highlight the effects of changing the task space constraints on the fly (in this case max velocities) on the covered distance for one horizon. (c) and (d) illustrate the continuous adaptation of the trajectory when the target location is modified online. . . . . . . . . . . . . . . . . . . . 4.4 The experimental setup is composed of a torque-controlled 7-DoF Franka Emika Panda serial robot and an Optitrack camera array.

  . . . . . . . . . . . . . . . . . . . . . . . . . 5.18 Normalized reference TAP trajectory generated with [Berscheid et al., 2021b]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.19 Smooth stopping trajectory with acceleration reversing, where the initial velocity and acceleration have the same direction, requiring a max-jerk trajectory to inverse the acceleration. . . . . . . . . . . . . . . 5.20 When the initial acceleration and velocity have opposed sense, the acceleration reversing stage is not necessary. . . . . . . . . . . . . . . . 5.21 When the initial acceleration and velocity have opposed sense, but the initial velocity is low enough that the Deceleration stage does not require a constant acceleration part. . . . . . . . . . . . . . . . . . . . . 5.22 revisited ↑

2. 1

 1 Shows the average compute time and error for the horizons shown in Figure 2.10. 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. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1 Values of the gains and weights retained for the inverse dynamic solver used for task-space tracking shown in Figure 4.2 and described in Section 4.2. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 Cartesian constraints for ν, ν, ν 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. . . . . . . . . . . . . 5.1 Overview of the experimental validation, showing the experimental objectives, scenarios and setups used in each case. . . . . . . . . . . . . 5.

Figure 1 . 1 :

 11 Figure 1.1: Example images of heavyly automated tasks with industrial robots. Obtained from [ Stellantis Communication, 2023]; [Julien Cresp Groupe PSA, Direction de la communication, 2017a].

Figure 1 . 2 :

 12 Figure 1.2: Stellantis brands, after the merger in 2021.

Figure 1 . 3 :

 13 Figure 1.3: Example of a robot deployed inside a cage. Obtained from [Groupe PSA Communication Metz Tremery, 2018]; [Stellantis Communication, 2022a].

Figure 1 . 4 :

 14 Figure 1.4: Collaborative robots are often used in non-collaborative tasks. [Julien Cresp Groupe PSA, Direction de la communication, 2017b].

Figure 1 . 5 :

 15 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].

  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 ↑

  Orientation representation in 3D space.

Figure 2 .

 2 Figure 2.1: A simple way to represent 3D positions and orientations in space is through vectors.

Figure 2 . 2 :

 22 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.

Figure 2 .

 2 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 inFigure 2.3(b).

  An example re-orientation where the rotation axis is coincident with the z axis. φ φ (b) A more complex example of a 3D rotation with an arbitrary rotation axis.

Figure 2 . 3 :

 23 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.

  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.

Figure 2 .

 2 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.

Figure 2 . 4 :

 24 Figure 2.4: Representation of a pose in 3D space.

Figure 2 . 5 :

 25 Figure 2.5: Representation of a position path in 3D space.

Figure 2 . 6 :

 26 Figure 2.6: Representation of a pose path in 3D space.

Figure 2 . 7 :

 27 Figure 2.7: Depiction 2 of an integration scheme in the tangent space.

Figure 2 . 8 :

 28 Figure 2.8: Depiction of the path taken taking the end-effector of a Franka Emika robot from an initial to a target pose X i , X t ∈ SE(3).

  Figure 2.10: Variable speed integration using ME2 for a 300ms horizon.

1 :

 1 Shows the average compute time and error for the horizons shown in Figure2.10.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 2 .

 2 Figure 2.11: Average error for different horizon lengths.

Figure 3

 3 Figure 3.1:revisited ↓ 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 intervens 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.

Figure 3 . 2 :

 32 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.

Figure 3 . 3 :

 33 Figure 3.3: There exist multiple paths that connect some initial and target poses X i , X t ∈ 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 X k = e (1-α k )ξ 0 +α k ξ t for some α(t) ∈ [0, 1].

Figure 3 . 4 :

 34 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.

Figure 3

 3 Figure 3.5: revisited ↑ ↓ 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 taskspace online trajectory re-planning over a receding horizon (MPC block) and a highfrequency 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.

Figure 3 .

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

Figure 3 . 6 :

 36 Figure 3.6: The planned horizon in time. The online MPC progressively "discovers" a path from the initial pose (on the left) towards the target pose (on the right). Given the MPC optimizes over a receding time window, the optimizing horizon does not cover the complete path until it is close enough to the target pose.

Figure 3 .

 3 Figure 3.7: Numerical simulation of the replanning controller for 1-Degree of Freedom(DoF). This example shows the result of employing the MPC to replanify at 50Hz and apply the resulting twists for a Cartesian motion along one of the axis (x from the frame attached to the robot base). To compare optimality, the output is compared to the jerkbounded algorithm in[Berscheid et al., 2021b].

Figure 3

 3 Figure 3.8: Velocity modulation experimental setup. The photo shows the setup. The robot is placed on a table to perform a task provided on-the-fly. The distance to the operator d h used for safety-aware velocity modulation is measured via the laser sensor.

  and 3.8 (in the real setup); andFigure 3.6 (in the simulation). The following values are used to modulate the robot velocity according to(3.52):

Figure 3 . 9 :

 39 Figure 3.9: Shows the result of requesting the robot to move between 4 preset poses.While the robot moves, a laser sensor is used to capture the minimal distance to a human and modulate the maximum velocity (shown in red shade). The error curves show the distance between the current position and orientation (p k , R k ) and the preset target poses (p t , R t ). The linear error is e = p tp k , while the angular error is computed as φ = log(R -1 k R t ). The MPC (T=300ms, h = 10, ∆ h = 30ms, f MPC = 50Hz) modulates the body twist ν = [v T , ω T ] T respecting the safe limits imposed even when they become so low that the robot stops moving.

Figure 4

 4 Figure 4.1:revisited ↑ ↓ 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.

Figure 4 . 3 :

 43 Figure 4.3: A linear Model Predictive Control (MPC) is used to compute a position and orientation trajectory towards a target (plastic brick) in a receding horizon. (a) and (b) highlight the effects of changing the task space constraints on the fly (in this case max velocities) on the covered distance for one horizon. (c) and (d) illustrate the continuous adaptation of the trajectory when the target location is modified online.

Figure

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

Table 4 . 2 :

 42 Cartesian constraints for ν, ν, ν for the MPC during the experiment. The MPC used for motion generation is shown in Figure4.2 and described in Section 4.2.

Figure 4 .

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

Figure 4 .

 4 Figure 4.7: QR code for the video of the high-frequency acceleration-based Model Predictive Controller experiment for full-pose online constrained motion generation. On PDF file readers, it is also a clickable hyperlink.

Figure 5 . 3 :

 53 Figure 5.3: The translational velocity polyhedron. It visualizes the feasible velocity set P ṗ| Q as defined in (5.8) once transformed into the "equivalently occupied space".Multiplying the set by a small time interval ∆t = 0.15 corresponds to the space the endeffector 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.

5 2. 3 .

 53 Zonotope constraints with non-zero initial conditions shown in Figure5.6 Polyhedral constraints with non-zero initial conditions conditions shown in Figure5.7

Figure 5 . 4 :Figure 5 . 5 :Figure 5 Figure 5

 545555 Figure 5.4: The translational convex reachable space approximation polyhedron. This illustrates the attainable space for two different configurations. It presents P p|Q obtained from (5.21) with the multiplication of the Jacobian J l by a scaling factor of 0.15. This scaling is essential for visualization since the approximation becomes coarse in extreme cases, particularly those farthest from the joint boundaries. The first configuration, displayed in Figure5.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.

Figure 5

 5 Figure 5.9: An example of the back and forth scenario (as defined in Section 5.4.3) used for the experiments, showing the starting and ending poses on the real setup. This scenario is also used in simualtions but the video shows one of the experiments being executed on the robot.

Figure 5 .

 5 Figure 5.11: This image showcases the results of a joint-bounds aware task-space planning addressed in Section 5.4.6. The experiment corresponds to a simulation of a motionless planning scenario (as defined in Section 5.4.3) in which the robot is driven to a configuration near its joint bounds so that it cannot move towards the target cube. When the joint bounds limits are disabled (in (a), (b) and (c)), the robot plans a horizon path through the geodesic towards the target. Conversely, once the joint bounds limits are employed in (d) and (e), the horizon becomes very small and tries to find a path that minimizes the distance to the target without violating the joint bounds constraints.

Figure 5

 5 Figure 5.13: Simulation: Showcases a back and forth task scenario between two poses without changing the orientation, while changing the maximum allowed q (showed with R q) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints.

Figure 5

 5 Figure 5.14: Simulation: Showcases a back and forth task scenario between two poses that have different orientations, while changing the maximum allowed q (showed with R q) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints.

Figure 5

 5 Figure 5.15: Real-world experiment: Showcases a back and forth task scenario between two poses that have the same orientation, while changing the maximum allowed q (showed with R q) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints.

Figure 5

 5 Figure 5.16: Real-world experiment: Showcases an interactive task scenario where the robot tracks a pose captured in real-time while changing the maximum allowed q (showed with R q) to demonstrate the MPC capability to achieve a taskspace trajectory that takes into account joint level constraints in an online fashion. The corresponding video is shown in Figure5.10.

Figure 5 .

 5 Figure 5.17: A smooth jerk-bounded (TAP) stopping trajectory example for a worstcase scenario: v 0 = -0.7v M , a 0 = -a M (acceleration starts at maximum value and not inversed to velocity while velocity starts near its maximum value).

Figure 5 .

 5 Figure 5.18: Normalized reference TAP trajectory generated with [Berscheid et al., 2021b].

Figure 5 .Figure 5 .

 55 Figure 5.19: Smooth stopping trajectory with acceleration reversing, where the initial velocity and acceleration have the same direction, requiring a max-jerk trajectory to inverse the acceleration.

x 1 2 x

 2 T Qx + g T x s.t.A l ≤ Ax ≤ A u(A.4) A.1 Weighted Least Square ↑The following least-square optimization problem can be reformulated in a standard quadratic problem form as in (A.1a positive semi-definite matrix containing weights the optimization variable.Given:∥Ex -b∥ 2 W = (Exb) T W (Exb) (A.6) = x T E T W Ex -2b T W Ex + b T b (A.7)then we can express (A.5) in the initial formulation of the QP optimization (A.1):Q = 2E T W E, g T = -2b T W E, r = b T b (A.8)A commonly used case of this least squares optimization is for when the weight matrix is an identity W = I (giving equal weight to all elements in the decision vari-The time-discrete equivalent of (B.1) and (B.2) are:x k+1 = A d x k + B d u k (B.5)y k = Cx k + Du k (B.6)

e

  A c τ δτB c = KB c (B.8) with K = A -1 c (e A c T s -I n ) (B.9)It is important to note that (B.8) is defined even if A -1 c does not (A c not reversible), as shown by developing the expressions though their taylor series: joint accelerations through the robot Jacobian, enabling a QP formulation of the taskspace tracking problem: τ opt = arg min τ f main (τ) + w reg f reg (τ)

2 ν 2 (

 22 10)f main (τ) = || B ν * -ν(τ)|| 2 (τ) = J(q k ) q(τ) (D.11) B ν * = K p B e + K d B ė + B νr e, ė, ν, ν ∈ se(3) B e = log(X -1 k X r ) X ∈ SE(3) B ė = B ν r -B ν k (D.12) f reg (τ) = ||τ reg -τ|| 2

Table 2 .

 2

3 Constrained Motion Generation with Model Predictive Control

  Rationale: Constrained Online Motion Generation . . . . . . . . 3.1.2 Control Architecture Proposal . . . . . . . . . . . . . . . . . . . . 3.2 Task-space Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2.1 Kinematic Robot Model . . . . . . . . . . . . . . . . . . . . . . . 3.2.2 Pose Tracking as a Quadratic Program . . . . . . . . . . . . . . .

3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.1.1 3.2.3 Constraints Formulation . . . . . . . . . . . . . . . . . . . . . . . 3.2.4 On the Solver Reference Frame . . . . . . . . . . . . . . . . . . . 3.. . . 3.3.1 Problem Statement: Motion Generation via MPC . . . . . . . . . 3.3.2 Linear Model Predictive Control . . . . . . . . . . . . . . . . . . 3.3.3 Optimization on the Pose Manifold . . . . . . . . . . . . . . . . 3.3.4 Geodesic Path in the Tangent Space . . . . . . . . . . . . . . . . 3.3.5 Task-Space Linear MPC in the Tangent Space . . . . . . . . . . . 3.4 Experimental Validation: The Case of Velocity Modulation . . . . . . 3.4.1 Implementing Online Re-planning . . . . . . . . . . . . . . . . . 3.4.2 Numerical Simulation . . . . . . . . . . . . . . . . . . . . . . . . 3.4.3 Experimental Scenario & Safety Constraints . . . . . . . . . . . 3.4.4 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  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].

  .59) 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.

	Control Architecture Implementation			Unmodeled Dynamics
	Target X t	Task-space Dynamic Trajectory Planner	Horizon Trajectory X mpc , ν mpc , νmpc	Inverse Dynamics Solver	Input τ	Robot	HRI	Human
	1kHz	Constraints X max , ν max , νmax , νmax	Workspace Current State X , ν, ν	Analysis Workspace Current State X , ν, q, q		Environment Dynamics
			Configuration
	Figure 4.2:							revisited ↑ ↓

Table 4 .

 4

1: Values of the gains and weights retained for the inverse dynamic solver used for task-space tracking shown in Figure

4

.2 and described in Section 4.2.

Table 5 .

 5 1: Overview of the experimental validation, showing the experimental objectives, scenarios and setups used in each case.

  3.5. Roughly speaking, this architecture is composed of an MPC block

	Name	Symbol	Value
	Horizon Steps	h	5
	Step Duration	T	100ms
	Horizon Duration	hT	0.5s
	Terminal Weight	w T	1, T
	Terminal Weight Matrix	P T	I 6×6 0 6×6 0 6×6 I 6×6 T
	Regularization Weight	γ	10 -2 T/2

Table 5 .

 5

2: MPC parameters used in this implementation. In the case of w T , the terminal weight is homogeneized to account for the dimensional difference in the state (i.e. positions and velocities) as defined in

(5.60)

.

•

  Real Robot: The hardware experimental setup is the same one used in Figure 4.4. When used, the plastic pick in the scene is detected in real time via an infra-red Optitrack camera array.

  Joint 1 Joint 2 Joint 3 Joint 4 Joint 5 Joint 6 Joint 7 Unit

	q ′ M	1.7384 1.0577 1.7384 -0.0698 1.7384 2.2515 1.7384 rad
	q ′ m	-1.7384 -1.0577 -1.7384 -1.8431 -1.7384 -0.0175 -1.7384 rad

Table 5 .

 5 5: Artifitially limiting the joint bounds to 60% of the range for experimental valudation yields the limits shown here. The full range are shown inTable 5.4.

  yields the viable Joint-space Figure5.21: When the initial acceleration and velocity have opposed sense, but the initial velocity is low enough that the Deceleration stage does not require a constant acceleration part. P ṗ, P ṗ, P p, P p

		a k a D a M	j M	-v k Deceleration j M -
		0	t 1	t 3	t
				t stop
		-a M			v k ≥ 0 a k ≤ 0
	Workspace Analysis Convexification Task-space Projection Viability Filter P p|Q Workspace Constraints P p|W , P ṗ|W Robot Q V , QV Q, ... Q Capacities Q, Q, Q, ... Q T , Ṫ	Workspace Configuration Pp , Robot Constraints C τ|Q V , C τ| QV C τ|T , C τ| Ṫ
	Figure 5.22:	Polytope constraints from compatibilized joint-space constraints.	revisited ↑

V

Measurements

q k , qk , qk , τ k

  < t ≤ t 1 v D (t 1 ) + a M t t 1 < t ≤ t 12 v D (t 12 ) + a M t -< t ≤ t 1 ∆p D (t 1 ) + v D (t 1 )t + a M t 2 2 t 1 < t ≤ t 12 ∆p D (t 12 ) + v D (t 12 )t + a M t 2 2 -j M t3 6 t 12 < t ≤ t 123 ∆p D (t 123 ) t 123 < t Benhabib, Nassim, Vincent Padois, and David Daney (May 2020). "Securing Industrial Operators with Collaborative Robots: Simulation and Experimental Validation for a Carpentry task". In: ICRA 2020 -IEEE International Conference on Robotics and Automation. IEEE ICRA Best Paper Award in Automation. Paris, France. (Cited on page 153) Gambao, Ernesto (2023). Analysis exploring risks and opportunities linked to the use of collaborative industrial robots in Europe. Tech. rep. Panel for the Future of Science, Technology (STOA), and managed by the Scientific Foresight Unit, within the Directorate-General for Parliamentary Research Services (EPRS) of the Secretariat of the European Parliament.

							t 1
			0				t 1 ≤ t < t 12	(E.9)
			-j M				t 12 ≤ t < t 123
			0				t 123 ≤ t
	a D (t) =	              	a k a k + j M t a M -j M t 0 a M			t = 0 0 < t < t 1 t 12 < t < t 123 t 3 ≤ t t 1 ≤ t ≤ t 12	(E.10)
	v D (t) =	              	-v k -v k + a k t + 0	j M t 2 2	t = 0 t 123 ≤ t t 12 < t < t 123 0 j M t 2 2	(E.11)
	∆p D (t) =	              	0 -v k t -	j M t 3 6			t = 0 0 (E.12)
					6		+ (-v k + a k t 1 +	j M t 2 1 2	+ a M t 12 )t 123	(E.19)
	Two cases considered:
			1 ⃝ 0 ≤ a k ≤ a m , v k ≤	2a 2 m -a 2 k 2j M	(E.20)
					t 3 =	a m j m	, t 1 =	a m -a k j m	(E.21)
	Only a triangular trajectory is required.
				2 ⃝ 0 ≤ a k , v k >	2a 2 m -a 2 k 2j M	(E.22)
	A full TAP trajectory is required.
							(E.23)

(Cited on page 153)

Based on tikz.net/manifold.

In differential geometry, the pushforward refers to a linear approximation of smooth mappings that operates on tangent spaces.

Expressions for these can be found inLynch and Park's "Modern Robotics: Mechanics, Planning, and Control" (2017) [START_REF] Lynch | Modern Robotics: Mechanics, Planning, and Control[END_REF]. 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).

The code used for experiments in this chapter is publicly available at: https://gitlab.inria.fr/auctus-team/publications/shared-paper-code/ark2022-lin

In this case, control architecture is used in a vague way to refer to both the task design process and control during execution.

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.

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.

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.

Linear constraints can be used to formulate convex polytopes as half planes.

Throughout this work, the log and exp operations adopts 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 (seeSections 2.2.1.3 and 2.2.2.2) or[START_REF] Solà | A micro Lie theory for state estimation in robotics[END_REF];[Torres Alberto et al., 2022a].

 7 The one to one correspondence is maintained under the conditions detailed in[START_REF] Forster | On-Manifold Preintegration Theory for Fast and Accurate Visual-Inertial Navigation[END_REF] and[Torres Alberto et al., 2022a] (Section 2.1). Otherwise, the exp map is

surjective.8 Given the adopted vector form of the tangent space in this work, the dlog ξ k takes the form of a 6 × 6 matrix.

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

The experiments were run on desktop computer equipped with a CPU i9-10900K and

32GB of ram.3 https://gitlab.inria.fr/auctus-team/people/nicolas-torres/lmpcpoly-ws/

Bounded convex polyhedron are also referred to as polytopes[Fukuda et al.,

2004].

https://auctus-team.gitlabpages.inria.fr/team-members/antunskuric/

Prioritizing notation clarity over consistency, the Forward Kinematics function is denoted with the same name (as FK(q)) both in the position sense (which outputs a 3-vector position) as well as in the full pose sense, which outputs a homogeneous transformation matrix in SE(3).

https://frankaemika.github.io/docs/franka _ ros.html

https://gitlab.inria.fr/auctus-team/people/nicolas-torres/lmpcpoly

https://gitlab.inria.fr/auctus-team/people/nicolas-torres/lmpcpoly-ws

(Cited on pages 10, 16, 59, 68, 79)

: (cited on page 64)

(Cited on page 115)

(Cited on page 152)

Chapter 3

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

Efficient workspace sharing of collaborative robots and human operators 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 posteriori safety measures: it must be tackled at the control level.

To address the need of adaptation to human presence as well as to endow 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 orientation 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.

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

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

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

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

Chapter 5

Actuation-aware Task-space Model Predictive Control

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

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

Through experiments with Franka Emika robots, the relevance and effectiveness of this control architecture are demonstrated in scenarios where tasks are dynamically modified in real-time, showcasing its potential in real-world applications of collaborative robotics. revisited ↑ ↓ The proposed modular control architecture is based on a cascade loop composed of: 1) a task-space Model Predictive Controller (Model Predictive Control (MPC)) for online re-planning; 2) a constrained Task Space Inverse Dynamics solver to follow the planned trajectory. The whole control loop runs synchronously at the control rate. The Workspace Analysis block is in charge of sensing the environment, treating the information and transmitting the relevant information (workspace configuration and constraints) to the controller blocks. point of view; and other times, due to the non-linear dynamics of the actuators, it may constitute an overvaluing. This last case could be considered potentially more dangerous as it breaks the underlying assumptions of the operating conditions, effectively increasing the risk of implicitely driving the system outside specifications by design.

CONTENTS

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

Actuation-aware Control Framework ↑

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

The design principles underneath this control architecture remain consistent with the modular and task-centric point of view of this work. The modularity stems from CONTENTS

From Joint-space Constraints to Task-space Feasible Motions ↑

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

A first step towards this objective is to extend the mathematical model introduced in previous chapters to relate robot reconfiguration trajectories to task-space motions. This can be achieved by revisiting the kinematic and dynamic ascpects from previous chapters, through the subsequent time derivatives of the twist: revisited ↑ ν = J(q) q (5.4) revisited ↑ ν = J(q) q + J(q, q) q (5.5) ν = J(q) ... q + J(q, q, q) q + 2 J(q, q) q (5.6) ν, ν, ν ∈ se(3) q, q, q ∈ R n

where J refers to the robot analytical Jacobian and J, J to its time derivatives; moreover, ν, ν, ν respectively denote the twist, spatial acceleration and jerks generated by a given joint velocity q, acceleration q and jerk ... q .

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

J l , Jl , Jl , J a , Ja , Ja ∈ R 3×n which separate the upper and lower (in the convention used here) parts of the matrices.

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

where q k , qk , qk denote the measured robot state at a time instant k; the CONTENTS the frame of quantities in the tangent space.

And now, the safety-related constraints denoted with an S can be formulated: 5.69) where now the bounds are expressed in the body frame. So, for instance, ν M|S denotes the maximum body twist.

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

again with the same clarifications about the frame. Then, the set of workspace constraints can be formulated (denoted with W):

P ξ|W = P ξ|S ∩ P ξ|M ∩ P ξ|A (5.73) P ν|W = P ν|S ∩ P ν|M ∩ P ν|A (5.74) P ν|W = P ν|S ∩ P ν|M ∩ P ν|A (5.75) P ν|W = P ν|S ∩ P ν|M ∩ P ν|A (5.76) where the sub-index A denotes arbitrary and application-dependent sets.

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

ξ ∈ P p P ξ = P ξ|Q (q k ) ∩ P ξ|W (5.79) ν ∈ P ν P ν = P ν| Q(q k ) ∩ P ν|W (5.80) ν ∈ P ν P ν = P ν| Q(q k , qk ) ∩ P ν|W (5.81) ν ∈ P ν P ν = P ν| ... Q (q k , qk , qk ) ∩ P ν|W (5.82) where X k , ν k respectively designate the initial pose and twist. These equations expresses the convex sets that need to be to be formulated as linear constraints as in (4.40) and (4.41).

CONTENTS

rate.

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

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

Experimental Setups & Software Infrastructure ↑

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

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

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

• Simulation: The simulation setup is based on the Gazebo [START_REF] Koenig | Design and use paradigms for gazebo, an open-source multi-robot simulator[END_REF] physics engine (interfaced through Franka's libraries).

CONTENTS

error is:

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

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

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

• Joint Velocities: This shows the raw values for the joint velocities as measured by the robot. They are computed with a vector q.

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

• Normalized Non-kernel Joint Velocities: Similar to the previous normalized definition, this shows the joint velocities as normalized against the maximum joint velocities specified by the manufacturer (see Table 5.4). However, in this case, only the components not corresponding to the movements in the kernel of the Jacobian are accounted for. The joint velocities component projected in the kernel are denoted as qker and the normalized joint velocities, once they are removed, are denoted with q′ . For each joint i, they are computed as: 5.88) where J + corresponds to the right pseudo inverse (or right Moore-Penrose inverse) of the Jacobian.

Joint Configuration (rad) q 1 q 2 q 3 q 4 q 5 q 6 q 7 0 2

(rad)

q 1 q 2 q 3 q 4 q 5 q 6 q 7 R q Figure 5.12: Real-world Experiment: Showcases a back and forth task scenario between two poses that have the same orientation. The objective is to show the robot behavior while the allowed joint range is artificially reduced through a factor R q beween 0 and 1 (also shown in the plot) to demonstrate the MPC capability to achieve a task-space trajectory that takes into account joint level constraints. The passage from R q = 0.6 to R q = 1 is highlighted with a dashed vertical line.

Appendix A

Quadratic Programming

Quadratic Programming is a technique that permits optimizing some vector variable x with respect to some second order cost function f (x) and some linear equality and inequality constraints (lower/upper bounds).

The canonical QP optimization problem is formulated as:

where:

• x ∈ R n×1 designates the optimizing variable (also referred to as the decision variable).

• Q ∈ R n×n contains the quadratic cost function coefficients.

• g ∈ R 1×n refers to the linear coefficients, also referred to as the gradient vector.

• r ∈ R refers to a constant cost term, independent of the decision variable, and so it can be neglected, as it doesn't affect the result of the optimization.

• The inequality constraints are expressed as a linear system described with the lower and upper bounds vectors C l , C u and the coefficients matrix A that relates them to the optimizing variable. This constitutes the most general set of constraints and is often used to formulate the other onces. Some solvers only offer this alternative (and equivalent) formulation:

• The equality constraints are expressed as a linear system described with the vector d and the coefficients matrix C that relates them to the optimizing variable.

They can be embedded into the inequality constraints by employing d both as the lower and upper bounds.

Appendix B

Discretization of Continuous LTI systems

A dynamic system can be parameterized with state x ∈ R n and input u ∈ R m vectors (where n 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 the system to some desired or target state.

Given some non-linear system dynamics described with f (x, u), the continuous state-space system representations is:

with:

where:

ẋ, x: represent the system state and its derivative u: represents the system input y: represents the system observed state A c , B c : LTI matrices that express the relation between the system state derivative and its state and input C, D: LTI matrices that express the relation between the system observed state and its state and input

The solution to the differential equation is:

)

Appendix C

Summary: Velocity-based Pose Tracking as a Quadratic Program

The inverse kinematic problem consists of finding the robot configuration satisfying some operational-space position and orientation. This section introduces a way to solve this problem for a robot that admits being controlled by its joint velocities.

This controller is similar to the one in [START_REF] Joseph | Online velocity constraint adaptation for safe and efficient human-robot workspace sharing[END_REF]. It is formulated as a linearly constrained Quadratic Program (QP) that optimizes for the joint velocity; the main difference being the added generic linear constraints in (C.5) that will be detailed shortly. The resulting QP formulation is:

q, qm , qM ∈ R n with:

where B implies the corresponding quantity is expressed in the body frame (see Section 3.2.4). Additionally:

• qopt corresponds to the optimal q that minimizes the cost function; this implies it will be applied in the next step as qk+1 . On the other hand, the current states of the robot referred to as X k , q k , qk and are used in (C.6), (C.7), (C.8), (C.9) and (C.9).

• f main ( q) in (C.8) designates the main task cost function. In this case, it corre- sponds to a pose tracking task that minimizes the distance between some desired twist ν * (see (C.9) explained below) and the robot current twist, which is computed as a function of the optimizing variable (the joint velocities) through the robot Jacobian J(q).

• ν * from (C.8) is defined in (C.9) as a Proportional Derivative (PD) from a trajectory providing a reference pose X r and twist ν r . The proportional gain K p provides a way to modulate the correction from the error term. Finally, e is constructed as the desired twist displacement required to correct the robot pose from X to X r .

• f reg ( q) is a regularisation task or function that helps uniquely determine an optimal solution in redudant robots by acting on the extra degrees of freedom.

In this case, the task tries to maintain a configuration close to the robot mean position relative to its bounds. The details can be found in [START_REF] Joseph | Online velocity constraint adaptation for safe and efficient human-robot workspace sharing[END_REF]. Then w reg is chosen small enough not to affect the main task.

• C q in (C.6) offers one way to implement (C.3) via the first order taylor expansion on the current robot configuration q k , establishing a relation between the joint bound limits (q m , q M ) and the joint velocities.

• Similarly, C q in (C.7) is one possible implementation of (C.4); in this case, it exploits the first order taylor expansion of the current joint velocity qk to incorporate the joint acceleration limits ( qm , qM ) as a function of the decision variable, the joint velocities.

• Finally, (C.5) offers a way to incorporate linear constraints. For instance, it can be used to express workspace-related limits like the geometric space or, for example, the twist constraints can be formulated as:

Appendix D

Summary: Torque-based Pose Tracking as a Quadratic Program

An inverse kinematics solver as a Quadratic Program (QP) is presented in Appendix C. This section extends the previous strategy to formulate an inverse dynamics solver. While both problems involve working backwards to solve for unknowns, inverse kinematics focuses on the geometric view of the problem, solving for the joint angles necessary to achieve a certain end-effector pose, ignoring the required forces or torques. In contrast, inverse dynamics deals with the problem of finding the torques and forces required for an end-effector motion. This accounts for the system dynamic properties such as the mass, inertia, external forces, etc.

The controller presented here is similar to the one in [Joseph et al., 2018b]. It is designed for torque-controlled robots. By exploiting the robot dynamics model (see Section 4.2.1), we can formulate the joint accelerations as a function of the joint torques. Furthermore, the Cartesian spatial acceleration can then be related to the For convenience and to further clarify how the different components are connected, this Section revisits some of the concepts previously introduced in Section 4.2.2:

where X mpc , ν mpc , νmpc are respectively the interpolated desired pose, twist and acceleration outputted by the MPC; X , ν are the measured pose and twist of the robot and K p and K d are positive proportional and derivative gains.

Given this control acceleration, one can solve task-space inverse dynamics under constraints through a QP formulation, similar to the one in [Joseph et al., 2018b].

Appendix E

Smooth stopping trajectory

The smooth stopping analysis contemplates two stages in the trajectory, as depicted in Figure 5.19 and Table 5.6.

E.1 Acceleration Reversing Stage ↑

When the initial acceleration and velocity are in the same sens. This considers the two following starting conditions:

and finally:

where: (Cited on page 59) Tan, Yang (Mar. 2016). "Continuous tasks and constraints transitions for the control of robots". Theses. Université Pierre et Marie Curie -Paris VI.

(Cited on page 62) Mayne, D.Q. et al. (2000). "Constrained model predictive control: Stability and optimality". In: Automatica 36.6, pp. 789-814.

(Cited on page 62) Bard, Yonathan (1974). Nonlinear parameter estimation. New York: Academic Press.

(Cited on page 62) Alexis, K., G. Nikolakopoulos, and A. Tzes (Aug. 2012). "Model predictive quadrotor control: Attitude, altitude and position experimental studies". English (US). In: IET Control Theory and Applications 6.12, pp. 1812-1827.

(Cited on page 62)

Bemporad, Alberto et al. (2002). "The explicit linear quadratic regulator for constrained systems". In: Automatica 38. 2017). "An industrial security system for human-robot coexistence". In: Industrial Robot: An International Journal.

(Cited on page 67) Mainprice, Jim, Rafi Hayne, and Dmitry Berenson (2016). "Goal set inverse optimal control and iterative replanning for predicting human reaching motions in shared workspaces". In: (Cited on pages 90, 124) Skuric, Antun et al. (2022b). "On-line feasible wrench polytope evaluation based on human musculoskeletal models: an iterative convex hull method". In: IEEE Robotics and Automation Letters. (Cited on page 106) Fukuda, Komei et al. (2004). "Frequently asked questions in polyhedral computation". In: ETH, Zurich, Switzerland 85.

(Cited on page 106) Skuric, Antun, Vincent Padois, and David Daney (Sept. 2022c). "Approximating robot reachable space using convex polytopes". In: 15th International Workshop on Human-Friendly Robotics. Delft, Netherlands.

(Cited on page 107)