# Whole-Body Kinematics Modeling in presence of Closed-Linkages: application to the Kangaroo Biped Robot

Enrico Mingo Hoffman, Sai Kothakota, Adria Moreno, Andrea Curti, Narcis Miguel, Luca Marchionni

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

## ► To cite this version:

Enrico Mingo Hoffman, Sai Kothakota, Adria Moreno, Andrea Curti, Narcis Miguel, et al. Whole-Body Kinematics Modeling in presence of Closed-Linkages: application to the Kangaroo Biped Robot. International Conference on Robotics and Automation (ICRA) 2022, Workshop on "New frontiers of parallel robotics" (second edition), May 2022, Philadelpia, U.S. Virgin Islands. hal-03652472

HAL Id: hal-03652472

https://hal.science/hal-03652472

Submitted on 26 Apr 2022

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

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

<figure><img src="image_2.png" /></figure>
# Whole-Body Kinematics Modeling in presence of Closed-Linkages: application to the Kangaroo Biped Robot

Enrico Mingo Hoffman, Sai Kishor Kothakota, Adria Roig Moreno, Andrea Curti, Narcis Miguel and Luca Marchionni

***Abstract***—This short paper presents preliminary studies and results on the kinematic modeling and control of a novel bipedal platform designed and built by PAL Robotics, named *Kangaroo*, characterized by the presence of multiple closed and parallel linkages. These kinematic structures are known to have superior mechanical performance at the cost of increased modeling and control complexity. We introduce the methodology to handle such complexity applied to the lower body of the Kangaroo robot, in particular considering whole-body formulation based on quadratic programming optimization. The presented approach is finally validated both in simulation and with experiments performed on the real platform.

## I. INTRODUCTION

The presence of closed and parallel linkages in robots have multiple advantages w.r.t. open-kinematic chains, e.g. the possibility to relocate actuation to achieve better mass and inertia distribution, superior stiffness and high payload-to-weight ratio [1]. Despite these clear advantages, open-kinematic chains remain a popular choice, particularly in legged robotics, due to simpler design and control. However, closed linkages are becoming more and more used in the new generation of biped robots, in particular to achieve highly dynamic movements with high resilience to impact. In fact, the new BostonDynamic Atlas [2] and AgilityRobotics Cassie/Digit [3], present closed linkages in their mechanical design. In particular, Cassie/Digit uses electric actuators localized near its torso/pelvis with a transmission, consisting of closed-bar linkages, to transfer the motion in order to reduce inertia, especially during impacts while walking.

Following this new trend in humanoid robotics, in this work we present preliminary studies on the kinematic modeling and control of the legs of the Kangaroo platform, a novel bipedal platform recently designed and developed at PAL Robotics. Kangaroo follows a series of PAL humanoid robots, started back in 2004 with the REEM-A, passing through the REEM-B and C series, and TALOS [4] platform, as shown in Figure 1. The Kangaroo robot has been designed using novel high-power and robust linear actuation units located near the pelvis area, while the motion is transferred to joints through a complex system of closed parallel linkages. This design entails multiple advantages such as robustness during impact, low inertia at the end-effectors and light-weight leg structure, making Kangaroo the ideal platform to perform highly dynamic and contact

rich movements. We introduce how we modeled the legs of Kangaroo and how to encompass this complex systems of closed linkages into Quadratic Programming (QP)-based Whole-Body controllers.

This short paper is organized as follows: Section II briefly overviews the related works, Section III introduces the Kangaroo platform and its modeling, Section IV presents an approach to QP-based whole-body control considering both actuated and passive DOFs as optimization variables, Section V presents results of the proposed approach and finally Section VI discusses conclusions and future works.

<figure><img src="image_3.png" /><figcaption>Fig. 1: Humanoid bipedal robots designed and produced by PAL Robotics since 2004.</figcaption></figure>

## II. RELATED WORKS

Several bipedal humanoid robots present closed linkages, in particular parallel four-bar mechanisms for the actuation of the pitch-roll ankle joints [5]–[7]. However, in this case the actuators are moved along the shin, making the advantages of such designs localized only to the ankle part. Also, concerning the control, most of the time the mechanism is considered isolated from the full-model of the robot and the joint side is treated as actuated while performing control, possibly introducing errors when considering linear joint limits.

