## I Introduction

Close interaction between humanoid and human gives rise to safety concerns. Hence, whole-body compliance is an essential function for the humanoid robot with a service purpose. Previous researches achieved compliant behaviour by applying admittance control [1], [2], [3], [4]. In our case, we measure the external force via 6-axis force/torque sensors equipped at the wrists of the humanoid robot, and the forces are transformed to position references to fulfil the compliant behaviour. But, when the robot cooperates with people without knowledge of robotics, its constraints can be violated. Thus, the HQP is employed since this method can explicitly consider the inequality constraints. Moreover, HQP has strict prioritised task hierarchies, which ensures the compliance task does not interfere with other tasks [5], [6].

However, the HQP is formulated only according to the states of the immediate one servo-step, and a regular motion inequality constraint update is via a numerical integration approach [7], [8], [9]. For these reasons, Figure 1 shows that the one-step HQP solution can possess significant jerks or even be infeasible when a considerable external force acts on the robot. Therefore, an intuitive solution for this problem is to provide a relatively larger time interval than the sampling time [9].

This paper utilises MPC to predict the distances between the bounds and the control target. The MPC optimises the control action several servo steps in advance to avoid the abrupt hit of bounds. Furthermore, this MPC uses an augmented state-space model of a single-axis inertia system to restrict and optimise the jerk by considering the jerk as an optimisation variable. Thus, this strategy can provide smooth and feasible solutions by prediction and optimisation in advance.

MPC is an optimisation problem that uses future information to choose the best control actions and improve controller performance. Using future information is an essential concept for controlling a humanoid robot. For instance, Kajita et al. used preview control [10], and Wieber used receding horizon control [11]

to track the zero-moment-point (ZMP) reference. Koenemann et al. applied MPC for the whole-body multi-contact and reaching problem of HRP-2, but the MPC computation is offline the robot

[12]. Guo et al. formulated an offline MPC problem for the dynamic bipedal locomotion, and it was running online by approximation and interpolation

[13]. The offline applications are due to the MPC’s substantial computation load, which is challenging for real-time computation. In many real-time MPC applications, e.g., [14], [15], [16] and [17], MPC modules run at a lower rate than the whole-body control rate because of the same reason. For instance, Yuan-Li tried to restrict the jerk with minimum-jerk MPC at a 100 Hz control rate [18]. Different from the above applications of MPC, this work hybrid uses the MPC and the PD controllers, terms as mixed control. The mixed controllers run at the same 500 Hz servo rate as the whole-body control loop. In this work, the tasks with inequality constraints use a group of single-axis model predictive controllers, i.e., ZMP, elbows and knees constraints. PD controllers regulate other tasks to save computation resources.Since the MPC can already consider inequality constraints, and the equality quadratic programming is identical to the NSP based method [19], we use NSP instead of HQP to save the computation resources further. In addition, the NSP method decouples the tasks into different hierarchies and directions, so we can apply single-axis MPCs to control tasks and make them multi-thread for parallel computation. We use the dynamically consistent NSP method for the whole-body control of our humanoid robot since this method is theoretically coherent and clear. Moreover, it provides a strict task hierarchy, and its computation time is promising [20], [21], [22]. Various humanoid robots realised remarkable achievements by utilising this method, e.g., ASIMO [23] [24], Valkyrie [25], DLR-Toro [26] [27] and REEM-C [4].

In summary, the main contribution is the proposed mixed control which combines the MPC and PD controllers for whole-body compliance. The mixed control adequately resolves the deficiencies of HQP. Inevitably, the application of MPC increases the computation load. To achieve 500 Hz real-time control, we use more efficient NSP instead of HQP and distribute the single-axis MPCs into four CPU cores for parallel computation. The rest organisation of the paper is as follows. Section II describes the mathematical formulation of the mixed control, including the single-axis MPC. Section III describes the implementation of the mixed control to the NSP to obtain the joint torques for execution. This section also shows the formulation of inequality constraints in both joint space and task space. In section IV, the simulations and the experiment of the whole-body compliance demonstrate the feasibility of the proposed strategy. Finally, Section V is the conclusion and our future work.

