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 E-Stop button.
To achieve such a routine we integrate different planners with a whole-body 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 whole-body controller via interpolations. The demonstration of this paper shows the effectiveness of our system and techniques regarding to loco-manipulation in real industrial applications.
I-a Related work
Autonomous mobile manipulation has been a research hot-spot 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. Loco-manipulation 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 , by solving inverse kinematics and execution by single joint controllers. Obviously, kinematic coordination cannot account for dynamic decoupling and satisfy dynamic constraints. Optimization-based whole-body controllers are proposed to overcome the drawback of single joint control.  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  has to control the swing legs using joint PD controllers. Therefore, it is not a pure torque-based whole-body controller.  firstly proposed the hierarchical quadratic programming (HQP) approach to solve each task in prioritized QPs with full dynamic constraints sequentially.  also formulates a cascade of QP to involve in inequality constraints for humanoid robot control. Herzog et al.  combine the two benefits of having inequalities in all hierarchical levels  and reducing the number of variables from one QP to the other .  compares the computation efficiency of different HQP formulations. Beyond those instantaneous whole-body controller, optimal control techniques  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 loco-manipulation of low dimension mobile robots  or offline motion planning .
Compared to optimization-based controllers, inverse dynamic controllers can give us more analytical insights of systems. Khatib  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 null-space projections for legged robots . Aghili  proposes the projected inverse dynamic control scheme to decouple constraint and motion in a constrained system. Mistry et al.  applies the projection matrix to underacted systems and implements OSC in constraint-free space. Satisfying inequality constraints, particularly friction cone, is crucial to legged robots because of the interaction with environments. In , we extend that approach to combine analytical Cartesian impedance controller proposed in 
and QP optimization aimed at full-filling inequality constraints. This QP-based semi-analytical controller benefits on computation efficiency and gives us the ability to estimate contact forces leveraging Cartesian impedance control.
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 whole-body motion (moving base with fixed end-effector posture) showcasing the decoupling of operational space foot posture control from base motion. We also demonstrate the controller can handle body-ground 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 ANYmal111https://www.anybotics.com with a RGB camera and a thermal camera for perception. A HRI222http://humanisticrobotics.com remote controller is used to send walking velocities of the base frame attached on the torso, meanwhile a Sigma 7333https://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 on-board PC is running planners  and controllers plus a state estimator within 400 Hz cycle. Another on-board PC is connected to two cameras, sending perception images to the GUI. Each on-board computer has an Intel 4th generation (Haswell ULT) i7-4600U (1.4 GHz-2.1 GHz) processor and two HX316LS9IBK2/16 DDR3L memory cards. The operation commands are wrapped as ROS messages to communicate with on-board systems via wireless network. The diagram of Fig. 2 shows the modules of the system.
The goal of whole-body 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 whole-body controller has to be an optimization-based controller. In our case, we use a semi-analytical QP-based controller described in  with the benefits of efficient computation and contact force estimation  compared to other purely optimization-based controller such as HQP controllers . We use the same controller for locomotion, operation and body-ground contact scenario. Particularly, body-ground contact scenario was never explored before in terms of whole-body control in the literature. Here we show that we can use the same control strategy to handle constraints acting on feet and body.
Iii-a 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
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
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 .
Iii-B Whole-body control
In general, a one-step look-ahead 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 , we could decouple Eq. (1) into constraint-free and constrained spaces where we can implement trajectory tracking and satisfy physical constraints respectively.
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 Constraint-free space, i.e., Eq. (3). As pointed out in , the two sub-spaces are not totally decoupled as exists in Eq. (4). is determined by forward dynamics of Eq. (3),
is an invertible matrix defined as, meanwhile is the torque commands generated by a certain control law. Substituting Eq. (5) into Eq. (4) yields
Then we formulate the following Quadratic Programming problem in Constrained space,
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 4-edge pyramid. The final torque command generated from our controller is the sum of both sub-spaces
where denotes the operational space control command which enforces the system to obey the impedance behavior subject to external disturbances,
where is the deviation of end-effectors 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  for details). is the dynamic consistent null-space projector  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 null-space 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 loco-manipulation 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 null-space 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:
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-a Whole-body motion with fixed end-effector position and orientation
The primary feature of a whole-body controller is to handle multiple tasks. A typical test for whole-body control is to maintain one end-effector (a foot) with fixed posture while moving another end-effector (the base), as shown in Fig. 6. In this simulation, we controlled 5 dimensions of the left-fore foot with respect to the fixed inertia frame by relaxing Yaw of the foot. Fig. 3 shows the positions of the torso and the left-fore foot during this simulation. The torso was tracking a circle in x-y 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 null-space of foot control. It proves that the null-space projector in Eq. (9) works as expected. Readers can check out this demonstration motion in the accompanying video (https://youtu.be/tIyfUjJgJIM).
Iv-B Body-ground contact evaluation
In this subsection, we evaluate the controller can handle body-ground contact cases. In Fig. 7, one prong and two rear feet get contact with ground. The constraint Jacobian where subscripts , and stand for prong, right-hind and left-hind 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 pseudo-inverse of
with zero or near-zero eigenvalues removed, leading to the projector.
Iv-C 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 sub-contractors) designed a mock industrial application scenario for us. The robot equipped with cameras is deployed to press down an E-stop 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 E-stop 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 static-walking gait to climb the slope and also to traverse the slippery terrain. A state estimator  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 left-fore foot to reach the E-stop 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 sub-problems to fit the needs. The advantage of using the whole-body 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.
We have presented an architecture for whole-body control and planning of legged robots. This system exploits a generic formulation of tasks as a QP-based semi-analytical 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 loco-manipulation tasks and constraints including body-ground singular constraints. Particularly, the body-ground contact case is helpful for saving energy, improving payload capability as well as releasing more degrees of freedom for manipulation.
Our evaluation on the whole-body maneuver validates the architecture. The tracking results then show the overall performance of the system. The rotating of base around body-ground contact point validates the generality of the whole-body 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, off-shore 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 E-stop 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 . 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 . 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.
-  M. Mistry, J. Nakanishi, G. Cheng, and S. Schaal, “Inverse kinematics with floating base and constraints for full body humanoid robot control,” in Humanoids 2008-8th IEEE-RAS International Conference on Humanoid Robots. IEEE, 2008, pp. 22–27.
-  L. Righetti, J. Buchli, M. Mistry, M. Kalakrishnan, and S. Schaal, “Optimal distribution of contact forces with inverse-dynamics control,” International Journal of Robotics Research, vol. 32, no. 3, pp. 280–298, 2013.
-  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.
-  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.
-  M. De Lasa and A. Hertzmann, “Prioritized optimization for task-space control,” in 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2009, pp. 5755–5762.
-  M. De Lasa, I. Mordatch, and A. Hertzmann, “Feature-based locomotion controllers,” in ACM Transactions on Graphics (TOG), vol. 29, no. 4. ACM, 2010, p. 131.
-  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.
-  A. Herzog, N. Rotella, S. Mason, F. Grimminger, S. Schaal, and L. Righetti, “Momentum control with hierarchical inverse dynamics on a torque-controlled humanoid,” Autonomous Robots, vol. 40, no. 3, pp. 473–491, 2016.
-  A. Del Prete, F. Nori, G. Metta, and L. Natale, “Prioritized motion–force control of constrained fully-actuated robots:“task space inverse dynamics”,” Robotics and Autonomous Systems, vol. 63, pp. 150–157, 2015.
-  R. Budhiraja, J. Carpentier, C. Mastalli, and N. Mansard, “Differential dynamic programming for multi-phase rigid contact dynamics,” in 2018 IEEE-RAS 18th International Conference on Humanoid Robots (Humanoids). IEEE, 2018, pp. 1–9.
-  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 multi-contact optimal control,” arXiv preprint arXiv:1909.04947, 2019.
-  W. Merkt, V. Ivan, Y. Yang, and S. Vijayakumar, “Towards shared autonomy applications using whole-body control formulations of locomanipulation,” in 2019 IEEE 15th International Conference on Automation Science and Engineering (CASE). IEEE, 2019, pp. 1206–1211.
-  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.
-  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.
-  L. Sentis, J. Park, and O. Khatib, “Compliant control of multicontact and center-of-mass behaviors in humanoid robots,” IEEE Transactions on robotics, vol. 26, no. 3, pp. 483–501, 2010.
Y. Lee, S. Hwang, and J. Park, “Balancing of humanoid robot using contact force/moment control by task-oriented whole body control framework,”Autonomous Robots, vol. 40, no. 3, pp. 457–472, 2016.
-  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.
-  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.
-  M. Mistry and L. Righetti, “Operational space control of constrained and underactuated systems,” Robotics: Science and systems VII, pp. 225–232, 2012.
-  G. Xin, H.-C. Lin, J. Smith, O. Cebe, and M. Mistry, “A model-based hierarchical controller for legged systems subject to external disturbances,” in 2018 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2018, pp. 4375–4382.
-  A. Albu-Schaffer, C. Ott, U. Frese, and G. Hirzinger, “Cartesian impedance control of redundant robots: Recent results with the dlr-light-weight-arms,” in 2003 IEEE International Conference on Robotics and Automation (Cat. No. 03CH37422), vol. 3. IEEE, 2003, pp. 3704–3709.
-  G. Xin, W. Wolfslag, H.-C. Lin, C. Tiseo, and M. Mistry, “An optimization-based locomotion controller for quadruped robots leveraging cartesian impedance control,” Frontiers in Robotics and AI, 2020.
-  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 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids). IEEE, 2016, pp. 1052–1058.
-  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.
-  C. Dario Bellicoso, C. Gehring, J. Hwangbo, P. Fankhauser, and M. Hutter, “Perception-less terrain adaptation through whole body control and hierarchical optimization,” IEEE-RAS International Conference on Humanoid Robots, pp. 558–564, 2016.
-  F. Aghili, “Quadratically constrained quadratic-programming 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.
-  M. Bloesch, M. Hutter, M. A. Hoepflinger, S. Leutenegger, C. Gehring, C. D. Remy, and R. Siegwart, “State estimation for legged robots-consistent fusion of leg kinematics and imu,” Robotics, vol. 17, pp. 17–24, 2013.
-  C. D. Bellicoso, K. Krämer, M. Stäuble, D. Sako, F. Jenelten, M. Bjelonic, and M. Hutter, “Alma-articulated locomotion and manipulation for a torque-controllable robot,” in 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019, pp. 8477–8483.
-  A. W. Winkler, C. D. Bellicoso, M. Hutter, and J. Buchli, “Gait and trajectory optimization for legged systems through phase-based end-effector parameterization,” IEEE Robotics and Automation Letters, vol. 3, no. 3, pp. 1560–1567, 2018.