Due to the recent development of bipedal legged system with closed linkages, several works on the control and modeling side are emerging. For example [8], proposes a novel efficient method for solving forward dynamics equations involving constraints, such as closed linkages. In [9], a robust Inverse Kinematics (IK) algorithm for robots with closed linkages is proposed.

The purpose of this short paper is to describe how the complex closed linkages in Kangaroo legs are modeled and included inside a hierarchical whole-body QP-based controller.

<footnote>All the Authors are with PAL Robotics, Carrer de Pujades 77, 08005 Barcelona, Spain. E-mail: {enrico.mingo, sai.kishor, adria.roig, andrea.curti, narcis.miguel, luca.marchionni}@pal-robotics.com</footnote>
### III. THE KANGAROO ROBOT

The Kangaroo robot is actuated by 12 linear ball-screw modules with electrical motors, 6 per leg, while the complete leg can be modeled by 76 DOFs, among them, 64 are passive. Figure 2 shows the location of the actuators in Kangaroo, 5

<figure><img src="image_4.png" /><figcaption>Fig. 2: On the left, side and front view of the Kangaroo robot, on the right, side and front view of Kangaroo model with frames visualized in RViz.</figcaption></figure>

out of 6 are placed at the hip and only 1 placed at the femur. The actuation is reported at the joint by a series of closed and parallel mechanisms consisting by several four-bar linkages and differential linkages.

The peculiar design of these linkages on Kangaroo permits to move the hip in the roll, pitch and yaw directions, to extend the feet maintaining constant orientation of the ankle w.r.t. the base frame emulating a prismatic joint, and to move the ankle in the roll and pitch directions. In particular, the extension/retraction of the leg is obtained by just actuating the motor on the femur. Notice that the hip pitch and roll, as well the ankle pitch and roll are coupled, by the respective parallel linkages.

To model such complex mechanical structure, we use a constraint-based formulation for mechanical joints where the closed linkage is *opened* at one joint and a constraint is added. Saying that, a single leg of Kangaroo can be divided into 4 parts, each one consists of one or more closed linkages opened at one specific joint (see Table I).

<table><caption>TABLE I: Kangaroo leg's closed linkages</caption><thead><tr><th rowspan="2">Part</th><th rowspan="2">Type</th><th colspan="2">#DOFs</th></tr><tr><th>Passive</th><th>Active</th></tr></thead><tbody><tr><td>Hip Yaw</td><td>Crank slider</td><td>2</td><td>1</td></tr><tr><td>Hip Pitch/Roll</td><td>Differential</td><td>8</td><td>2</td></tr><tr><td>Knee</td><td>Planar</td><td>6</td><td>1</td></tr><tr><td>Ankle Pitch/Roll</td><td>Differential</td><td>16</td><td>2</td></tr></tbody></table>

Each spatial linkage, introduces a constraint in the form:

$$ ^{a}\mathbf{p}_{u}(\boldsymbol{\theta}) = \mathbf{0}, \qquad (1) $$

with $^{a}\mathbf{p}_{u}(\boldsymbol{\theta}) \in \mathbb{R}^m$ the relative pose between the frame $\mathcal{F}_a$ and the frame $\mathcal{F}_u$, as shown for the case of the crank slider in Figure 3, that differentiate became:

$$ ^{a}\mathbf{J}_{a,u}(\boldsymbol{\theta}) \dot{\boldsymbol{\theta}} = \mathbf{0}, \qquad (2) $$

with $^{a}\mathbf{J}_{a,u}(\boldsymbol{\theta}) \in \mathbb{R}^{m \times n}$ the relative Jacobian between the frame $\mathcal{F}_a$ and the frame $\mathcal{F}_u$, expressed in frame $\mathcal{F}_a$, and

$$ \boldsymbol{\theta} = \begin{bmatrix} \boldsymbol{\theta}_u \\ \boldsymbol{\theta}_a \end{bmatrix} \in \mathbb{R}^n, \qquad (3) $$

where $\boldsymbol{\theta}_a \in \mathbb{R}^a$ are the actuated DOFs and $\boldsymbol{\theta}_u \in \mathbb{R}^u$ are the passive DOFs, such that $n = a + u$. In this work we consider mechanical closed linkages with no redundancy, therefore $m = u$.