## Ii The Mixed Control

### Ii-a The Control Structure

Figure 2 shows the control structure for the whole-body compliance. The 6-axis force/torque sensors at the wrists measure the external forces . The admittance module transforms the forces to the position and orientation references

. Dynamic mappings of the forces are also established to the movements of the whole-body Centre of Mass (CoM) and the upper-body pose. During this process, significant accelerations generated by the compliant behaviour can break the ZMP. Moreover, we need to restrict the elbows and knees to avoid undesired configurations, such as the upper elbow configuration. Hence, we prescribe the ZMP, elbows and knees as critical tasks in this application and introduce mixed control to satisfy the inequality constraints better. The mixed control is of the form:

where denotes control actions in the operational space, the set is the control actions generated via single-axis MPC controllers for critical tasks with inequality constraints. The diagonal matrices and are closed-loop gains of PD controllers for normal tasks.

### Ii-B The Single-Axis MPC

As previously mentioned, we utilise the single-axis MPC to resolve the deficiencies of the HQP. In this section, we aim to explain the formulation of the single-axis MPC. First, consider the discrete state space of the single-axis inertia model as:

where and are state values, is the time interval, is the current time step, and is the apparent mass. The matrices , and are state transfer matrix, state input matrix and state output matrix, respectively. The augmented model of (2) for predictive control is:

where is the increment of state . With , we can get a standard form of the predictive model through (3) as:

where:

In (5), is the prediction horizon, is the control horizon and . Then, we can formulate the single-axis MPC as a Quadratic Programming (QP) problem:

where denotes the optimisation variable with the tracking error and the increment of control action . The term denotes the desired trajectory of future steps, and denotes the positive-defined Hessian Matrix. The matrix denotes the equality constraint matrix, denotes the inequality constraint matrix, the terms and are the inequality lower and the upper bound, respectively. They can be computed as:

The equality constraint in (6) is used to track the reference trajectory from the current servo step to the future servo step , and it will provide an optimal control law with respect to the cost function by the proper setting of weight matrix . The weight is related to the stiffness of closed-loop system, greater gives less settling time and phase delay, though it is limited by the system bandwidth. The inequality constraints are used to restrict the rate change of control action, the control action and the state output of the system. Also, notice that the terms and are the predicted distances between the bounds and the control object.

Figure 3 shows the comparison between the one-step QP and the single-axis MPC by controlling an ideal one-dimensional plant. In this control system, the input bounds are [-15, 15] m/s, and the output bounds are [-0.1, 0.1] m. The system tracks a sine wave with an amplitude of 0.2 m, which is beyond the output bounds. The red lines show the performance of the QP only considering the immediate servo step. The position of the plant violates its constraints with the restriction of control action. Moreover, differentiating the control action provided by the one-step QP identifies significant jerks that actuators cannot achieve. On the other hand, the MPC controller (see blue lines) can provide feasible solutions that satisfy the output bounds with the same restriction of control action. It also shows that the single-axis MPC provides a smoother control action with small jerks.

## Iii Implementation with the NSP Method

### Iii-a The NSP Method

Since the MPC already considers inequality constraints, we use a more efficient dynamically consistent NSP whole-body controller to achieve a 500 Hz real-time servo rate instead of HQP. Sentis-Khatib proposed the dynamically consistent NSP to transform the operational control actions to the joint torques [22], [20], [23]. Kim-Sentis reported that this method costs 637 s in mean on a dual-core 3.0 GHz CPU [25]. We can evidently save computation resources with the NSP-based method for the single-axis MPC controllers.

Consider the dynamic equations of the robot as:

where is the mass matrix,

is the joint acceleration vector. The term

is the selection matrix due to floating base, is the nonlinear term relating to Coriolis force, centrifugal force and gravitational force, and is the vector of joint torques. For the dynamically consistent NSP, the torque is:where is the Jacobian matrix from the floating base to the foot, is the dynamically consistent null space of the matrix , is the force exerted by the foot on the ground, is the force of the adjacent tasks.

The mapping of the operational space acceleration and the joint space acceleration is expressed as,

where

