I Introduction
The increase of the human life value over the last two centuries has been unprecedented, considering that few decades ago what is now considered child labor was a reality in all the G7 countries. The increase in population wealth is not only related with the perceived value of human life but it is also related to the increase life expectancy and the minimization of infant mortality. Therefore, the risk associated with jobs such as working on offshore platforms and nuclear sites is becoming unaffordable in the modern society.
Projects as ORCA (Offshore Robotics for Certification of Assets) aim to develop, test and validate robotic technology that in the foreseeable future can be deployed along side the human operators to minimise the work related perils. Legged robots can help in such sites because they offer more maneuverability than wheeled robots in real world scenarios, making them easier to deploy in traditional human operated industrial facilities. However, this comes at the cost of limited payloads and lower reliability due to possible and more difficult locomotion stability, which lead ORCA to research also drones and wheeled platforms.
An integral part of our research within the ORCA project is testing our methods in industrial environments, where we conduct regular demonstrations attended by leading companies in the offshore industry. Feedback from previous interactions with these companies helped us develop a comprehensive obstacle course to showcase our controllers for a comprehensive routine inspection with some intervention capabilities as shown in Fig. 1. The course requires the removal of an obstacle to access to the scaffolding, which is composed of a slope up, a cluttered turn, a slippery bridge leading to a control box where the remote operator uses one of the legs to push an EStop button.
To achieve such a routine we integrate different planners with a wholebody controller developed for floating base systems. Planners generate the foot step sequence and trajectories of feet and base for locomotion and manipulation, feeding the instantaneous wholebody controller via interpolations. The demonstration of this paper shows the effectiveness of our system and techniques regarding to locomanipulation in real industrial applications.
Ia Related work
Autonomous mobile manipulation has been a research hotspot since DARPA Robotics Challenge (DRC), with subsequent international international competitions such as DARPA Subterranean Challenge and Mohamed Bin Zayed International Robotics Challenge (MBZIRC). Legged robots are a particularly challenging agent to accomplish these competitive tasks due to the control complexity of a high dimensional robot and the inherent risk of falling. Locomanipulation control allows the robot to coordinate multiple tasks simultaneously and also to tackle any interference of tasks due to potential lack of degrees of freedom.
Early work to coordinate multiple tasks are introduced in the kinematic level [1], by solving inverse kinematics and execution by single joint controllers. Obviously, kinematic coordination cannot account for dynamic decoupling and satisfy dynamic constraints. Optimizationbased wholebody controllers are proposed to overcome the drawback of single joint control. [2] formulates internal and external constraints into a quadratic programming (QP) problem with optimized contact forces. A widely used contact force optimization method called the Virtual Model Controller [3][4] has to control the swing legs using joint PD controllers. Therefore, it is not a pure torquebased wholebody controller. [5][6] firstly proposed the hierarchical quadratic programming (HQP) approach to solve each task in prioritized QPs with full dynamic constraints sequentially. [7] also formulates a cascade of QP to involve in inequality constraints for humanoid robot control. Herzog et al. [8] combine the two benefits of having inequalities in all hierarchical levels [7] and reducing the number of variables from one QP to the other [6]. [9] compares the computation efficiency of different HQP formulations. Beyond those instantaneous wholebody controller, optimal control techniques [10][11] are proposed to generate optimal control command along with optimal trajectories. But optimal control is too slow for real time control when considering inequality constraints. A more general framework employing nonlinear programming (NLP) is proposed to cope with planning and control for locomanipulation of low dimension mobile robots [12] or offline motion planning [13].
Compared to optimizationbased controllers, inverse dynamic controllers can give us more analytical insights of systems. Khatib [14] derives the operational space control (OSC), establishing the straightforward relationship between operational tasks and inverse dynamics. OSC has been extended to hierarchical OSC by adding iterative nullspace projections for legged robots [15][16][17]. Aghili [18] proposes the projected inverse dynamic control scheme to decouple constraint and motion in a constrained system. Mistry et al. [19] applies the projection matrix to underacted systems and implements OSC in constraintfree space. Satisfying inequality constraints, particularly friction cone, is crucial to legged robots because of the interaction with environments. In [20], we extend that approach to combine analytical Cartesian impedance controller proposed in [21]
and QP optimization aimed at fullfilling inequality constraints. This QPbased semianalytical controller benefits on computation efficiency and gives us the ability to estimate contact forces leveraging Cartesian impedance control
[22].IB Contributions
In this work, we apply our control framework to implement a sequence of locomotion and manipulation tasks in industrial environments. We evaluate our proposed system by demonstrating a wholebody motion (moving base with fixed endeffector posture) showcasing the decoupling of operational space foot posture control from base motion. We also demonstrate the controller can handle bodyground contact constraints without any adaptation. We highlight the great potential of a quadruped robot running our controller in real world applications. Figure 1 shows the testing site, and accompanying video is available at: https://youtu.be/tIyfUjJgJIM.
Ii System overview
Our system consists of a quadruped robot ANYmal^{1}^{1}1https://www.anybotics.com with a RGB camera and a thermal camera for perception. A HRI^{2}^{2}2http://humanisticrobotics.com remote controller is used to send walking velocities of the base frame attached on the torso, meanwhile a Sigma 7^{3}^{3}3https://www.forcedimension.com haptic joystick is for teleoperating a foot to do manipulation. The GUI integrates camera windows and interfaces for controller switching and parameter handling. One onboard PC is running planners [23] and controllers plus a state estimator within 400 Hz cycle. Another onboard PC is connected to two cameras, sending perception images to the GUI. Each onboard computer has an Intel 4th generation (Haswell ULT) i74600U (1.4 GHz2.1 GHz) processor and two HX316LS9IBK2/16 DDR3L memory cards. The operation commands are wrapped as ROS messages to communicate with onboard systems via wireless network. The diagram of Fig. 2 shows the modules of the system.
Iii Methods
The goal of wholebody control is to generate torque commands for all the actuators corresponding to desired tasks and subject to physical constraints. Since there are inequality constraints such as friction cones, the wholebody controller has to be an optimizationbased controller. In our case, we use a semianalytical QPbased controller described in [20] with the benefits of efficient computation and contact force estimation [24] compared to other purely optimizationbased controller such as HQP controllers [25][8]. We use the same controller for locomotion, operation and bodyground contact scenario. Particularly, bodyground contact scenario was never explored before in terms of wholebody control in the literature. Here we show that we can use the same control strategy to handle constraints acting on feet and body.
Iiia Model formulation
The configuration of a floating base system with limbs is fully determined by a generalized coordinate vector
, where denotes the floating base’s position and orientation with respect to a fixed inertia frame, meanwhile denotes the vector of actuated joint positions. Also, we define the generalized velocity vector as , where and are the linear and angular velocities of the Base with respect to the inertia frame expressed respectively in the and frame. The equations of motion of a floating base system subject to environment contacts are written as(1) 
where is the inertia matrix, is the generalized vector containing Coriolis, centrifugal and gravitational effects, is the vector of torques, is the constraint Jacobian that describes constraints, denotes the number of contact points accounting foot contact and body contact, are constraint forces acting on contact points, and
(2) 
is the selection matrix with
dimensional identity matrix
. In the following, we are going to use denotation , and for convenience by omitting dependent variables and .IiiB Wholebody control
In general, a onestep lookahead problem can be formulated as an optimization problem subject to all physical bounds and environment constraints. In that way, we will lose property analysis of the system since optimization solvers directly give the admissible solutions. By using projection matrix [26][19], we could decouple Eq. (1) into constraintfree and constrained spaces where we can implement trajectory tracking and satisfy physical constraints respectively.
(3) 
(4) 
Note that Eq. (3) plus Eq. (4) is equal to the whole system dynamics, i.e., Eq. (1). However, constraint forces are removed by projector in Constraintfree space, i.e., Eq. (3). As pointed out in [18], the two subspaces are not totally decoupled as exists in Eq. (4). is determined by forward dynamics of Eq. (3),
(5) 
where
is an invertible matrix defined as
, meanwhile is the torque commands generated by a certain control law. Substituting Eq. (5) into Eq. (4) yields(6) 
Then we formulate the following Quadratic Programming problem in Constrained space,
(7)  
subject to  
where , and denotes the contact force of th contact point, is the friction coefficient. The last three inequality constraints in Eq. (7) represent the linearized friction cone constraints with 4edge pyramid. The final torque command generated from our controller is the sum of both subspaces
(8) 
To generate , people can resort to normal robotic arm controllers with the consideration of underactuation factor in Eq. (3). We derived the Cartesian impedance control law in [20] as follows,
(9) 
along with
(10) 
where denotes the operational space control command which enforces the system to obey the impedance behavior subject to external disturbances,
(11) 
where is the deviation of endeffectors in Cartesian space. Subscripts and denote swing feet and the base respectively. and represent nonlinear term of operational space dynamics and operational space inertia matrix (see [20] for details). is the dynamic consistent nullspace projector [19] of the swing foot, which enforces strictly hierarchical priorities. In the case of foot posture control, will deal with the overlap between swing foot Jacobian and base Jacobian , leading to the convenience of leaving base Jacobian to be always a matrix. As the torso is in the nullspace of the swing foot, the torso is enforced to satisfy the swing foot’s motion requirement, which results in automatic motion coordination and reachability extension of foot. For locomotion control, we only control three dimensions of a foot, leading to . On the other hand, for manipulation tasks, we control six dimensions of a foot (in fact, the shank) with .
One of the benefits of our locomanipulation controller is that we can use Eq. (11) to estimate the external forces, such as contact forces acting on the foot during doing manipulation tasks. Note that the torso motion error does not affect the foot position error because the torso is controlled in the nullspace of the manipulation foot. We can always only measure the motion of the foot and then use the following equation to estimate external force without any torso motion error interfering:
(12) 
Here in our experiments, we employ this estimation as haptic feedback for teleoperation, and thus do not require a force/torque sensor at the point of contact.
Iv Evaluation
Iva Wholebody motion with fixed endeffector position and orientation
The primary feature of a wholebody controller is to handle multiple tasks. A typical test for wholebody control is to maintain one endeffector (a foot) with fixed posture while moving another endeffector (the base), as shown in Fig. 6. In this simulation, we controlled 5 dimensions of the leftfore foot with respect to the fixed inertia frame by relaxing Yaw of the foot. Fig. 3 shows the positions of the torso and the leftfore foot during this simulation. The torso was tracking a circle in xy plane with desired constant height. Figs. 4 and 5 depict the foot’s position error and orientation error during the simulation respectively. It is obvious that the error of Yaw is much greater than the other directions whereas the tracking performance of torso is not good as the foot. That is reasonable since the torso control is in the nullspace of foot control. It proves that the nullspace projector in Eq. (9) works as expected. Readers can check out this demonstration motion in the accompanying video (https://youtu.be/tIyfUjJgJIM).
IvB Bodyground contact evaluation
In this subsection, we evaluate the controller can handle bodyground contact cases. In Fig. 7, one prong and two rear feet get contact with ground. The constraint Jacobian where subscripts , and stand for prong, righthind and lefthind respectively. is a constant matrix since it is not configuration dependant. In this case, the torso can rotate in three dimensions around the prong actuated by two rear legs. The proposed controller does not need to adapt to this special case because the projection matrix exists as normal in this case. We defined a sequence of rotation actions to move Pitch, Roll and Yaw and implemented in simulation as shown in Fig. 7. It is also included in the accompanying video. During the orientation movement, we control the two fore feet to keep current positions, which is achieved by the hierarchical feature of our controller as discussed in former subsection. Furthermore, when there are two prongs contacting with ground, is not full row rank because two prongs are linear dependant.
in this case. We use singular value decomposition (SVD) to compute pseudoinverse of
with zero or nearzero eigenvalues removed, leading to the projector
.IvC Testing in industrial environments
The ORCA project gave us an industrial environment to test our work as described in the Introduction. Our industrial partners (such as Total, BP and their subcontractors) designed a mock industrial application scenario for us. The robot equipped with cameras is deployed to press down an Estop button on a platform. The robot and operator were sharing autonomy via command and feedback information communication. The operator used an HRI remote controller to steer the robot for locomotion. As shown in Fig. 8, the robot had to climb up a slope and walk over a slippery bridge covered by artificial ice before approaching the electrical box where Estop button located. However, we intentionally placed a large box in front of the slope. The robot trotted to a side of the box and lay on the two prongs and two rear legs in order to use two front legs to push the box away. Then the robot switched to a staticwalking gait to climb the slope and also to traverse the slippery terrain. A state estimator [27] updates the inclined terrain information for the control loop. After climbing up on the platform, the operator switches to a trotting gait to speed up turning in place. While traversing the bridge, the operator had to change the friction coefficient parameter in the controller manually before stepping on the artificial ice since there was no algorithms to estimate the friction coefficient online. The friction coefficient between robot’s feet and the artificial ice is about 0.2. That is close to some terrains covered by water and oil in industrial environments. Eventually, the operator teleoperated the leftfore foot to reach the Estop button and pressed it down with the assistant of camera vision. During lifting the foot, the leg overcame a singularity configuration without instability, which proves the robustness of our controller. The total process was recorded in the accompanying video.
The success of this application experiment gives us a baseline of our system’s capability. The task can be easily modified for similar scenarios by changing any of the subproblems to fit the needs. The advantage of using the wholebody controller in this scenario is that the framework can handle all the locomotion and operation modes, which allows us to execute different tasks by switching motion trajectories and foot contact sequences.
V Discussion
We have presented an architecture for wholebody control and planning of legged robots. This system exploits a generic formulation of tasks as a QPbased semianalytical operational space controller, and it integrates inputs from normal joystick, haptic joystick, perception, and motion planner. The Cartesian impedance controller then minimizes the tracking error and also maintains compliance against disturbances while satisfying physical constraints. The formulation of the problem allows us to adapt to a variety of locomanipulation tasks and constraints including bodyground singular constraints. Particularly, the bodyground contact case is helpful for saving energy, improving payload capability as well as releasing more degrees of freedom for manipulation.
Our evaluation on the wholebody maneuver validates the architecture. The tracking results then show the overall performance of the system. The rotating of base around bodyground contact point validates the generality of the wholebody controller. The last experiment host in the industrial environment strengthen the practicality of employing quadruped robots for real world needs. Equipped with various sensors, the robot can be useful for inspection and intervention and reduce workers’ labour in extreme environments. Such an approach can rapidly accelerate the development and deployment of robotic systems in autonomous nuclear equipment maintenance, offshore asset maintenance, and many other fields.
However, the system also has some aspects that can be improved in the future. The foot cannot reach very high. The Estop button in our experiment is high. Therefore, the application of teleoperating a foot is limited to low height operation, such as improving perception accuracy via haptic foot exploration. For more complex manipulation tasks, we need an extra arm mounted on the torso to increase degrees of freedom and reachability [28]. Second, the motion planner is a one step looking forward planner. A long time horizon considering planner could improve efficiency and stability of the robot because it will make full use of the robot’s physical capability [29]. The controller proposed in this paper is available for some dynamic gaits such as walking trotting and flying trotting. However dynamic gaits will rely heavily on suitable foot step planning to keep balance.
References
 [1] M. Mistry, J. Nakanishi, G. Cheng, and S. Schaal, “Inverse kinematics with floating base and constraints for full body humanoid robot control,” in Humanoids 20088th IEEERAS International Conference on Humanoid Robots. IEEE, 2008, pp. 22–27.
 [2] L. Righetti, J. Buchli, M. Mistry, M. Kalakrishnan, and S. Schaal, “Optimal distribution of contact forces with inversedynamics control,” International Journal of Robotics Research, vol. 32, no. 3, pp. 280–298, 2013.
 [3] C. Gehring, S. Coros, M. Hutter, M. Bloesch, M. A. Hoepflinger, and R. Siegwart, “Control of dynamic gaits for a quadrupedal robot,” in 2013 IEEE international conference on Robotics and automation. IEEE, 2013, pp. 3287–3292.
 [4] T. Boaventura, C. Semini, J. Buchli, M. Frigerio, M. Focchi, and D. G. Caldwell, “Dynamic torque control of a hydraulic quadruped robot,” in 2012 IEEE international conference on robotics and automation. IEEE, 2012, pp. 1889–1894.
 [5] M. De Lasa and A. Hertzmann, “Prioritized optimization for taskspace control,” in 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2009, pp. 5755–5762.
 [6] M. De Lasa, I. Mordatch, and A. Hertzmann, “Featurebased locomotion controllers,” in ACM Transactions on Graphics (TOG), vol. 29, no. 4. ACM, 2010, p. 131.
 [7] O. Kanoun, F. Lamiraux, and P.B. Wieber, “Kinematic control of redundant manipulators: Generalizing the taskpriority framework to inequality task,” IEEE Transactions on Robotics, vol. 27, no. 4, pp. 785–792, 2011.
 [8] A. Herzog, N. Rotella, S. Mason, F. Grimminger, S. Schaal, and L. Righetti, “Momentum control with hierarchical inverse dynamics on a torquecontrolled humanoid,” Autonomous Robots, vol. 40, no. 3, pp. 473–491, 2016.
 [9] A. Del Prete, F. Nori, G. Metta, and L. Natale, “Prioritized motion–force control of constrained fullyactuated robots:“task space inverse dynamics”,” Robotics and Autonomous Systems, vol. 63, pp. 150–157, 2015.
 [10] R. Budhiraja, J. Carpentier, C. Mastalli, and N. Mansard, “Differential dynamic programming for multiphase rigid contact dynamics,” in 2018 IEEERAS 18th International Conference on Humanoid Robots (Humanoids). IEEE, 2018, pp. 1–9.
 [11] C. Mastalli, R. Budhiraja, W. Merkt, G. Saurel, B. Hammoud, M. Naveau, J. Carpentier, S. Vijayakumar, and N. Mansard, “Crocoddyl: An efficient and versatile framework for multicontact optimal control,” arXiv preprint arXiv:1909.04947, 2019.
 [12] W. Merkt, V. Ivan, Y. Yang, and S. Vijayakumar, “Towards shared autonomy applications using wholebody control formulations of locomanipulation,” in 2019 IEEE 15th International Conference on Automation Science and Engineering (CASE). IEEE, 2019, pp. 1206–1211.
 [13] H. Ferrolho, W. Merkt, V. Ivan, W. Wolfslag, and S. Vijayakumar, “Optimizing dynamic trajectories for robustness to disturbances using polytopic projections,” arXiv preprint arXiv:2003.00609, 2020.
 [14] O. Khatib, “A unified approach for motion and force control of robot manipulators: The operational space formulation,” IEEE Journal on Robotics and Automation, vol. 3, no. 1, pp. 43–53, 1987.
 [15] L. Sentis, J. Park, and O. Khatib, “Compliant control of multicontact and centerofmass behaviors in humanoid robots,” IEEE Transactions on robotics, vol. 26, no. 3, pp. 483–501, 2010.

[16]
Y. Lee, S. Hwang, and J. Park, “Balancing of humanoid robot using contact force/moment control by taskoriented whole body control framework,”
Autonomous Robots, vol. 40, no. 3, pp. 457–472, 2016.  [17] L. Sentis and O. Khatib, “A wholebody 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.
 [18] F. Aghili, “A unified approach for inverse and direct dynamics of constrained multibody systems based on linear projection operator: applications to control and simulation,” IEEE Transactions on Robotics, vol. 21, no. 5, pp. 834–849, 2005.
 [19] M. Mistry and L. Righetti, “Operational space control of constrained and underactuated systems,” Robotics: Science and systems VII, pp. 225–232, 2012.
 [20] G. Xin, H.C. Lin, J. Smith, O. Cebe, and M. Mistry, “A modelbased hierarchical controller for legged systems subject to external disturbances,” in 2018 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2018, pp. 4375–4382.
 [21] A. AlbuSchaffer, C. Ott, U. Frese, and G. Hirzinger, “Cartesian impedance control of redundant robots: Recent results with the dlrlightweightarms,” in 2003 IEEE International Conference on Robotics and Automation (Cat. No. 03CH37422), vol. 3. IEEE, 2003, pp. 3704–3709.
 [22] G. Xin, W. Wolfslag, H.C. Lin, C. Tiseo, and M. Mistry, “An optimizationbased locomotion controller for quadruped robots leveraging cartesian impedance control,” Frontiers in Robotics and AI, 2020.
 [23] P. Fankhauser, C. D. Bellicoso, C. Gehring, R. Dubé, A. Gawel, and M. Hutter, “Free gait—an architecture for the versatile control of legged robots,” in 2016 IEEERAS 16th International Conference on Humanoid Robots (Humanoids). IEEE, 2016, pp. 1052–1058.
 [24] G. Xin, J. Smith, D. Rytz, W. Wolfslag, H.C. Lin, and M. Mistry, “Bounded haptic teleoperation of a quadruped robot’s foot posture for sensing and manipulation,” in 2020 IEEE International Conference on Robotics and Automation (ICRA), 2020.
 [25] C. Dario Bellicoso, C. Gehring, J. Hwangbo, P. Fankhauser, and M. Hutter, “Perceptionless terrain adaptation through whole body control and hierarchical optimization,” IEEERAS International Conference on Humanoid Robots, pp. 558–564, 2016.
 [26] F. Aghili, “Quadratically constrained quadraticprogramming based control of legged robots subject to nonlinear friction cone and switching constraints,” IEEE/ASME Transactions on Mechatronics, vol. 22, no. 6, pp. 2469–2479, 2017.
 [27] M. Bloesch, M. Hutter, M. A. Hoepflinger, S. Leutenegger, C. Gehring, C. D. Remy, and R. Siegwart, “State estimation for legged robotsconsistent fusion of leg kinematics and imu,” Robotics, vol. 17, pp. 17–24, 2013.
 [28] C. D. Bellicoso, K. Krämer, M. Stäuble, D. Sako, F. Jenelten, M. Bjelonic, and M. Hutter, “Almaarticulated locomotion and manipulation for a torquecontrollable robot,” in 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019, pp. 8477–8483.
 [29] A. W. Winkler, C. D. Bellicoso, M. Hutter, and J. Buchli, “Gait and trajectory optimization for legged systems through phasebased endeffector parameterization,” IEEE Robotics and Automation Letters, vol. 3, no. 3, pp. 1560–1567, 2018.