<figure><img src="image_5.png" /><figcaption>Fig. 3: Crank Slider linkage.</figcaption></figure>

Dividing the active from the passive part of the constraint Jacobian (1) is possible to derive a *mapping Jacobian* to map actuated velocities to underactuated ones:

$$ \dot{\boldsymbol{\theta}}_{u} = \mathbf{J}_{m}(\boldsymbol{\theta}) \dot{\boldsymbol{\theta}}_{a}, \qquad (4) $$

and (virtual) underactuated torques to actuated ones:

$$ \boldsymbol{\tau}_{a} = \mathbf{J}_{m}(\boldsymbol{\theta})^{T} \boldsymbol{\tau}_{u}, \qquad (5) $$

with $\mathbf{J}_{m} \in \mathbb{R}^{u \times a}$.

More complex linkages, such as differential and parallel linkages, can be modeled by just *stacking* the constraint Jacobian computed for each (sub-)closed linkage. For example, considering the spatial differential linkage in Figure 4, we consider the two Jacobians $^{a_1}\mathbf{J}_{a_1,u_1}(\boldsymbol{\theta})$ and $^{a_2}\mathbf{J}_{a_2,u_2}(\boldsymbol{\theta})$ computed considering *half* of the full differential linkage and stacking them such that:

$$ \mathbf{J}(\boldsymbol{\theta}) = \begin{bmatrix} ^{a_1}\mathbf{J}_{a_1,u_1}(\boldsymbol{\theta}) \\ ^{a_2}\mathbf{J}_{a_2,u_2}(\boldsymbol{\theta}) \end{bmatrix}. \qquad (6) $$

Notice that also in this case the number of introduced constraint matches with the number of passive DOFs.

Therefore, the full set of linkages for the Kangaroo robot can be represented by the constraint equation (2) with $n = 76$, $m = u = 64$ and $a = 12$.

### IV. QP-BASED KINEMATIC WHOLE-BODY CONTROL

A floating-base system in contact with the environment presents the same constraint depicted in (2), one per contact but expressed w.r.t. the inertial frame (see Figure 5):

$$ \mathbf{J}_c(\mathbf{q}) \boldsymbol{\nu} = \mathbf{0}, \qquad (7) $$
<figure><img src="image_6.png" /><figcaption>Fig. 4: Differential linkage modeling.</figcaption></figure>

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

being $\mathbf{q}$ the robot generalized coordinates:

$$ \mathbf{q} = \begin{bmatrix} \mathbf{p} \\ \mathbf{\rho} \\ \mathbf{\theta} \end{bmatrix}, \qquad (8) $$

with $\mathbf{p} \in \mathbb{R}^3$ and $\mathbf{\rho} \in \mathbb{R}^4$ a parameterization in $SE(3)$ of the pose of the floating-base, in particular using quaternions for the orientation, and $\mathbf{\theta} \in \mathbb{R}^n$ the actuated and underactuated DOFs in the robot, therefore $\mathbf{q} \in \mathbb{R}^{n+7}$, $\mathbf{\nu} \in \mathbb{R}^{n+6}$ the generalized velocities:

$$ \mathbf{\nu} = \begin{bmatrix} \dot{\mathbf{p}} \\ \dot{\mathbf{\rho}} \\ \dot{\mathbf{\theta}} \end{bmatrix}, \qquad (9) $$

and $\mathbf{J}_c(\mathbf{q}) = [\mathbf{J}_{c,1}^T(\mathbf{q}) \ \mathbf{J}_{c,2}^T(\mathbf{q})]^T \in \mathbb{R}^{12 \times n+6}$, considering 2 planar contacts with the environment.

<figure><img src="image_8.png" /><figcaption>Fig. 5: Floating-Base system in contact with the environment, presenting multiple closed chain linkages.</figcaption></figure>

State-of-the-art techniques for kinematic whole-body control of floating-base systems make use of QP optimization, permitting also to take into account priorities between Tasks, and the inclusion of Constraints, in the form of equalities and inequalities [10]. In the whole-body control framework, the

closed linkage is another type of equality constraint in the form

$$ {}^a\mathbf{J}_{a,u}(\mathbf{q}) \mathbf{\nu} = \lambda^a \mathbf{e}_u(\mathbf{q}), \qquad (10) $$