is the acceleration of the foot in 6 degree-of-freedoms. The foot task is usually the highest priority for the whole-body control of a humanoid robot. The NSP-based method usually considers that the support foot is virtually constrained on the ground to initiate the recursive computation. Therefore, the original research assumed that

, and it cancels the force . However, the dynamic parameters of the real robot and the model can be different, and the computed does not guarantee . Hence, we apply the mixed control to the task and give the operational space dynamics as:where is the apparent mass matrix in the operational space and is the control action provided by the mixed control.

Equation (11) is the essential task for WBC, but it has two unknown terms, the force and the joint torques . Control actions of the task with lower priority do not interfere with the support foot task, requiring that:

Due to the existence of other tasks with lower priority, i.e., , it must satisfies that:

and, the dynamically consistent inverse of that satisfies (13) is of the form:

Then, the operational space dynamics of the adjacent tasks with lower priority can be expressed as:

where is termed as prioritized Jacobian for the priority, and the subscript represents tasks with lower priority are in the null space tasks with higher priority. A detailed derivation of (15) can be seen in [21]. This paper extends (15) with an additional term since we apply the mixed control to the . This additional term modifies the control action of every tasks with lower priority and ensures that the strict hierarchy of NSP will not be broken. Furthermore, the mixed control regulates the task as operational space acceleration via (1).

### Iii-B Implementation for ZMP

With the application of (6), we can consider the inequality constraints with the NSP-based whole-body controller. The bounds of and relate to the jerk/force constraints, and the bounds of relates to the motion constraints. For the task with the highest priority, which is the support foot task, the conventional procedure assumed . Instead, we do not fully adopt the assumption for support foot but use the mixed control to bring the ZMP constraints under consideration. Moreover, the closed-loop control of the foot using mixed control improves the problems caused by the inconsistency of the mass parameters of the model and the actual robot and the improper execution of the joint actuators.

However, the control actions of all directions are co-related since the apparent mass matrix is not diagonal. Therefore, we need to apply certain approximations to fit the single-axis MPC. Thus, we express the ground reaction force (GRF) of the force in the form:

where is the ideal GRF calculated with respect to the assumption . The controller ensures the virtual constraint between the support foot and its contact surface by modifying the ideal GRF with the mixed control. Notice that the X-axis goes from the robot’s right to left, the Z-axis from back to front, and the Y-axis from down to up in our coordinate system. Equation (16) can be substituted into the definition of friction cone and ZMP [28] to calculate the bounds for (7) as:

where is the friction coefficient, and are its corresponding bounds. Then, we conduct minor modifications to (17) to avoid the inverse of since their product is not square. Intuitively, we can use the GRF of the previous servo step by applying:

where the inverse of matrix can be computed as , and are static friction coefficients, and are half lengths of foot.

### Iii-C Implementation for Joint Tasks

We can also use the proposed strategy for inequality constraints in the joint space. The formulation of the Jacobian matrix that directly maps the joint space and task space is:

where is the Jacobian matrix of joint tasks, is the number of joints under consideration, and is the total number of joints. Setting the corresponding element of the considered joint (e.g., the elbows and the knees) as 1 directly maps their corresponding joints into the task space. Then, we can restrict the joint torques by using their apparent inertia matrix similar to (13). Moreover, setting the bounds of in (7) can limit the movement of the joint, but it requires the prescribed set-point . We can obtain it by calculating the forward dynamics as:

Equation (20) gives the desired joint accelerations , and the numerical integration can provide the desired joint position , which can be assigned to .

## Iv Simulations and Experiment

We conducted simulations of whole-body compliance to compare the mixed control with the NSP algorithm and the conventional HQP. We wrote the mixed control and the NSP-based whole-body controller in C++, the RBDL library [29] computes the multi-body dynamics, and qpOASES [30] solves the defined QP problems. The simulations demonstrate the advantages of the mixed control with no computation time restrictions for either the proposed algorithm or the HQP. However, considering the comparison of the two methods on the actual machine could damage our robot, in the robot experiment, we only performed the proposed algorithm to demonstrate its whole-body compliance function and time efficiency for 500 Hz real-time control.

### Iv-a Simulations in Webots

The simulation environment is the open-source robot simulator Webots. The robot model is the UBTech Walker X. In this simulation, the robot’s hands can be dragged in any direction. The CoM of the whole body can also lean in the coronal and sagittal direction to provide a bigger operational space for the hands. The compliant behaviour requires a fast response to the dragging force. Without considering the above inequality constraints, the robot falls over due to the unsatisfaction of ZMP and reaching an undesired posture.

We conducted two simulations for comparison. In the first simulation, we formulated and applied the conventional HQP for whole-body compliance. The second one utilised the proposed mixed control with NSP-base whole-body controller. These were the only difference for the experimental set-ups, and the computation time was out of consideration. In the HQP, the excessive movement of the total CoM can break the ZMP criteria. It led to the divergence of the control system and caused severe damage to the robot, see Figure 1.

During the compliant behaviour, the arm is singular when the elbow joint goes to 0 rad, and an odd configuration can occur. So, the elbow joint position has to be constrained. We prioritised the joint task between the CoM task and the hand task in the two simulations. We deliberately set the joint bounds as [-2.618, -0.436] rad for the ease of demonstration. The HQP method worked when we applied the external force to make the hand move 0.3 m in 2 seconds. However, when we increased the force to make the hand move 0.4 m in 2 seconds, the HQP method refused to satisfy the desired constraints, plotted as the red lines in Figure 4. But, the red line in the upper shows that the joint angle generally satisfied the inequality constraints by applying the mixed control and the NSP-based WBC controller.

Figure 4 also shows the ZMP constraints shaded in grey. The robot’s weight is about 60 Kg, and the length of the foot width is 0.16 m. They provide the bounds [-23.5, +23.5] Nm for the torque

if the robot uniformly distributes its weight to both feet. As the total CoM leaning left, the ZMP of the left foot expands, but the ZMP of the right foot shrinks. The red line shows that, with one-step HQP,

violated the ZMP limit. So when the foot flipped, the task-space torque plotted in the red line diverged at about 1.5 seconds, the abnormal behaviour of the robot stopped the simulation. With the mixed control, the trajectory of was within the area of ZMP, as can be seen in the plot of blue lines. It should be aware that minor violations of constraints (circled in black) can be identified in Figure 4 because the applied single-axis inertia model is still a reduced-order model. The inconsistency of models brings the small error.### Iv-B The Experiment

Figure 5 shows the keyframes of the supplemented video, which recorded the experiment. We used (20) to obtain the position and the velocity control commands to improve the performance. The robot’s wrists are equipped with 6-axis force/torque sensors to measure the external disturbance forces. The admittance controller transforms measured forces to the motion in the task space, and we compensated the mass of hands beforehand. If the robot’s hand moves too far to comply with the force, a fraction of the force drives the movements of the CoM and upper-body pose to expand operational space for hands.

Module | Thread | Average [s] | Maximum [s] |

Mixed Control | Main | 535 | 867 |

Child | 239 | 673 | |

NSP | Main | 440 | 790 |

Total | 962 | 1623 |

Table I shows the computation time of each module. As previously mentioned, we decoupled and approximated the tasks and applied the mixed control via parallel computation. In this case, the mixed control used eight single-axis MPC controllers, and the rest were PD controllers. We distributed them uniformly into the four physical cores of the Intel Core i7-8559U. Hence, this control program contains one main thread and three child threads. Moreover, we coded the NSP-based WBC into the main thread and a child thread to have similar computation times for both threads.

The restriction of real-time control hardware is that the total computation time of the control program cannot exceed 1750 s to achieve a 500 Hz control rate. By setting the qpQASES solver with the options setTOMPC and hotstart, the average computation time of the mixed control is 239 s. However, if the robot’s configuration changes too significant, the hotstart option no longer works. As a result, the maximum computation time can rise to 673 s. In addition, data communication and scheduling between multiple threads cost extra time. Finally, the experiment shows that our maximum computation time is 1623 s, meeting the requirement. Notice that the minimum and maximum computation times of the mixed control and NSP-based whole-body controller do not coincide. Accordingly, the total computation time is not a direct summation.

## V Conclusions