with ${}^a\mathbf{e}_u(\mathbf{q})$ the error associated to the pose (1) and $\lambda$ a positive gain. This term is used to satisfy exactly the closed linkage constraint once the solution of the QP, a generalized optimal velocity $\mathbf{\nu}^*$, is integrated to obtain the new generalized coordinates $\mathbf{q}$:

$$ \mathbf{q}_{k+1} = \mathbf{q}_k \oplus dt\mathbf{\nu}^* \qquad (11) $$

at each control loop, with $\oplus$ a proper operator that integrates the floating-base angular velocities in $SE(3)$ and $dt$ the control cycle time.

We will now present three optimization schemes to realize Forward Kinematics (FK), Inverse Kinematics (IK), and Cartesian Inverse Kinematics (CIK), considering actuation position limits and velocity limits<fnref n="1" />.

### A. Forward/Inverse Kinematics

We consider the problem of computing the passive DOFs positions $\mathbf{\theta}_u$ given a reference for the motor ones $\mathbf{\theta}_{a,r}$, namely Forward Kinematics problem, and its counterpart, the problem of computing the active DOFs positions $\mathbf{\theta}_a$ given references for a sub-set of the passive ones $\tilde{\mathbf{\theta}}_{u,r} \in \mathbb{R}^a$, namely Inverse Kinematics problem.

Both problems can be solved using the same QP problem structure:

$$ \min_{\mathbf{\nu}} \|\mathbf{x} - \mathbf{x}_r\|^2 + \epsilon \|\mathbf{\nu}\|^2 \qquad (12a) $$

$$ \begin{array}{ll} \text{s.t.} & {}^a\mathbf{J}_{a,u}(\mathbf{q})\mathbf{\nu} = \lambda^a \mathbf{e}_u(\mathbf{q}) \\ & \frac{\mathbf{\theta}_m - \mathbf{\theta}}{dt} \le \dot{\mathbf{\theta}} \le \frac{\mathbf{\theta}_M - \mathbf{\theta}}{dt} \qquad (12b) \\ & \dot{\mathbf{\theta}}_{a,m} \le \dot{\mathbf{\theta}}_a \le \dot{\mathbf{\theta}}_{a,M} \end{array} $$

with different meaning for the $\mathbf{x}$ and $\mathbf{x}_r$ in the case of FK or IK, $\epsilon$ a regularization term, $\mathbf{\theta}_M$ and $\mathbf{\theta}_m$ the maximum and minimum DOFs position limits and $\dot{\mathbf{\theta}}_{a,M}$ and $\dot{\mathbf{\theta}}_{a,m}$ the actuated DOFs velocity limits. Notice that not all the passive

<footnote n="1">¹From now on we will discard the dependency on $\mathbf{q}$ for conciseness.</footnote>
DOFs position limits may be known while actuated DOFs position limits are normally known.

In particular, for the FK:

$$x = \dot{\theta}_a, \quad x_r = \dot{\theta}_{a,r} = K(\theta_{a,r} - \theta_a),$$

while for the IK:

$$x = \dot{\theta}_u, \quad x_r = \dot{\theta}_{u,r} = K(\bar{\theta}_{u,r} - \bar{\theta}_u),$$

with K a positive definite diagonal gain matrix.

For the FK/IK problems, the presence of the floating-base velocities is not really important, in fact the constraint Jacobian (2) does not depend on them.

## B. Cartesian Inverse Kinematics

We consider the problem of Operational Space control in presence of contacts, in particular the position of the Center of Mass (CoM) and the orientation of the base of the robot.

We denote the CoM task as:

$$\mathbf{J}_{\text{CoM}}(\mathbf{q})\boldsymbol{\nu} = \mathbf{v}_{\text{CoM},r}(\mathbf{q}) \quad (15)$$

with $\mathbf{v}_{\text{CoM},r}(\mathbf{q}) = \mathbf{K}_{\text{CoM}}(\mathbf{x}_{\text{CoM},r} - \mathbf{x}_{\text{CoM}}(\mathbf{q}))$, $\mathbf{K}_{\text{CoM}}$ a positive definite diagonal gain matrix, $\mathbf{x}_{\text{CoM}}(\mathbf{q})$ the actual position of the CoM and $\mathbf{x}_{\text{CoM},r}$ the desired one.

Similarly, the orientation task for the floating-base:

$$\mathbf{J}_{fb}(\mathbf{q})\boldsymbol{\nu} = \omega_{fb,r}(\mathbf{q}) \quad (16)$$

with $\omega_{fb,r}(\mathbf{q}) = \mathbf{K}_{fb}e_{fb}(\mathbf{q})$, $\mathbf{K}_{fb}$ a positive definite diagonal gain matrix and $e_{fb}(\mathbf{q})$ a properly computed orientation error for the floating-base.

Finally, for the feet/contacts task, we consider a modified version of (7):

$$\mathbf{J}_c(\mathbf{q})\boldsymbol{\nu} = \mathbf{K}_c e_c(\mathbf{q}), \quad (17)$$

encompassing as well the pose error $e_c(\mathbf{q})$ properly computed, with $\mathbf{K}_c$ a positive definite diagonal gain matrix.

For the CIK we can consider priorities between feet/contact tasks and CoM and base orientation tasks. In particular, to have motions of the base that are contact consistent, the feet/contacts tasks has to be at high priority w.r.t. the CoM and base orientation tasks. The latter should therefore act on the null-space of the contacts.

We first setup a level 0 QP problem where we solve:

$$\begin{array}{ll} \min_{\boldsymbol{\nu}} & \|\mathbf{J}_c(\mathbf{q})\boldsymbol{\nu} - \mathbf{K}_c e_c(\mathbf{q})\| + \epsilon \|\boldsymbol{\nu}\| \\ \text{s.t.} & \text{constraints (12b)} \end{array} \quad (18)$$

computing the optimal solution $\boldsymbol{\nu}_0$.

We consecutively setup a level 1 QP in the form:

$$\begin{array}{ll} \min_{\boldsymbol{\nu}} & \|\mathbf{F}_{\text{CoM}}(\mathbf{q})\|_{\mathbf{w}_1} + \|\mathbf{F}_{fb}(\mathbf{q})\|_{\mathbf{w}_2} + \epsilon \|\boldsymbol{\nu}\| \\ \text{s.t.} & \text{constraints (12b)} \end{array} \quad (19)$$

$$\mathbf{J}_c(\mathbf{q})\boldsymbol{\nu} = \mathbf{J}_c(\mathbf{q})\boldsymbol{\nu}_0$$

where the final optimal solution $\boldsymbol{\nu}^*$ is computed, with $\mathbf{F}_{\text{CoM}}(\mathbf{q}) = \mathbf{J}_{\text{CoM}}(\mathbf{q})\boldsymbol{\nu} - \mathbf{v}_{\text{CoM},r}(\mathbf{q})$, $\mathbf{F}_{fb}(\mathbf{q}) = \mathbf{J}_{fb}(\mathbf{q})\boldsymbol{\nu} - \omega_{fb,r}(\mathbf{q})$, and $\mathbf{w}_{(\cdot)} \in \mathbb{R}^{3 \times 3}$ positive-definite relative weights between the tasks. This second QP permits to track desired

CoM position and floating-base orientation ensuring contact consistency.

It is worth to notice that putting directly the feet/contacts tasks as constraints, in other word using only the level 1 QP with $\mathbf{J}_c(\mathbf{q})\boldsymbol{\nu}_0 = \mathbf{K}_c e_c(\mathbf{q})$, may result in unfeasibility due to incompatibility between the feet/contacts constraint and the other constraints. This technique of cascading QPs to enforce priorities is often called inequality Hierarchical QP (iHQP) [11]. Both the QPs in (18) and (19) can be solved using state-of-the-art QP solvers such as qpOASES [12].

## V. RESULTS

The proposed Whole-Body QP-based schemes, discussed in the previous Section IV, has been implemented using the CartesI/O framework [13]. Simulation results are reported in the video at the following link https://youtu.be/tl7o0jGMaOw.

Preliminary experiments on the real Kangaroo platform estimating and visualizing the whole pose of the kinematics structure of the robot using the mapping Jacobian in (4), for which we wrote a dedicated C++ library called Closed Linkage Library (CLL), see Figure 6, are reported in the second video https://youtu.be/Cc-WILUqIKE.