This paper proposed a new strategy, termed Mixed Control, for the whole-body compliance of the humanoid robot Walker X. The MPC predicts the distances between the control object and its bounds and optimises the solution several steps in advance to avoid infeasible solutions. Moreover, the jerk as the optimisation variable avoids its excessive and unrealisable magnitude, providing smooth solutions. Due to the strict hierarchical decomposition of tasks via the NSP, the application of the single-axis MPC and PD controllers in the mixed control is flexible, and the MPC controllers can be applied independently to any user-defined critical task. On the other hand, conventional PD controllers can still control other tasks. The mixed control with the NSP method runs at a 500 Hz servo rate with Intel Core i7-8559U (4-core, 1.9 GHz), which fulfils the whole-body compliance function for the humanoid robot Walker X.

Due to the limitation of computation resources, we can only apply eight MPC controllers with multi-threads for parallel computation, and the maximum computation time is about 1623 s. It is enough for the whole-body compliance function, but the dynamic walking algorithm requires more MPC controllers. So, our future work is to improve the computational efficiency of the proposed method.

## References

- [1] C. Ott and Y. Nakamura, “Admittance control using a base force/torque sensor.” IFAC Proceedings Volumes, vol. 42, no. 16, pp. 467–472, 2009.
- [2] V. Padois, S. Ivaldi, J. Babiˇc, M. Mistry, J. Peters, and F. Nori, “Whole-body multi-contact motion in humans and humanoids: Advances of the codyco european project,” Robotics and Autonomous Systems, vol. 90, pp. 97–117, 2017.
- [3] A. Q. Keemink, H. van der Kooij, and A. H. Stienen, “Admittance control for physical human–robot interaction,” The International Journal of Robotics Research, vol. 37, no. 11, pp. 1421–1444, 2018.
- [4] E. Dean-Leon, J. R. Guadarrama-Olvera, F. Bergner, and G. Cheng, “Whole-body active compliance control for humanoid robots with robot skin,” in 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019, pp. 5404–5410.
- [5] A. Escande, N. Mansard, and P.-B. Wieber, “Hierarchical quadratic programming: Fast online humanoid-robot motion generation,” The International Journal of Robotics Research, vol. 33, no. 7, pp. 10061028, 2014.
- [6] H. Dai, A. Valenzuela, and R. Tedrake, “Whole-body motion planning with centroidal dynamics and full kinematics,” in 2014 IEEE-RAS International Conference on Humanoid Robots. IEEE, 2014, pp. 295–302.
- [7] L. Saab, N. Mansard, F. Keith, J.-Y. Fourquet, and P. Sou‘eres, “Generation of dynamic motion for anthropomorphic systems under prioritized equality and inequality constraints,” in 2011 IEEE International Conference on Robotics and Automation. IEEE, 2011, pp. 1091–1096.
- [8] R. Rossi, A. Santamaria-Navarro, J. Andrade-Cetto, and P. Rocco, “Trajectory generation for unmanned aerial manipulators through quadratic programming,” IEEE Robotics and Automation Letters, vol. 2, no. 2, pp. 389–396, 2016.
- [9] C. D. Bellicoso, C. Gehring, J. Hwangbo, P. Fankhauser, and M. Hutter, “Perception-less terrain adaptation through whole body control and hierarchical optimization,” in 2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids). IEEE, 2016, pp. 558–564.
- [10] S. Kajita, F. Kanehiro, K. Kaneko, K. Fujiwara, K. Harada, K. Yokoi, and H. Hirukawa, “Biped walking pattern generation by using preview control of zero-moment point,” in 2003 IEEE International Conference on Robotics and Automation (Cat. No. 03CH37422), vol. 2. IEEE, 2003, pp. 1620–1626.
- [11] P.-B. Wieber, “Trajectory free linear model predictive control for stable walking in the presence of strong perturbations,” in 2006 6th IEEERAS International Conference on Humanoid Robots. IEEE, 2006, pp. 137–142.
- [12] J. Koenemann, A. Del Prete, Y. Tassa, E. Todorov, O. Stasse, M. Bennewitz, and N. Mansard, “Whole-body model-predictive control applied to the HRP-2 humanoid,” in 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2015, pp. 3346–3351.
- [13] Y. Guo, M. Zhang, H. Dong, and M. Zhao, “Fast online planning for bipedal locomotion via centroidal model predictive gait synthesis,” IEEE Robotics and Automation Letters, vol. 6, no. 4, pp. 6450–6457, 2021.
- [14] T. Erez, K. Lowrey, Y. Tassa, V. Kumar, S. Kolev, and E. Todorov, “An integrated system for real-time model predictive control of humanoid robots,” in 2013 13th IEEE-RAS International conference on humanoid robots (Humanoids). IEEE, 2013, pp. 292–299.
- [15] F. Farshidian, E. Jelavic, A. Satapathy, M. Giftthaler, and J. Buchli, “Real-time motion planning of legged robots: A model predictive control approach,” in 2017 IEEE-RAS 17th International Conference on Humanoid Robotics (Humanoids). IEEE, 2017, pp. 577–584.
- [16] M. Neunert, M. St¨auble, M. Giftthaler, C. D. Bellicoso, J. Carius, C. Gehring, M. Hutter, and J. Buchli, “Whole-body nonlinear model predictive control through contacts for quadrupeds,” IEEE Robotics and Automation Letters, vol. 3, no. 3, pp. 1458–1465, 2018.
- [17] Y. Ding, A. Pandala, and H.-W. Park, “Real-time model predictive control for versatile dynamic motions in quadrupedal robots,” in 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019, pp. 8484–8490.
- [18] K. Yuan, C. McGreavy, C. Yang, W. Wolfslag, and Z. Li, “Decoding motor skills of artiﬁcial intelligence and human policies: A study on humanoid and human balance control,” IEEE Robotics & Automation Magazine, vol. 27, no. 2, pp. 87–101, 2020.
- [19] H. Hanafusa, T. Yoshikawa, and Y. Nakamura, “Analysis and control of articulated robot arms with redundancy,” IFAC Proceedings Volumes, vol. 14, no. 2, pp. 1927–1932, 1981.
- [20] L. Sentis and O. Khatib, “Task-oriented control of humanoid robots through prioritization,” in IEEE International Conference on Humanoid Robots. IEEE, 2004, pp. 1–16.
- [21] L. Sentis, Synthesis and control of whole-body behaviors in humanoid systems. Ph.D. Dissertation, Department of Electrical Engineering, Stanford University, 2007.
- [22] L. Sentis and O. Khatib, “A whole-body control framework for humanoids operating in human environments,” in Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006. IEEE, 2006, pp. 2641–2648.
- [23] O. Khatib, P. Thaulad, T. Yoshikawa, and J. Park, “Torque-position transformer for task control of position controlled robots,” in 2008 IEEE international conference on robotics and automation. IEEE, 2008, pp. 1729–1734.
- [24] T. Yoshikawa and O. Khatib, “Compliant humanoid robot control by the torque transformer,” in 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2009, pp. 3011–3018.
- [25] D. Kim, J. Lee, J. Ahn, O. Campbell, H. Hwang, and L. Sentis, “Computationally-robust and efﬁcient prioritized whole-body controller with contact constraints,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 1–8.
- [26] D. Lee, C. Ott, and Y. Nakamura, “Mimetic communication model with compliant physical contact in human—humanoid interaction,” The International Journal of Robotics Research, vol. 29, no. 13, pp. 1684–1704, 2010.
- [27] C. Ott, A. Dietrich, and A. Albu-Sch¨affer, “Prioritized multi-task compliance control of redundant manipulators,” Automatica, vol. 53, pp. 416–423, 2015.
- [28] Y. Fujimoto and A. Kawamura, “Simulation of an autonomous biped walking robot including environmental force interaction,” IEEE Robotics & Automation Magazine, vol. 5, no. 2, pp. 33–42, 1998.
- [29] M. L. Felis, “Rbdl: an efﬁcient rigid-body dynamics library using recursive algorithms,” Autonomous Robots, vol. 41, no. 2, pp. 495511, 2017.
- [30] 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.

Comments

There are no comments yet.