<figure><img src="image_9.png" /><figcaption>Fig. 6: FK mapping to estimate underactuation state for visualization purpose on the Kangaroo real hardware.</figcaption></figure>

## VI. CONCLUSION AND FUTURE WORKS

This short paper presented the approach to model the closed linkages in the legs of Kangaroo, the new humanoid platform developed by PAL Robotics, based on relative Jacobian. The computed closed linkage constraint is also used to derive a mapping Jacobian between actuated and underactuated quantities. The closed linkage constraint is added inside a QP-based whole-body kinematic controller in order to produce or estimate consistent motions of the legs. Future works will consider to extend the formulation to floating-base dynamics with a particular with a focus on exploiting actuated and passive structure of the closed linkage constraint in order to reduce computation complexity.

## REFERENCES

[1] S. Kumar, H. Wöhrle, J. de Gea Fernández, A. Müller, and F. Kirchner, “A survey on modularity and distributivity in series-parallel hybrid robots,” *Mechatronics*, vol. 68, p. 102367, 2020.
[2] E. Guizzo, “By leaps and bounds: An exclusive look at how boston dynamics is redefining robot agility,” *IEEE Spectrum*, vol. 56, no. 12, pp. 34–39, 2019.

[3] J. Hurst, “Walk this way: To be useful around people, robots need to learn how to move like we do,” *IEEE Spectrum*, vol. 56, no. 3, pp. 30–51, 2019.

[4] O. Stasse, T. Flayols, R. Budhiraja, Giraud-Esclasse et al., “Talos: A new humanoid research platform targeted for industrial applications,” in *IEEE-RAS International Conference on Humanoid Robotics (Humanoids)*, 2017, pp. 689–695.

[5] C. Knabe, R. Griffin, J. Burton, G. Cantor-Cooke, L. Dantanarayana, G. Day, O. Ebeling-Koning, E. Hahn, M. Hopkins, J. Neal et al., “Team valor’s escher: A novel electromechanical biped for the darpa robotics challenge,” in *The DARPA Robotics Challenge Finals: Humanoid Robots To The Rescue*. Springer, 2018, pp. 583–629.

[6] F. Ruscelli, A. Laurenzi, E. Mingo Hoffman, and N. G. Tsagarakis, “A fail-safe semi-centralized impedance controller: Validation on a parallel kinematics ankle,” in *IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, 2018, pp. 1–9.

[7] N. A. Radford, P. Strawser, K. Hambuchen, J. S. Mehling, W. K. Verdeyen, A. S. Donnan, J. Holley, J. Sanchez, V. Nguyen, L. Bridgwater et al., “Valkyrie: NASA’s first bipedal humanoid robot,” *Journal of Field Robotics*, vol. 32, no. 3, pp. 397–419, 2015.

[8] J. Carpentier, R. Budhiraja, and N. Mansard, “Proximal and sparse resolution of constrained dynamic equations,” in *Robotics: Science and Systems 2021*, 2021.

[9] C. Schumacher, E. Knoop, and M. Bächer, “A versatile inverse kinematics formulation for retargeting motions onto robots with kinematic loops,” *Robotics and Automation Letters (RA-L)*, vol. 6, no. 2, pp. 943–950, 2021.

[10] E. M. Hoffman, A. Rocchi, A. Laurenzi, and N. G. Tsagarakis, “Robot control for dummies: Insights and examples using opensot,” in *IEEE-RAS International Conference on Humanoid Robotics (Humanoids)*, 2017, pp. 736–741.

[11] O. Kanoun, F. Lamiraux, and P.-B. Wieber, “Kinematic control of redundant manipulators: Generalizing the task-priority framework to inequality task,” *IEEE Transactions on Robotics*, vol. 27, no. 4, pp. 785–792, 2011.

[12] H. J. Ferreau, C. Kirches, A. Potschka, H. G. Bock, and M. Diehl, “qpoases: A parametric active-set algorithm for quadratic programming,” *Mathematical Programming Computation*, vol. 6, no. 4, pp. 327–363, 2014.

[13] A. Laurenzi, E. M. Hoffman, L. Muratore, and N. G. Tsagarakis, “Cartesi/o: A ros based real-time capable cartesian control framework,” in *IEEE International Conference on Robotics and Automation (ICRA)*, 2019, pp. 591–596.