Adaptive robot climbing with magnetic feet in unknown slippery structure

Firm foot contact is the top priority of climbing robots to avoid catastrophic events, especially when working at height. This study proposes a robust planning and control framework for climbing robots that provides robustness to slippage in unknown environments. The framework includes 1) a center of mass (CoM) trajectory optimization under the estimated contact condition, 2) Kalman filter–like approach for uncertain environment parameter estimation and subsequent CoM trajectory re-planing, and 3) an online weight adaptation approach for whole-body control (WBC) framework that can adjust the ground reaction force (GRF) distribution in real time. Though the friction and adhesion characteristics are often assumed to be known, the presence of several factors that lead to a reduction in adhesion may cause critical problems for climbing robots. To address this issue safely and effectively, this study suggests estimating unknown contact parameters in real time and using the evaluated contact information to optimize climbing motion. Since slippage is a crucial behavior and requires instant recovery, the computation time for motion re-planning is also critical. The proposed CoM trajectory optimization algorithm achieved state-of-art fast computation via trajectory parameterization with several reasonable assumptions and linear algebra tricks. Last, an online weight adaptation approach is presented in the study to stabilize slippery motions within the WBC framework. This can help a robot to manage the slippage at the very last control step by redistributing the desired GRF. In order to verify the effectiveness of our method, we have tested our algorithm and provided benchmarks in simulation using a magnetic-legged climbing robot Manegto.

1 Introduction

Climbing robots can keep humans away from dangerous tasks such as inspection of vertical structures. Various types of climbing robots are developed with different adhesion modalities: suction Tummala et al. (2002), biologically inspired adhesion, for example, micro-splines Spenko et al. (2008); Parness et al. (2017) and micro-fibrillar Kim et al. (2007). Magnetic adhesion is predominant in industry Jose et al. (2018), for maintenance and inspection purposes of ferromagnetic surfaces. Robots with wheels Tavakoli et al. (2013); Eich and Vögele (2011) and continuous tracks Kermorgant (2018) are often used with magnetic adhesion but these robots have difficulties in traversing complex 3D structure. Legged platforms, on the other hand, offer greater mobility given their ability to execute discontinuous contact transition Bandyopadhyay et al. (2018). However, these benefits are achieved at the cost of increased complexity requiring great intelligence to control a robot over the unstructured ground.

1.1 What do we need to concern about for climbing behaviors?

Most current studies on climbing robots focused on their design and mechanism and only a few studies have been conducted on dynamic robot climbing behavior. Several studies propose to utilize biologically inspired templates to generate climbing motions Brown et al. (2018), Lynch et al. (2012), but their implementations are limited to certain behavior and do not consider any constraint regarding safe holdings that should be guaranteed for stable climbing. Motion planning and control have also been studied for free-climbing Bretl (2006); Miller and Rock (2008) and vertical wall climbing Lin et al. (2019); Lin et al. (2018). However, most works are also limited since the problems are formulated quasi-static to avoid computational complexity.

Apparently, the climbing and walking behavior of legged robots share a common mechanism: multi-contact transition to relocate their footholds to the proper position. It is more challenging for climbing robots as they need to hold onto an inclined surface while lifting their entire mass. Besides, given that most climbing robots are developed to inspect in an unknown environment, assumed adhesion and friction quality can often be different from the real values. Therefore, the need for a method to avoid falling and deal with unexpected slippage cannot be overemphasized especially for climbing robots.

1.2 How can we deal with slippage using robot control?

A combination of walking pattern parameter adaptation (long-term strategy) and force control (short-term strategy) to compensate for slippages is effective for robots walking on a slippery ground Takemura et al. (2005). However, predefined walking pattern adaptation can recover only from the limited amount of slippage. Instead, the idea to handle slip force as disturbance acting on a robot is suggested by Kaneko et al. (2005). A humanoid can avoid falling by estimating slip force via observer and compensating for the reactive motion caused by slip force. Focchi et al. (2018); Jenelten et al. (2019) proposed slip detection and recovery mechanism for quadruped robots developed to be employed in a WBC framework. However, both methods can still fail to recover from traction loss without proper centroidal motion re-planning to generate required GRFs to keep contact firm.

1.2.1 Centroidal trajectory optimization

This study proposes to re-plan CoM motion via trajectory optimization to address the above limitations. Generating the proper CoM motion plays a critical role in vertical climbing Brown et al. (2018), though most studies achieve it via biologically inspired motion pattern. Trajectory optimization with centroidal dynamics is becoming popular in locomotion community Orin et al. (2013); Kim et al. (2019); Carpentier and Mansard (2018); Dai et al. (2014) due to its availability to handle friction condition simpler than solving full-body dynamics. Given the estimated adhesion force and friction coefficient, we can help a robot remain firm in contact by generating proper CoM motion. For example, by decelerating CoM in the gravity direction or normal to surface direction, we can relatively increase the normal contact force and reduce the tangential friction force at poor contact.

One well-known strategy to simplify centroidal dynamics optimization is the convexification of the system Ponton et al. (2018). By approximating non-convex quadratic constraint in a discrete-time system, it can obtain optimal CoM trajectory given multi-contact scenarios in 1 s for around 100 time steps. Another effective way to achieve fast computation is parameterization. Winkler et al. (2018); Ahn et al. (2021) leveraged phase-based parameterization to simplify trajectory description. Then, they formulate the problem to automatically determine the gait sequence, step timings, footholds, and swing-leg motions by solving nonlinear optimization in a discrete-time system. Though they provide the more generic form, the problem remains non-convex, and thus it takes about 4 s to be solved. Fernbach et al. (2020) utilized both parameterization and convexification via Bezier curves. However, since the whole trajectory over the multi-contact sequence is determined by one 3d vector decision variable, the generated motion could be limited. Also, the double description (DD) method is known to be unstable means to compute the linear constraints, which can be critical in robotics.

The proposed centroidal trajectory optimization leverages phase-based parameterization and several assumptions are applied to convexify the problem. Also, we showed that inequality constraints hold over the entire time horizon if it holds at the boundary of the function. Reduced dimension of constraints compared to that of the discrete-time domain provides faster computation. As a result, the proposed CoM trajectory optimization algorithm achieves a state-of-the-art fast computation simultaneously with an online contact parameter estimation algorithm to update the CoM trajectory immediately.

1.2.2 QP-based whole-body control framework

Whole-body control (WBC) is a generic task-oriented control method that is particularly useful in controlling redundant robots like legged robots Khatib et al. (2004). A multilevel hierarchical control structure can be established via torque-based whole-body control, which allows humanoid robots to interact with the environment Sentis et al. (2010) compliantly. Optimization-based approaches have been popular for its ability to incorporate inequality constraints addressed within full-body dynamics Escande et al. (2014); Feng et al. (2015); Kim et al. (2020). Task hierarchy can be formulated as hierarchical least-square quadratic problems Escande et al. (2014); Bellicoso et al. (2016), but also can be considered via weighted cost terms in a single quadratic program Feng et al. (2015); Wiedebach et al. (2016). Even though weighted QP formulation can reduce the number of optimization problems to be solved, heuristically determined weights can violate the priority.

Whole-body locomotion controller (WBLC) leveraged a projection-based approach to consider equality task hierarchy and a QP formulation to find the command satisfying inequality constraints and full-body dynamics Kim et al. (2020). WBLC has successfully demonstrated dynamic locomotion of passive ankled robots. However, heuristic weights in QP formulation can violate the task hierarchy considered in task-priority projection-based inverse kinematics, especially when external disturbances or uncertainties exist in the environment. Lee et al. (2021) proposes a feedback gain adaptation method to stabilize external disturbances in WBLC based on the stability analysis. In this study, we suggest online weight adaption in QP formulation to address uncertainties, especially those that cause slippage. By regulating weights regarding reaction force depending on the slip level, we can enforce QP to find the command robust to the slippage.

1.3 Contribution

We summarize the contributions of this study as follows:

• We devise a state-of-the-art fast CoM trajectory optimization, which can be solved in 50 μs for one-step climbing motion. By using phase-based parameterization and several assumptions to convexify the centroidal dynamics, the problem to determine CoM trajectory while satisfying the friction and adhesion condition during the swing phase can be solved about two orders of magnitude faster than other existing methods.

• We propose a Kalman filter–like approach for estimating unknown friction coefficient and (magnetic) adhesion force at slippery contact. Based on the contact force measurement, we linearize the observation model and formulate the system propagating itself to apply Kalman filter algorithm. This can be used in combination with our state-of-the-art fast algorithm to re-plan CoM trajectory to generate an instant slip recovery motion.

• We provide an online weight adaptation approach to be used in a QP-based WBC framework to stabilize the slippage instantly. We increase the normal contact force and decrease the tangential force by regulating the weights for ground reaction force with respect to the slippage rate.

Altogether, our main contribution is an integrated framework that provides planning and control strategies for robot climbing robust to the unknown environment. The proposed CoM trajectory optimization can be solved in real time allowing instant and effective slippage recovery. In combination with an online weight adaptation, the framework yields the best results in dealing with unknown slippery condition.

1.4 Organization of the article

This article is structured as follows:

• Problem definition (Section 2): This section introduces the robot climbing problem. An overview of our problem solving is also described in this section with the summary of WBLC as preliminary studies for the proposed framework.

• Multi-contact CoM trajectory generation for climbing (Section 3): This section describes the proposed CoM trajectory optimization method. Details are addressed from parameterization to approximations and applied linear algebra tricks step by step.

• Contact parameter estimation and CoM re-planning (Section 4): This section develops a slip reflex strategy when the slippage occurs. The overall procedure can be described as follows: 1) Friction coefficient and adhesion force in slippery contacts are estimated via either solving the least-square problem or the Kalman filter–like approach. 2) Given the re-evaluated friction coefficient and adhesion force, CoM re-planning is performed through the revised CoM trajectory optimization described in Section 3 to satisfy the new constraints.

• Online weight adaptation to stabilize slippery motions (Section 5): This section provides an online weight adaptation method that can be used in QP-based WBC. Defining the slippage rate that determines how the slip is severe, the weights can be adjusted to reduce slip behaviors.

• Experimental validation (Section 6): This section shows the performance of the proposed method via simulation experiment using a magnetic legged climbing robot Manegto. We have tested our algorithm and provided benchmarks. Several multi-contact scenarios including the different shapes of the climbing structure and various settings of the contact environment were used for the test.

2 Problem definition

In this study, we are aiming to solve a one-step climbing problem that can be described as follows: given the initial configuration of a robot in static equilibrium and given the next foot placement, the desired CoM position can be calculated—we chose the position close to the initial configuration and far from singularity—and we want to generate optimal trajectories for climbing and design the control laws robust to the environment uncertainties, especially the slip phenomenon.

2.1 Overview

Figure 1 shows an overview of the proposed control architecture. This framework is developed based on a whole-body locomotion controller (WBLC) Kim et al. (2020), which takes prioritized tasks as an input and outputs torque command with desired joint position and velocity commands at the instant time horizon. Usually, tasks prioritized in the following order are widely used in a legged robot: 1) maintaining rigid contact, 2) tracking CoM (centroidal) trajectory, 3) tracking swing foot trajectory, and 4) staying close to the initial configuration. Both CoM trajectory and swing foot trajectory are simply described as a straight line or parabola in most WBC works. However, since CoM trajectory can play an important role in generating required GRF in climbing motion, we propose to generate CoM motion via trajectory optimization. In addition, to deal with the slippage caused by the unknown environment, the estimation of adhesion force and friction coefficient is applied to the slipping contact. The re-evaluated parameters are updated in WBC that computes torque commands that satisfies friction constraints associated with the estimated friction coefficient and full-body dynamics affected by estimated magnetic adhesion. CoM re-planning could be also needed to generate the GRF that lies in the associated friction cone. Finally, a slip-aware online weight adaptation approach is integrated to the work with quadratic programming (QP) formulation in WBC to stabilize slippery motion instantly.

FIGURE 1

FIGURE 1. Control architecture.

2.2 Whole-body locomotion controller extended to climbing

In this section, we first review WBLC with an extended formulation for climbing robots, especially those that use magnetism for adhesion, for example, Magneto Bandyopadhyay et al. (2018). WBLC proposed in Kim et al. (2020) solves the WBC problem by two sequential blocks: a Kinematic-level WBC that considers task hierarchy in inverse kinematics via projection-based method and a Dynamic-level WBC to compute torque command that satisfies all the dynamics constraints by solving quadratic programming (QP).

2.2.1 Magnetic climbing dynamics

Magneto is developed to climb the wall using a switchable electromagnet attached to each foot. Each leg of Magneto consists of actuated joints for actuators and passive joints for gimbals. Given a configuration space $\mathcal{Q}\in {\mathbb{R}}^n$ and an input space $\mathcal{U}\in {\mathbb{R}}^{n_a}$, the dynamics of a robot can be formulated as follows:

$\mathbf{M}\left(\mathbf{q}\right)\ddot{\mathbf{q}}+\mathbf{b}\left(\mathbf{q},\dot{\mathbf{q}}\right)={\mathbf{S}}_a{\mathit{\tau }}_a+{\mathbf{J}}_c{\left(\mathbf{q}\right)}^{⊺}{\mathbf{F}}_c+{\mathbf{J}}_m{\left(\mathbf{q}\right)}^{⊺}{\mathbf{F}}_m,(1)$

where $\mathbf{q}\in \mathcal{Q}$, $\mathbf{M}(\mathbf{q})\in {\mathbb{S}}_{++}^n$, $\mathbf{b}(\mathbf{q},\dot{\mathbf{q}})\in {\mathbb{R}}^n$, and ${\mathit{\tau }}_a\in \mathcal{U}$ denote the joint vector, mass/inertia matrix, sum of Coriolis/centrifugal force and gravitational forces, and actuator command, respectively. ${\mathbf{S}}_a\in {\mathbb{R}}^{n\times n_a}$ denotes the selection matrix indicating the index set of actuated joints, which maps τ_a into the generalized forces. ${\mathbf{F}}_c\in {\mathbb{R}}^{6n_c}$ is a vertically concatenated contact wrench vector, where n_c is the number of contacts and the corresponding contact Jacobian is represented as ${\mathbf{J}}_c(\mathbf{q})\in {\mathbb{R}}^{6n_c\times n}$. Similarly, magnetic force applied to the climbing robot for adhesion can be represented as a stacked wrench vector ${\mathbf{F}}_m\in {\mathbb{R}}^{6n_m}$ and the corresponding Jacobian matrix ${\mathbf{J}}_m(\mathbf{q})\in {\mathbb{R}}^{6n_m\times n}$, where n_m is the number of activated magnetic mechanisms. In this study, we have assumed that both the contact forces and magnetic forces are applied at the center of each foot if and only if contact is made; thus, we have n_c = n_m and J_c(q) = J_m(q). Then the ground reaction force wrench can be represented as F_r = F_c + F_m; the sum of forces are applied to the contacts.

2.2.2 Kinematic level WBC

A kinematic level WBC computes the desired joint command ${\mathbf{q}}^d\in {\mathbb{R}}^n$ given the tasks defined in hierarchy. Let J_k(q) and ${\mathbf{x}}_k^{\text{des}}$ denote kth prioritized task Jacobian and the desired position. Then Δq_k, the change of joint configuration related to the kth task iteration, can be obtained by the propagation below:

$\begin{array}{cc}\hfill {\mathbf{J}}_{k|k-1}\left(\mathbf{q}\right)& ={\mathbf{J}}_k\left(\mathbf{q}\right){\mathbf{N}}_{k-1}\left(\mathbf{q}\right),\hfill \\ \hfill \mathrm{\Delta }{\mathbf{q}}_k& ={\mathbf{J}}_{k|k-1}^{†}\left({\mathbf{x}}_k^{\text{des}}-{\mathbf{x}}_k-{\mathbf{J}}_k\left(\mathbf{q}\right)\mathrm{\Delta }{\mathbf{q}}_{k-1}\right),\hfill \\ \hfill {\mathbf{N}}_k\left(\mathbf{q}\right)& ={\mathbf{N}}_{k-1}\left(\mathbf{q}\right)-{\mathbf{J}}_{k|k-1}{\left(\mathbf{q}\right)}^{†}{\mathbf{J}}_{k|k-1}\left(\mathbf{q}\right),\hfill \\ \hfill {\mathbf{N}}_0\left(\mathbf{q}\right)& ={\mathbf{I}}_{n\times n},\hfill \end{array}(2)$

where N_k(q) and J_k|k−1 represent kth task null-space projection and prioritized Jacobians. Then the desired joint position can be obtained by ${\mathbf{q}}^d=\mathbf{q}+{\sum }_{k=1}^N\mathrm{\Delta }{\mathbf{q}}_k$ in consideration of task priority. Similarly, ${\dot{\mathbf{q}}}^d={\sum }_{k=1}^N{\dot{\mathbf{q}}}_k^d$ and ${\ddot{\mathbf{q}}}^d={\sum }_{k=1}^N{\ddot{\mathbf{q}}}_k^d$ can be obtained from the following propagation:

$\begin{array}{cc}\hfill {\dot{\mathbf{q}}}_0^d& =0,{\ddot{\mathbf{q}}}_0^d=0,\hfill \\ \hfill {\dot{\mathbf{q}}}_k^d& ={\mathbf{J}}_{k|k-1}^{†}\left({\dot{\mathbf{x}}}_k^d-{\mathbf{J}}_k\left(\mathbf{q}\right){\dot{\mathbf{q}}}_{k-1}^d\right),\hfill \\ \hfill {\ddot{\mathbf{q}}}_k^d& ={\mathbf{J}}_{k|k-1}^{†}\left({\ddot{\mathbf{x}}}_k^d-{\dot{\mathbf{J}}}_k\left(\mathbf{q},\dot{\mathbf{q}}\right)\dot{\mathbf{q}}-{\mathbf{J}}_k\left(\mathbf{q}\right){\ddot{\mathbf{q}}}_{k-1}^d\right).\hfill \end{array}(3)$

2.2.3 Dynamic level WBC

A dynamic level WBC calculates the desired torque command and modified joint command that satisfies dynamic constraints based on the optimization framework. This can be achieved by solving QP formulated as follows:

$\begin{array}{c}\underset{\delta \ddot{\mathbf{q}},{\mathbf{F}}_r,{\ddot{\mathbf{x}}}_c}{\min },\delta {\ddot{\mathbf{q}}}^{\top }{\mathbf{W}}_{\ddot{\mathbf{q}}}\delta \ddot{\mathbf{q}}+{\mathbf{F}}_r^{\top }{\mathbf{W}}_f{\mathbf{F}}_r+{\ddot{\mathbf{x}}}_c^{\top }{\mathbf{W}}_c{\ddot{\mathbf{x}}}_c,\\ \mathbf{\text{subject to}}\mathbf{M}\left(\mathbf{q}\right)\ddot{\mathbf{q}}+\mathbf{b}\left(\mathbf{q},\dot{\mathbf{q}}\right)={\mathbf{S}}_a{\mathit{\tau }}_a+{\mathbf{J}}_c{\left(\mathbf{q}\right)}^{⊺}{\mathbf{F}}_c+{\mathbf{J}}_m{\left(\mathbf{q}\right)}^{⊺}{\mathbf{F}}_m,\\ \ddot{\mathbf{q}}={\ddot{\mathbf{q}}}^d+{\mathbf{K}}_d\left({\dot{\mathbf{q}}}^d-\dot{\mathbf{q}}\right)+{\mathbf{K}}_p\left({\mathbf{q}}^d-\mathbf{q}\right)+\delta \ddot{\mathbf{q}}\\ \ddot{{\mathbf{x}}_{\mathbf{c}}}={\mathbf{J}}_c\ddot{\mathbf{q}}+{\dot{\mathbf{J}}}_c\dot{\mathbf{q}},\\ {\mathbf{F}}_r={\mathbf{F}}_c+{\mathbf{F}}_m,\\ \mathbf{U}\left(\mathit{\mu }\right){\mathbf{F}}_c\ge 0,\\ {\mathit{\tau }}^{\text{min}}\le \mathit{\tau }\le {\mathit{\tau }}^{\text{max}},\end{array}(4)$

where ${\mathbf{W}}_{\ddot{\mathbf{q}}}=\mathrm{diag}(w_{\ddot{\mathbf{q}}}^1,\dots ,w_{\ddot{\mathbf{q}}}^n)$, ${\mathbf{W}}_f=\mathrm{diag}(w_f^1,\dots ,w_f^{6n_c})$, and ${\mathbf{W}}_c=\mathrm{diag}(w_c^1,\dots ,w_c^{6n_c})$ are weight matrices corresponding to the joint acceleration relation $\delta \ddot{\mathbf{q}}\in {\mathbb{R}}^n$, ground reaction force ${\mathbf{F}}_r\in {\mathbb{R}}^{6n_c}$, and contact acceleration ${\ddot{\mathbf{x}}}_c\in {\mathbb{R}}^{6n_c}$. ${\mathbf{K}}_p,{\mathbf{K}}_d\in {\mathbb{S}}_{+}^n$ are feedback gains corresponding to the desired joint commands ${\mathbf{q}}^d,{\dot{\mathbf{q}}}^d,{\ddot{\mathbf{q}}}^d\in {\mathbb{R}}^n$ obtained by Kinematic Level WBC, and $\mathbf{U}(\mathit{\mu })\in {\mathbb{R}}^{17n_c\times 6n_c}$ represents diagonally stacked rectangular contact wrench cones (Caron et al., 2015) with respect to the contacts where the friction coefficients are $\mathit{\mu }={[{\mu }_1,\dots ,{\mu }_{n_c}]}^{\top }$. Note that all the force wrenches and corresponding Jacobians in the formulation are expressed from the local body frame. Finally, torque command limits are considered given the minimum and maximum torques of the actuators τ^min, τ^max. By solving the aforementioned QP, we can finally compute the low-level joint torque command that satisfies all the dynamics constraints given the prioritized task.

2.2.4 Phase-based state machine

Phase-based state machines are used together with WBLC to control a legged robot. We defined four state machines for one-step climbing control: full-support, pre-swing transition, swing, and post-swing transition. Each state machine represents a WBLC controller defined to consider different contact phases—e.g., different contact dimensions and weight parameters—so that the multi-contact climbing motion can be controlled by using the proper controller. The details can be found in Kim et al. (2020).

3 Multi-contact CoM trajectory generation for climbing

This planner can cooperate with the WBLC framework by providing the optimal CoM trajectory for climbing motion as the first prioritized task. Given the current configuration and the next desired contact location, we want to find a CoM trajectory for one step climbing motion, which satisfies constraints established under the estimated adhesion forces and friction coefficients.

3.1 Centroidal dynamics with magnetic force and friction cone constraints

In this study, we formulated the problem with respect to the centroidal dynamics of a robot, which is commonly used in the legged robotics community as an effective way to simplify the robot dynamics while considering the contact constraints. By solving the Newton–Euler equations on the center of mass (CoM) of a robot, as described in free body diagram in Figure 2 we can formulate the centroidal dynamics as follows:

$\begin{array}{ccc}\hfill \left[\begin{array}{c}\hfill m{\mathbf{p}}_G\times \left({\ddot{\mathbf{p}}}_G-\mathbf{g}\right)+\dot{\mathbf{L}}\hfill \\ \hfill m\left({\ddot{\mathbf{p}}}_G-\mathbf{g}\right)\hfill \end{array}\right]& =\hfill & \sum _{i\in \text{Contact}}\left[\begin{array}{c}\hfill {\mathbf{p}}_i\times {\mathbf{R}}_i{\mathbf{f}}_{c,i}\hfill \\ \hfill {\mathbf{R}}_i{\mathbf{f}}_{c,i}\hfill \end{array}\right]+\sum _{i\in \text{Magnet}}\left[\begin{array}{c}\hfill {\mathbf{p}}_i\times {\mathbf{R}}_i{\mathbf{f}}_{m,i}\hfill \\ \hfill {\mathbf{R}}_i{\mathbf{f}}_m^i\hfill \end{array}\right],\hfill \\ \hfill & =\hfill & \underset{{\mathcal{R}}_c}{\underbrace{\stackrel{{\mathcal{P}}_c}{\overbrace{\left[\begin{array}{c}\hfill {\mathbf{p}}_{c_1}\times {\mathbf{R}}_{c_1}\cdots \hfill \\ \hfill {\mathbf{R}}_{c_1}\cdots \hfill \end{array}\right]}}}}\underset{{\mathbf{f}}_c}{\underbrace{\left[\begin{array}{c}\hfill {\mathbf{f}}_{c,c_1}\hfill \\ \hfill ⋮\hfill \end{array}\right]}}+\underset{{\mathcal{R}}_m}{\underbrace{\stackrel{{\mathcal{P}}_m}{\overbrace{\left[\begin{array}{c}\hfill {\mathbf{p}}_{m_1}\times {\mathbf{R}}_{m_1}\cdots \hfill \\ \hfill {\mathbf{R}}_{m_1}\cdots \hfill \end{array}\right]}}}}\underset{{\mathbf{f}}_m}{\underbrace{\left[\begin{array}{c}\hfill {\mathbf{f}}_{m,m_1}\hfill \\ \hfill ⋮\hfill \end{array}\right]}}\hfill \\ \hfill & =\hfill & \left[\begin{array}{c}\hfill {\mathcal{P}}_c\hfill \\ \hfill {\mathcal{R}}_c\hfill \end{array}\right]{\mathbf{f}}_c+\left[\begin{array}{c}\hfill {\mathcal{P}}_m\hfill \\ \hfill {\mathcal{R}}_m\hfill \end{array}\right]{\mathbf{f}}_m,\hfill \end{array}(5)$

where m is the mass of the robot, g = [0,0,−9.81]^⊤ is the gravity vector, ${\mathbf{p}}_G\in {\mathbb{R}}^3$ is the robot CoM position, and $\dot{\mathbf{L}}\in {\mathbb{R}}^3$ is angular momentum. ${\mathbf{p}}_i\in {\mathbb{R}}^3$, ${\mathbf{R}}_i\in \mathbb{SO}(3)$ are the position and orientation of the ith foot expressed in the world frame, which are used to map contact force ${\mathbf{f}}_{c,i}\in {\mathbb{R}}^3$, and magnetic force ${\mathbf{f}}_{m,i}\in {\mathbb{R}}^3$ is expressed from the local foot frame. Finally, we defined ${\mathcal{P}}_c,{\mathcal{R}}_c\in {\mathbb{R}}^{3\times 3n_c},{\mathcal{P}}_m,{\mathcal{R}}_m\in {\mathbb{R}}^{3\times 3n_m}$, ${\mathbf{f}}_c\in {\mathbb{R}}^{3n_c}$, and ${\mathbf{f}}_m\in {\mathbb{R}}^{3n_m}$ to simply describe the equation using the stacked matrix and vector form.

FIGURE 2

FIGURE 2. Free body diagram.

In order to describe the friction that resists relative lateral motion between two solid surfaces in contact, the linearized Coulomb friction model was used in this study. It approximately provides a threshold value for friction force parallel to the surface as a function of the normal force. Based on the model, we can formulate the inequality constraints on contact forces as follows:

$\begin{array}{c}\mathbf{D}{\mathbf{f}}_c\ge 0,\\ \text{where,}\mathbf{D}=\mathrm{diag}\left({\mathbf{U}}_f\left({\mu }_{c_1}\right),\cdots \right),\\ {\mathbf{U}}_f\left(\mu \right)=\left(\begin{array}{ccccc}\hfill 0\hfill & \hfill \hfill & \hfill 0\hfill & \hfill \hfill & \hfill 1\hfill \\ \hfill 1\hfill & \hfill \hfill & \hfill 0\hfill & \hfill \hfill & \hfill \tilde{\mu }\hfill \\ \hfill -1\hfill & \hfill \hfill & \hfill 0\hfill & \hfill \hfill & \hfill \tilde{\mu }\hfill \\ \hfill 0\hfill & \hfill \hfill & \hfill 1\hfill & \hfill \hfill & \hfill \tilde{\mu }\hfill \\ \hfill 0\hfill & \hfill \hfill & \hfill -1\hfill & \hfill \hfill & \hfill \tilde{\mu }\hfill \end{array}\right)\text{\hspace{0.17em}with\hspace{0.17em}}\tilde{\mu }=\frac{\mu }{\sqrt{2}}\end{array},(6)$

3.2 Phase-based CoM trajectory parameterization

Legged motion is often represented as a sequence of contact phases. For instance, one step climbing motion for a quadruped can be described as the contact sequence of (1) full support/pre-swing (n_c = 4) → (2) swing (n_c = 3) → (3) full support/post-swing (n_c = 4). In this study, we composed CoM trajectories for climbing considering the contact sequence using cubic Hermite spline, where each piece represents the pre-swing, swing, and post-swing phase sequentially, as shown in Figure 3. Then CoM trajectory can be parameterized as three pieces of cubic Hermite polynomial defined by its duration(T), the initial and goal position of each node (p_i, p_g), and the velocities (v_i, v_g). This phase-based parameterization also matches with our WBLC (whole-body locomotion controller) framework well. Based on four state machines defined within WBLC (full support–transition–swing–transition), given that we can consider the transition as a part of the adjacent full support phase, the parameterized trajectory can be used according to the corresponding state machine.

FIGURE 3

FIGURE 3. CoM trajectory represented by parameterized spline.

However, we still need several assumptions to remove the non-convexity of the problem caused by the cross product term ${\mathbf{p}}_G\times {\ddot{\mathbf{p}}}_G$. Del Prete et al. (2018) utilized the fact that the cross product of the parallel vectors is zero to solve the zero-step capturability problem. To be more specific, they assumed that the best strategy to stop CoM is to decelerate it in the direction CoM moves. Inspired by the strategy, we applied a similar assumption to our spline formulation. During the swing phase and post-swing phase, CoM moves along the straight line but accelerates in the opposite direction: ${\ddot{\mathbf{p}}}_G^{\text{swing}}(t)=\alpha \mathbf{d},{\ddot{\mathbf{p}}}_G^{\text{post\hspace{0.17em}swing}}(t)=\beta \mathbf{d}$, where α, β are constants with different signs and d is an unit vector indicating the direction of straight motion. Then given the initial and goal CoM position p_a and p_b, respectively, CoM trajectory parameterization for one step climbing can be summarized as Table 1.

TABLE 1

TABLE 1. Summary of phase-based CoM trajectory parameterization for one-step climbing motion. Given knots values described in the table, CoM trajectory can be computed via cubic Hermite polynomial (p_HS(t; T, p_i, p_g, v_i, v_g)). Detailed equations are described in Appendix.

3.3 Reformulation of centroidal dynamics based on spline parameterization

Now, we can reformulate the centroidal dynamics by substituting the parameterized CoM trajectory p_HS(t; T, p_i, p_g, v_i, v_g), summarized in Table 1 into Eqs 5, 6. Given that slippage is most likely to happen during (2) swing and (3) post-swing phase, we assumed that parameterized CoM motion that satisfies friction conditions during (2) swing and (3) post-swing phase will also satisfy friction conditions during (1) pre-swing phase. Last, we assumed that both magnetic force f_m and contact force f_c are applied at the center of each foot frame for Magneto only if the foot is in contact, that is, ${\mathcal{P}}_c={\mathcal{P}}_m$ and ${\mathcal{R}}_c={\mathcal{R}}_m$ in Eq. 5. Then we have the following:

2) Swing:

$\begin{array}{ccc}\hfill \left[\begin{array}{c}\hfill {\mathcal{P}}_s-{\left[{\mathbf{p}}_b\right]}_{\times }{\mathcal{R}}_s\hfill \\ \hfill {\mathcal{R}}_s\hfill \end{array}\right]{\mathbf{f}}_c& =\hfill & \left[\begin{array}{c}\hfill \frac{1}{2}m{\left[\mathbf{g}\right]}_{\times }{\left(T_2-t\right)}^2\hfill \\ \hfill m{\mathbf{I}}_3\hfill \end{array}\right]\alpha \mathbf{d}+\left[\begin{array}{c}\hfill \frac{1}{2}m{\left[\mathbf{g}\right]}_{\times }{T}_3\left(T_3+2T_2-t\right)\hfill \\ \hfill 0\hfill \end{array}\right]\beta \mathbf{d}+\left[\begin{array}{c}\hfill \dot{\mathbf{L}}-\left({\mathcal{P}}_s-{\left[{\mathbf{p}}_b\right]}_{\times }{\mathcal{R}}_s\right){\mathbf{f}}_m\hfill \\ \hfill -m\mathbf{g}-{\mathcal{R}}_s{\mathbf{f}}_m\hfill \end{array}\right],\hfill \\ \hfill \iff {\mathbf{A}}_s{\mathbf{f}}_c& =\hfill & {\mathbf{B}}_{sa}\left(t\right)\alpha \mathbf{d}+{\mathbf{B}}_{sb}\left(t\right)\beta \mathbf{d}+{\mathbf{c}}_s,\forall t\in \left(0,T_2\right),\hfill \end{array}(7)$

${\mathbf{D}}_s{\mathbf{f}}_c\ge 0(8)$

where ${\mathcal{P}}_s,{\mathcal{R}}_s\in {\mathcal{R}}^{3\times 9}$ represent the configuration matrices(${\mathcal{P}}_c,{\mathcal{R}}_c$ in Eq. 5) mapping the force vectors ${\mathbf{f}}_c,{\mathbf{f}}_m\in {\mathbb{R}}^9$ during the swing phase. Finally, we denoted the coefficients of the obtained parameterized centroidal dynamics during the swing phase as ${\mathbf{A}}_s\in {\mathbb{R}}^{6\times 9}$, ${\mathbf{B}}_{sa}(t),{\mathbf{B}}_{sb}(t)\in {\mathbb{R}}^{6\times 3}$, ${\mathbf{c}}_s\in {\mathbb{R}}^6$, and the corresponding friction cone matrix as ${\mathbf{D}}_s\in {\mathbb{R}}^{15\times 9}$.

3) Post swing:

$\begin{array}{ccc}\hfill \left[\begin{array}{c}\hfill {\mathcal{P}}_f-{\left[{\mathbf{p}}_b\right]}_{\times }{\mathcal{R}}_f\hfill \\ \hfill {\mathcal{R}}_f\hfill \end{array}\right]{\mathbf{f}}_c& =\hfill & \left[\begin{array}{c}\hfill \frac{1}{2}m{\left[\mathbf{g}\right]}_{\times }{\left(T_3-t\right)}^2\hfill \\ \hfill m{\mathbf{I}}_3\hfill \end{array}\right]\beta \mathbf{d}+\left[\begin{array}{c}\hfill \dot{\mathbf{L}}-\left({\mathcal{P}}_f-{\left[{\mathbf{p}}_b\right]}_{\times }{\mathcal{R}}_f\right){\mathbf{f}}_m\hfill \\ \hfill -m\mathbf{g}-{\mathcal{R}}_f{\mathbf{f}}_m\hfill \end{array}\right],\hfill \\ \hfill \iff {\mathbf{A}}_f{\mathbf{f}}_c& =\hfill & {\mathbf{B}}_f\left(t\right)\beta \mathbf{d}+{\mathbf{c}}_f,\forall t\in \left(0,T_3\right),\hfill \end{array}(9)$

${\mathbf{D}}_f{\mathbf{f}}_c\ge 0,(10)$

where ${\mathcal{P}}_f,{\mathcal{R}}_f\in {\mathcal{R}}^{3\times 12}$ represent the configuration matrices mapping the force vectors ${\mathbf{f}}_c,{\mathbf{f}}_m\in {\mathbb{R}}^{12}$ during the post swing phase. ${\mathbf{A}}_f\in {\mathbb{R}}^{6\times 12}$, ${\mathbf{B}}_f(t)\in {\mathbb{R}}^{6\times 3}$, and ${\mathbf{c}}_f\in {\mathbb{R}}^6$ are coefficients of the parameterized centroidal dynamics during the post-swing phase and ${\mathbf{D}}_f\in {\mathbb{R}}^{20\times 12}$ is the corresponding friction cone matrix.

3.4 Problem solution for parameterized CoM trajectory generation

By applying parameterized CoM spline to the centroidal dynamics, the problem of determining CoM trajectory during one step climbing can be simplified as a problem to find α, β, d that satisfies Eqs 7–10. However, the problem is still non-linear since we have multiplication of variables, αd, βd in equation. Also, the different dimension of contact force vector (f_c) in (7, 8) and (9, 10) makes a problem hard to be solved. Along with the assumptions we made in the CoM trajectory parameterization, we need several more tricks to solve the problem.

3.4.1 Pre-determining the ratio of accelerations

First, we pre-determine $\gamma =\frac{\alpha }{\beta }$, the ratio of the accelerations during the swing phase and the post swing phase. This way, we can linearize the problem by decoupling the constraint between αd and βd. We, specifically, chose γ that minimizes the maximum distance between the CoM goal position and the trajectory over the swing phase. This limits the trajectory to have minimal movement.

$\begin{array}{ccc}\hfill {\gamma }^{\ast }& =\hfill & \arg \underset{\gamma }{\min }\left(\underset{t}{\max }|g_{\gamma }\left(t\right)|,t\in \left(0,T_2\right)\right),\hfill \\ \hfill \mathrm{w}\mathrm{h}\mathrm{e}\mathrm{r}\mathrm{e}& \hfill & {\mathbf{p}}_s\left(t\right)-{\mathbf{p}}_b=g_{\gamma }\left(t\right)\cdot \beta \mathbf{d},t\in \left(0,T_2\right),\hfill \\ \hfill & \hfill & g_{\gamma }\left(t\right)=\frac{1}{2}\gamma {\left(T_2-t\right)}^2+T_3\left(T_2-t\right)+\frac{1}{2}{T}_3^2,\hfill \end{array}$

Assuming γ∗ ≠ 0 and solving for γ < 0, we get the following:

${\gamma }^{\ast }=-\frac{T_3\left(T_2+T_3\right)+T_3\sqrt{{\left(T_2+T_3\right)}^2+T_2^2}}{T_2^2}.(11)$

3.4.2 Contact force determination in centroidal dynamics

We took a simple solution for contact force in centroidal dynamics by mapping the weighted inverse of configuration matrix A_s and A_f. In the centroidal dynamics described in Eqs. 7, 9, given the rank of the equation is six and f_c is either nine or 12, there is an infinite number of possible f_c tracking the given CoM trajectory. One effective way to handle a set of inequalities and equates is using the double description method as Fernbach et al. (2020) reformulate the problem. This way, we can find all feasible solutions based on the numerical conversion between the convex hull description method, but at the same time, it is known to be sometimes computationally unstable, which can be problematic in robotics. Instead, we simply assumed that contact force can be easily obtained by mapping the weighted inverse matrix ${\mathbf{A}}^{-1}={\mathbf{W}}^{-1}{\mathbf{A}}^{\top }{({\mathbf{A}\mathbf{W}}^{-1}{\mathbf{A}}^{\top })}^{-1}$ to the dynamics. This method is quite reliable, and we can configure it to find the contact force as close to the center of the friction cone as possible by designing the weight matrix W considering the shape of friction cone D, where we provided the details in Appendix. Then by denoting x = βd and substituting ${\mathbf{f}}_c={\mathbf{A}}_f^{-1}({\mathbf{B}}_f(t)\beta \mathbf{d}+{\mathbf{c}}_f)$ and ${\mathbf{f}}_c={\mathbf{A}}_s^{-1}(({\mathbf{B}}_{sa}(t){\gamma }^{\ast }+{\mathbf{B}}_{sb}(t))\beta \mathbf{d}+{\mathbf{c}}_s)$ into each inequality equation, we have two linear matrix inequalities (LMI) for each phase.

${\mathbf{D}}_s{\mathbf{A}}_s^{-1}\left({\mathbf{B}}_{sa}\left(t\right){\gamma }^{\ast }+{\mathbf{B}}_{sb}\left(t\right)\right)\mathbf{x}+{\mathbf{D}}_s{\mathbf{A}}_s^{-1}{\mathbf{c}}_s\ge 0,\forall t\in \left(0,T_2\right),(12)$

${\mathbf{D}}_f{\mathbf{A}}_f^{-1}{\mathbf{B}}_f\left(t\right)\mathbf{x}+{\mathbf{D}}_f{\mathbf{A}}_f^{-1}{\mathbf{c}}_f\ge 0,\forall t\in \left(0,T_3\right).(13)$

By splitting the inverse matrix ${\mathbf{A}}_f^{-1},{\mathbf{A}}_s^{-1}$ into left and right column matrices and expressing B_f(t), B_sa(t), and B_sb(t) as a function of t, Eqs (12) and (13) can be rewritten as follows:

$\begin{array}{l}\hfill \left(\frac{1}{2}m\underset{f_s\left(t\right)}{\underbrace{\left({\gamma }^{\ast }{\left(T_2-t\right)}^2+T_3\left(T_3+2T_2-t\right)\right)}}{{\mathbf{D}}_s{\mathbf{A}}_s^{-1}}_{1;3}{\left[\mathbf{g}\right]}_{\times }+m{\gamma }^{\ast }{\mathbf{D}}_s{{\mathbf{A}}_s^{-1}}_{4;6}\right)\mathbf{x}+{\mathbf{D}}_s{\mathbf{A}}_s^{-1}{\mathbf{c}}_s\ge 0,\\ \hfill \left(\frac{1}{2}m\underset{f_f\left(t\right)}{\underbrace{{\left(T_3-t\right)}^2}}{\mathbf{D}}_f{{\mathbf{A}}_f^{-1}}_{1;3}{\left[\mathbf{g}\right]}_{\times }+m{\mathbf{D}}_f{{\mathbf{A}}_f^{-1}}_{4;6}\right)\mathbf{x}+{\mathbf{D}}_f{\mathbf{A}}_f^{-1}{\mathbf{c}}_f\ge 0.\end{array}$

3.4.3 Applying necessary and sufficient condition to satisfy inequality over the given time horizon

Proposition 1. Given an inequality $\left(f(t){\mathbf{B}}_1+{\mathbf{B}}_2\right)\mathbf{x}+\mathbf{c}\ge 0$ defined over a bounded function f_min ≤ f(t) ≤ f_max, ∀t ∈ (0, T), if an inequality holds at the boundary values $\left(f_{\text{min}}{\mathbf{B}}_1+{\mathbf{B}}_2\right)\mathbf{x}+\mathbf{c}\ge 0$ and $\left(f_{\text{max}}{\mathbf{B}}_1+{\mathbf{B}}_2\right)\mathbf{x}+\mathbf{c}\ge 0$ for x, then $\left(f(t){\mathbf{B}}_1+{\mathbf{B}}_2\right)\mathbf{x}+\mathbf{c}\ge 0,\forall t\in (0,T)$

The detailed proof of Proposition 1 is provided in Appendix. Based on Proposition 1, the problem to find x that satisfies inequalities along t can be reduced to the inequalities at the boundary of quadratic functions $f_f(t)={(T_3-t)}^2,t\in (0,T_3)$ and $f_s(t)={\gamma }^{*}{(T_2-t)}^2+T_3(T_3+2T_2-t),t\in (0,T_2)$. Then by stacking inequalities for f_min and f_max, we finally obtained the condition for x = βd:

$\underset{{\mathbf{D}}_x}{\underbrace{\left[\begin{array}{c}\hfill \frac{1}{2}m{f}_{s,\mathrm{m}\mathrm{i}\mathrm{n}}{\mathbf{D}}_s{{\mathbf{A}}_s^{-1}}_{1;3}{\left[\mathbf{g}\right]}_{\times }+m{\gamma }^{\ast }{\mathbf{D}}_s{{\mathbf{A}}_s^{-1}}_{4;6}\hfill \\ \hfill \frac{1}{2}m{f}_{s,\mathrm{m}\mathrm{a}\mathrm{x}}{\mathbf{D}}_s{{\mathbf{A}}_s^{-1}}_{1;3}{\left[\mathbf{g}\right]}_{\times }+m{\gamma }^{\ast }{\mathbf{D}}_s{{\mathbf{A}}_s^{-1}}_{4;6}\hfill \\ \hfill \frac{1}{2}m{f}_{f,\mathrm{m}\mathrm{i}\mathrm{n}}{\mathbf{D}}_f{{\mathbf{A}}_f^{-1}}_{1;3}{\left[\mathbf{g}\right]}_{\times }+m{\mathbf{D}}_f{{\mathbf{A}}_f^{-1}}_{4;6}\hfill \\ \hfill \frac{1}{2}m{f}_{f,\mathrm{m}\mathrm{a}\mathrm{x}}{\mathbf{D}}_f{{\mathbf{A}}_f^{-1}}_{1;3}{\left[\mathbf{g}\right]}_{\times }+m{\mathbf{D}}_f{{\mathbf{A}}_f^{-1}}_{4;6}\hfill \end{array}\right]}}\mathbf{x}+\underset{{\mathbf{d}}_x}{\underbrace{\left[\begin{array}{c}\hfill {\mathbf{D}}_s{\mathbf{A}}_s^{-1}{\mathbf{c}}_s\hfill \\ \hfill {\mathbf{D}}_s{\mathbf{A}}_s^{-1}{\mathbf{c}}_s\hfill \\ \hfill {\mathbf{D}}_f{\mathbf{A}}_f^{-1}{\mathbf{c}}_f\hfill \\ \hfill {\mathbf{D}}_f{\mathbf{A}}_f^{-1}{\mathbf{c}}_f\hfill \end{array}\right]}}\ge 0.(14)$

3.4.4 Parameterized CoM trajectory optimization

Given the predefined sequence of contact for one step climbing motion, we parameterized the CoM trajectory with three polynomials corresponding to each contact phase and found the sufficient condition for the parameters that satisfy the centroidal dynamics and linearized friction cone inequalities as described in Inequality 14. Based on parameterization and all the assumptions and tricks we apply to simplify the problem, the problem to determine CoM trajectory can be reduced to the quadratic programming for x as follows:

$\begin{array}{c}\mathrm{m}\mathrm{i}\mathrm{n}\Vert \mathbf{x}{\Vert }^2,\\ \text{subject\hspace{0.17em}to\hspace{0.17em}}{\mathbf{D}}_x\mathbf{x}+{\mathbf{d}}_x\ge 0,\end{array}(15)$

where $\mathbf{x}=\beta \mathbf{d}\in {\mathbb{R}}^3$, ${\mathbf{D}}_x\in {\mathbb{R}}^{70\times 3}$, and ${\mathbf{d}}_x\in {\mathbb{R}}^{70}$.

4 Contact parameter estimation and CoM re-planning

4.1 Friction and magnetic adhesion force estimation

If we have an F/T sensor on the feet so that we can measure the reaction force at each contact, we can estimate the friction and magnetic adhesion force. Once the slippage is detected, then the frictional force can be formulated at the sliding foot as follows:

$f_t=\sqrt{f_x^2+f_y^2},(16)$

${\widehat{f}}_t=\widehat{\mu }\left(f_z+{\widehat{f}}_m\right),(17)$

where ${[f_x,f_y,f_z]}^{\top }$ is the measured contact force represented in the local foot coordinate, $f_t\in \mathbb{R}$ is the measured friction force which is equivalent to the tangential contact force, and ${\widehat{f}}_t\in \mathbb{R}$ is the estimated friction force computed based on the normal contact force$(f_z\in \mathbb{R})$, the estimated friction coefficient $\widehat{\mu }\in \mathbb{R}$, and magnetic adhesion force $({\widehat{f}}_m\in \mathbb{R})$. Let us define the parameter we want to estimate as $\mathit{\theta }={[\widehat{\mu },\widehat{\mu }{\widehat{f}}_m]}^{\top }$ and assume that we can access the T-period of time sampling data for the estimation. Then we can estimate the parameter by solving the least-square problem below:

${\mathit{\theta }}^{\ast }=\arg \underset{\mathit{\theta }}{\min }\sum _{t=1:T}\Vert f_t\left(t\right)-\mu \left(f_z\left(t\right)+f_m\right){\Vert }^2.$18)

By solving the first order necessary condition (FONC), we get

$\left[\begin{array}{cc}\hfill \sum f_z\left(t\right)\hfill & \hfill \sum 1\hfill \\ \hfill \sum f_z{\left(t\right)}^2\hfill & \hfill \sum f_z\left(t\right)\hfill \end{array}\right]\left[\begin{array}{c}\hfill \widehat{\mu }\hfill \\ \hfill \widehat{\mu }{\widehat{f}}_m\hfill \end{array}\right]=\left[\begin{array}{c}\hfill \sum f_t\left(t\right)\hfill \\ \hfill \sum f_t\left(t\right)f_z\left(t\right)\hfill \end{array}\right].(19)$

However, the least square estimation can be problematic if the contact is unstable. This can give inconsistent estimation value, which can be sometimes even unusable. In order to treat this issue, we propose the Kalman filter–like approach for estimating the friction coefficient and magnetic adhesion force. We first assumed that the system is propagating itself and has a T-period of time sampling ground reaction force data as observation. Also, we assumed that the measured normal force can be used as an observation model rather than observed data to simply linearize the model. This way, we can estimate the environment parameters more robust to the noise.

$\begin{array}{c}{\mathit{\theta }}_{k+1}={\mathit{\theta }}_k+{\mathbf{w}}_k,{\mathbf{w}}_k\sim \mathcal{N}\left(0,{\mathbf{Q}}_k\right),\\ {\mathbf{z}}_k={\mathbf{H}}_k{\mathit{\theta }}_k+{\mathbf{v}}_k,{\mathbf{v}}_k\sim \mathcal{N}\left(0,{\mathbf{R}}_k\right),\\ \mathrm{w}\mathrm{h}\mathrm{e}\mathrm{r}\mathrm{e}{\mathbf{z}}_k=\left[\begin{array}{c}\hfill f_t\left(t_k-T+1\right)\hfill \\ \hfill ⋮\hfill \\ \hfill f_t\left(t_k\right)\hfill \end{array}\right],{\mathbf{H}}_k=\left[\begin{array}{cc}\hfill f_z\left(t_k-T+1\right)\hfill & \hfill 1\hfill \\ \hfill ⋮\hfill & \hfill ⋮\hfill \\ \hfill f_z\left(t_k-T+1\right)\hfill & \hfill 1\hfill \end{array}\right]\end{array}.(20)$

Though the formulated system is non-linear, given that the observation model H_k is not a constant but a function of the observed data, we just considered it as a time-varying observation model to apply the Kalman filter algorithm. Then we have the following:

$\begin{array}{ll}\hfill {\mathbf{K}}_k& =\left({\mathbf{P}}_{k-1}+{\mathbf{Q}}_k\right){\mathbf{H}}_k^{\top }{\left({\mathbf{H}}_k\left({\mathbf{P}}_{k-1}+{\mathbf{Q}}_k\right){\mathbf{H}}_k^{\top }+{\mathbf{R}}_k\right)}^{-1}\hfill \\ \hfill {\mathbf{P}}_k& =\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right)\left({\mathbf{P}}_{k-1}+{\mathbf{Q}}_k\right),\hfill \\ \hfill {\widehat{\mathit{\theta }}}_k& ={\widehat{\mathit{\theta }}}_{k-1}+{\mathbf{K}}_k\left({\mathbf{z}}_k-{\mathbf{H}}_k{\widehat{\mathit{\theta }}}_{k-1}\right).\hfill \end{array}(21)$

4.2 CoM re-planning for slip reflex

When a robot climbs in an unknown environment, we can expect a robot to experience slip behavior when the uncertainty for a contact is high, for example, when the foot in contact was a swing foot in the previous step. If a slip is detected and the estimated friction coefficient and adhesion forces are significantly inferior, then we will need to re-plan the CoM trajectory for generating the desirable contact forces to keep rigid contact. The slip is supposed to be detected during the pre-swing or swing phase when the uncertainty for a contact is high. If slip is detected during the pre-swing phase, then the CoM trajectory optimization for re-planning will have the same formulation as the one solved in Section 3. The re-computed α, β, d for re-evaluated $\widehat{\mu },{\widehat{F}}_m$ can be used to update CoM trajectory. The only difference is the initial position and velocity of the CoM set to zero in the previous problem will be substituted into the current position and velocity of CoM.

4.2.1 Revised CoM trajectory optimization algorithm for re-planning in the swing phase

The CoM re-planing problem can be defined as follows: given the current and the desired configuration; given the current and desired CoM position p_a, p_b; and given the current and desired contacts configuration${\mathcal{P}}_s,{\mathcal{P}}_f,{\mathcal{R}}_s,{\mathcal{R}}_f$, find the CoM trajectory that satisfies the new friction condition. Let t_now be the time past since a robot started the swing phase, then we can re-parameterize the CoM trajectory for the remaining climbing motion as Table 2.

TABLE 2

TABLE 2. Revised CoM parameterization for the swing phase re-planning.

Then the centroidal dynamics during the swing and post-swing phase can be rewritten as follows:

2) Swing:

$\left[\begin{array}{c}\hfill {\mathcal{P}}_s-{\left[{\mathbf{p}}_a\right]}_{\times }{\mathcal{R}}_s\hfill \\ \hfill {\mathcal{R}}_s\hfill \end{array}\right]{\mathbf{f}}_c=\left[\begin{array}{c}\hfill \left({\left[{\mathbf{p}}_a\right]}_{\times }{\mathcal{R}}_s-{\mathcal{P}}_s\right){\mathbf{f}}_m+\dot{\mathbf{L}}\hfill \\ \hfill -m\mathbf{g}-{\mathcal{R}}_s{\mathbf{f}}_m\hfill \end{array}\right]+m\left[\begin{array}{c}\hfill \frac{1}{2}{t}^2{\left[\mathbf{g}\right]}_{\times }\hfill \\ \hfill \mathbf{I}\hfill \end{array}\right]\alpha \mathbf{d}.(22)$

3) Post swing:

$\begin{array}{ll}\hfill \left[\begin{array}{c}\hfill {\mathcal{P}}_f-{\left[{\mathbf{p}}_1\right]}_{\times }{\mathcal{R}}_f\hfill \\ \hfill {\mathcal{R}}_f\hfill \end{array}\right]{\mathbf{f}}_c& =\left[\begin{array}{c}\hfill \left({\left[{\mathbf{p}}_a\right]}_{\times }{\mathcal{R}}_f-{\mathcal{P}}_f\right){\mathbf{f}}_m+\dot{\mathbf{L}}\hfill \\ \hfill -m\mathbf{g}-{\mathcal{R}}_f{\mathbf{f}}_m\hfill \end{array}\right]+m\left[\begin{array}{c}\hfill f_1\left(t\right){\left[\mathbf{g}\right]}_{\times }\hfill \\ \hfill f_2\left(t\right)\mathbf{I}\hfill \end{array}\right]\alpha \mathbf{d},\hfill \\ \hfill \mathrm{w}\mathrm{h}\mathrm{e}\mathrm{r}\mathrm{e}{f}_1\left(t\right)& =-\frac{1}{2}{T}_2^{\prime 2}\left(-2{\left(\frac{t}{T_3}\right)}^3+3{\left(\frac{t}{T_3}\right)}^2\right)+T_2^{\prime }\cdot T_3\left({\left(\frac{t}{T_3}\right)}^3-2{\left(\frac{t}{T_3}\right)}^2+\left(\frac{t}{T_3}\right)\right),\hfill \\ \hfill f_2\left(t\right)& =-\frac{1}{2}{\left(\frac{T_2^{\prime }}{T_3}\right)}^2\left(-12\frac{t}{T_3}+6\right)+\frac{T_2^{\prime }}{T_3}\left(6\frac{t}{T_3}-4\right).\hfill \end{array}(23)$

Then similar to the problem solving we described in Eq. 12 ∼(15), we can reformulate the optimization problem for x = αd with ${\mathbf{D}}_x^{\prime }$ and ${\mathbf{d}}_x^{\prime }$ defined similar to D_x and d_x in Eq. 14 as follows:

$\begin{array}{c}\mathrm{m}\mathrm{i}\mathrm{n}\Vert \mathbf{x}{\Vert }^2,\\ \text{subject\hspace{0.17em}to\hspace{0.17em}}{\mathbf{D}}_x^{\prime }\mathbf{x}+{\mathbf{d}}_x^{\prime }\ge 0\end{array}.(24)$

5 Online weight adaptation to stabilize slippery motions

In this section, we present an online weight adaptation approach to stabilize slippery motions by redistributing the contact force at each contact instantly. As discussed in the previous section, the final torque commands are computed by solving QP-based formulation in Dynamic level WBC block. Weights in QP formulation.

5.1 Slippage rate for weight adaptation

In order to update weight gains according to the slippage rate in contact, we need a way to measure the rate of slippage. In this study, we defined the slippage rate based on slip velocity at each contact as follows:

$\begin{array}{ccc}\hfill {\alpha }_s& =\hfill & \left\{\begin{array}{cc}1\hfill & \text{if\hspace{0.17em}}\Vert {\mathbf{v}}_{\text{contact\hspace{0.17em}foot}}\Vert \le v_{\text{threshold}}\hfill \\ \frac{\Vert {\mathbf{v}}_{\text{contact\hspace{0.17em}foot}}\Vert }{v_{\text{threshold}}}\hfill & \mathrm{o}\mathrm{t}\mathrm{h}\mathrm{e}\mathrm{r}\mathrm{w}\mathrm{i}\mathrm{s}\mathrm{e}\hfill \end{array}\right.\hfill \\ \hfill {\mathbf{v}}_{\text{contact\hspace{0.17em}foot}}& =\hfill & \text{LOW\hspace{0.17em}PASS\hspace{0.17em}FILTER}\left({\mathbf{J}}_c\left(\mathbf{q}\right)\dot{\mathbf{q}}\right).\hfill \end{array}$

By defining the rate with the threshold, we can provide weight adaptation only if the slippage is large enough and the rate parameter itself is continuous as well.

5.2 Weight adaptation to the slippage

Based on the structure of WBLC, we formulated an extension version of WBLC for climbing in Section 2.2. Recalling that the cost function is designed to minimize the ground reaction force and tracking error:

$\underset{\delta \ddot{\mathbf{q}},{\mathbf{F}}_r,{\ddot{\mathbf{x}}}_c}{\min },\delta {\ddot{\mathbf{q}}}^{\top }{\mathbf{W}}_{\ddot{\mathbf{q}}}\delta \ddot{\mathbf{q}}+{\mathbf{F}}_r^{\top }{\mathbf{W}}_f{\mathbf{F}}_r+{\ddot{\mathbf{x}}}_c^{\top }{\mathbf{W}}_c{\ddot{\mathbf{x}}}_c,$

Let us say we have contact foot 1, 2, … , c. Then the corresponding slippage ratio and weight matrix can be described as follows:

$\begin{array}{ccc}\hfill {\alpha }_s& =\hfill & \left({{\alpha }_s}_1,\dots ,{{\alpha }_s}_c\right),\hfill \\ \hfill {\mathbf{W}}_f& =\hfill & \mathrm{diag}\left(w_{f{x}_1},w_{f{y}_1},w_{f{z}_1},\dots ,w_{f{x}_c},w_{f{y}_c},w_{f{z}_c}\right).\hfill \end{array}$

Typically, we used predefined weight parameters in WBC and usually took $w_{f{x}_i},w_{f{y}_i}=w_{xy}$ and $w_{f{z}_i}=w_z$. Once we have foot in slippery contact, ${{\alpha }_s}_i>1$, we can update the corresponding weight parameters based on the ratio online as follows:

$w_{f{x}_i},w_{f{y}_i}=\left\{\begin{array}{c}{{\alpha }_s}_i{w}_{xy},\hfill \\ \frac{1}{{{\alpha }_s}_i}{w}_{xy},\hfill \end{array}\right.\text{and\hspace{0.17em}},w_{f{z}_i}=\left\{\begin{array}{cc}\frac{1}{{{\alpha }_s}_i}{w}_z,\hfill & \text{if}{{\alpha }_s}_i>1,\hfill \\ {{\alpha }_s}_i{w}_z,\hfill & \text{otherwise}.\hfill \end{array}\right.(25)$

6 Experimental validation

In this section, we focus on showing the performance of the proposed framework. In Section 6.1, we present how the proposed algorithm yields competitive performance against other CoM trajectory generation methods. Then, in Section 6.2, parameter estimation to identify unknown slippery condition is presented. Finally, in Section 6.4, we have demonstrated robot climbing behavior in unknown slippery conditions with and without the proposed strategies to evaluate the performance of the algorithms.

6.1 Comparison benchmarks of proposed CoM trajectory optimization

In order to show the performance of our CoM trajectory optimization, we choose to compare the computation time of the proposed algorithm with a state-of-the-art convexified formulation Ponton et al. (2018) and parameterized formulation (CROC) Fernbach et al. (2020). Generated CoM trajectory and corresponding ground reaction force distribution are also analyzed for comparison. The codes used in our benchmark modified for climbing is provided for CROC¹ and convexified centroidal optimization², and for the proposed method³.

For these benchmarks, we have performed a climbing simulation in two different structures.

• The first structure described in Figure 4A is a flat slope inclined at α = 1 rad, where all the contacts of a robot are always on the same planar. One of the contacts is set particularly poor, where the environment parameter of the left bottom foot is set to μ_s = 0.3 and f_m = 30(N), while other feet are set to μ_s = 0.5 and f_m = 70(N).

• The second structure described in Figure 4B is a hexagonal structure, which leads a robot to climb with a set of non-co-planar contacts. All the parameters that determine the contact quality are given the same for all feet as μ_s = 0.5 and f_m = 70(N).

FIGURE 4

FIGURE 4. Multi-contact scenarios on (A) a flat slope at α =1rad and (B) a hexagonal structure. A robot is climbing up on each structure by moving its feet in the order of left bottom (1→2), right top (2→3), left top (3→4), and right bottom (4→5).

In each structure, a robot is climbing up on a wall by moving its feet in the order of left bottom, right top, left top, and right bottom foot. All the benchmark simulations were run on Ubuntu 18.04.5 LTS with Processor Intel® Core^TM i7-8700K CPU @ 3.70GHz × 12, Memory 16 GB. QuadProg++ (Goldfarb and Idnani (1983)) were used to solve the QP problem in our software.

As described in Table 3, the computation time of the proposed algorithm is about 4 ∼5 orders of magnitude faster than the convexified centroidal dynamics optimization method and two orders of magnitude faster than CROC. This result is natural considering that the dimension of convexified centroidal dynamics optimization is much larger because it solves the problem for the time discretization variables. On the other hand, parameterization can reduce the dimension of the problem effectively. Though CROC also uses parameterization, the double description (DD) method to handle inequality conditions in CROC takes a relatively long time. Instead, we utilized the weighted inverse matrices to determine contact forces in the centroidal dynamics equation, which helps us to find a reasonable solution two orders of magnitude faster. Furthermore, as the DD method is known to suffer from computational instability, which can cause a critical problem in robotics, the proposed solving method can be beneficial in terms of computation stability as well.

TABLE 3

TABLE 3. Computation time comparison benchmark of CoM trajectory optimization for two different climbing scenarios between the proposed algorithm and other existing methods. Computation time is averaged over 100 tries for each scenario.

Not surprisingly, the trajectory obtained by different algorithms was different as shown in Figure 5. It is natural, considering that there are always an infinite number of trajectories that satisfy constraints in many trajectory optimization problems. One of the advantages we can take from parameterization is that we can expect a shape of the trajectory planning. This is important in robotics since unexpected plans can often cause a critical problem. In addition, the more variables the problem has, the more weight parameters we need to tune. In Ponton et al. (2018), the obtained trajectories were sensitive to the choice of weights, resulting in another difficulty in weight tuning. Finally, as we obtain the contact force placed as close as possible to the center of the friction cone, we can see the desired contact forces obtained by the proposed method tend to have more margin in inequalities.

FIGURE 5

FIGURE 5. The comparison of centroidal climbing motion obtained by different algorithms. One-step climbing motions for the right top foot are compared on two structures [(A) a flat slope at α = 1 rad and (B) a hexagonal structure]. The plots on the right show the corresponding desired GRF distribution on each foot with friction inequality constraints.

6.2 Parameter estimation

As described in Section 4, unknown environment parameters μ and F_m can be estimated based on the friction force model when a slip occurs. Figure 6 shows the parameter estimation based on two different algorithms. Once the contact velocity exceeds the threshold, we prepare the parameter estimation, assuming that there is a slip. Though we assumed that we can detect the slippage by thresholding the contact velocity computed based on the floating base configuration, this cannot be achieved in the real legged robot system. Instead, either comparing the velocity of the contact and the median of the velocity of all contacts Focchi et al. (2018) or a probabilistic state machine Jenelten et al. (2019) can be considered for slip detection in real robots.

FIGURE 6

FIGURE 6. Estimation of unknown environment parameters $={[\mu ,\mu F_m]}^{\top }$ during the slip condition. (A) shows the condition of the estimation activation. Once the contact velocity exceeds the threshold during the certain amount of time, we declare the slip detection so that the estimation can be executed. Yellow areas in the plots represent where the estimation is activated. (B) and (C) present the resulting estimation obtained by two different approaches, least-squares, and Kalman filter–like approach.

Once the slip detection is triggered, we store the contact force data during 15 time samples. Then, we can estimate the unknown parameters $\theta ={[\mu ,\mu F_m]}^{\top }$ that tell how slippery the surface is via either least-square estimation or Kalman filter-like approach. As you can see from Figure 6B, parameter estimation through the least-square estimation was not desirable; The observation of contact forces is too noisy even in simulation, and observation data captured during the traction loss doesn’t match with the friction force model we used for estimation. On the other hand, Kalman filter–like approach worked well with a small value for model covariance Q_k = diag([0.000004, 0.01]), and a relatively large value for observation covariance R_k = 0.25. From the initial guess of parameters, you can see it converges in 30ms to the ground-truth value in Figure 6C.

6.3 Slip-aware online weight adaption for QP based WBC

In Section 5, we present an online weight adaptation for QP-based WBC framework. Figure 7 shows a part of motion suffering from sliding. It describes how the proposed algorithm shapes the corresponding weights based on the slip velocity and how it affects the corresponding ground reaction forces. Once the slip velocity exceeds the threshold, it starts to change the weights, and this can help a robot to slightly redistribute contact forces to stop sliding.

FIGURE 7

FIGURE 7. Slip phenomenon with and without an online weight adaptation. (A) shows the linear velocity and ground reaction force at the contact in slippery condition without any adaptation and (B) shows reduced slippage at the contact in the same condition with the proposed weight adaptation.

6.4 Climbing in unknown slippery condition

In Section 4 and Section 5, we proposed CoM trajectory re-planning based on re-evaluated parameters and an online weight adaptation to stabilize a slip. Lastly, in this section, to verify the performance of the proposed control framework in terms of robustness to the unknown slippage, we examine the slip velocity of the foot in unknown slippery conditions with respect to the following 4 controllers:

• WBLC without any adaptation,

• WBLC with parameter estimation and CoM re-planning,

• WBLC with an online weight adaptation,

• WBLC with both methods.

In this simulation, the controller assumes the environment parameters for all feet to be μ = 0.5, F_m = 50N, while the parameters of the left bottom foot are set to μ = 0.3, F_m = 20N. The climbing scenario on a flat slope described in Figure 4A was used for this verification test.

In Figure 8, the slip velocities of the left bottom foot for each controller are shown with snapshots of the simulation. As shown in Figure 8A, without any adaptation, the controller cannot compute the adequate trajectory and control satisfying the real friction constraints, and that eventually results in contact loss at the left bottom foot after four climbing steps are made. You can also see that a slip tends to occur during the swing phase and post swing transition where one of the feet is used for making a step.

FIGURE 8

FIGURE 8. Comparative analysis on the slippage of the foot in unknown slippery conditions for each controller: (A) WBLC without any adaptation, (B) WBLC with unknown parameter estimation and CoM replanning (described in Section 4), (C) WBLC with online weight adaptation (described in Section 5), and (D) WBLC with both methods [applied in (B) and (C)]. Slip velocities of the left bottom foot and snapshots of the simulation are represented. After the first climbing step where the left bottom foot is used for swing, the velocity is supposed to be zero. Blue dots in the last zoom-in shots represent the desired foot position.

FIGURE 9

FIGURE 9. Linearized friction cone.

Figures 8B,C shows both CoM replanning and an online weight adaptation for WBC are effective to stabilize the slippery motion. The difference shown in the results for the two methods is that the online weight adaptation method faces the second slip once the slippery is stabilized especially around the start of the post-swing transition phase for each climbing step. That could be caused since the weights are initialized in every phase in our controller. Finally, as shown in Figure 8D, we validated that the slip can be prevented the most when both methods are applied.

7 Conclusion

In this study, we have presented several strategies to stabilize known and unknown slippery motion within a WBLC framework. In an effort to stabilize unknown slip, CoM re-planning based on unknown parameter estimation and online weight adaptation for WBC were presented. When the environment parameters are unknown and different from what we assume when we solve the problem, the obtained solution can suffer from unknown behaviors, for example, slip, traction loss, etc. Based on the friction force model, the friction coefficient and the friction force limit calculated from the adhesion force can be estimated when the contact is under the slip condition. Then, CoM re-planning is performed based on the proposed CoM trajectory optimization algorithm. The proposed CoM trajectory optimization achieved fast computation by leveraging a phase-based parameterization and we verified it by providing comparison benchmarks with other state-of-art algorithms. Therefore, the proposed CoM trajectory generation method allows for CoM re-planning in real time, and this can reduce the slip by generating adequate contact forces to increase the normal force and decrease the tangential force at slippery contact. The proposed strategies are shown to be effective to reduce slip through the simulation experiment.

Although we have shown that the proposed methods are fast and effective to stabilize the slippery motion, traction loss could be another issue that can cause devastating result especially for overhang climbing as you can see from the provided video. The proposed CoM optimization algorithm is expected to be extended to prevent a robot from those fetal movements. While working on this study, we also found that the wrong estimated parameter can make robot climbing more problematic, even though the estimation are sorely dependent on the noisy and uncertain observation. In addition, this study does not provide thorough theoretical analysis on the stability of the proposed method. In that in mind, instability resulting from the parameter estimation and stability analysis on weight adaptation should be addressed in our future research.

Lastly, our approach has not been tested yet on a real robot, although there could be a lot more challenges arising from hardware experiment. For example, in a real robot setup, a robot can always be susceptible to kinematic singularities, discretization of dynamics and model error, etc. Though we didn’t mention it in this study, we consider the singularity when we set the goal CoM position. We solve the optimization problem that is formulated to find the goal configuration far from singularities given the next foot placement, and we are using the goal CoM position obtained from the configuration. As for the dynamics errors, augmented PD control can be used along with the WBC to compensate for errors from the dynamics model. In addition, for legged robot system, it is hard to estimate the base configuration accurately as simulation, though we assumed that we can detect slip based on the floating based robot kinematics. However, we can use other measurement such as F/T sensor values or IMU values to define slippage ratio and apply it for online weight adaptation. Overall, thus verifying the feasibility to implement this work in real hardware setup is the next coming step, as well.

Data availability statement

The datasets presented in this study can be found in online repositories. The names of the repository/repositories and accession number(s) can be found at: https://github.com/jeeeunlee/ros-pnc/tree/devel-mpc-slip.

Author contributions

J-EL formulated the main idea of the planning and control strategy, implemented the controller, and set up the simulation. TB and LS provide advice on writing this manuscript.

Funding

This work has been partially supported by the Office of Naval Research, ONR Grant N000141912311, and by DATA61, CSIRO.

Acknowledgments

The authors would like to thank the members of the Human Centered Robotics Laboratory at The University of Texas at Austin, especially Junhyeok Ahn and Jaemin Lee, for their great help and support.

Conflict of interest

The authors declare that the research was conducted in the absence of any commercial or financial relationships that could be construed as a potential conflict of interest.

Publisher’s note

All claims expressed in this article are solely those of the authors and do not necessarily represent those of their affiliated organizations, or those of the publisher, the editors, and the reviewers. Any product that may be evaluated in this article, or claim that may be made by its manufacturer, is not guaranteed or endorsed by the publisher.

Supplementary material

The Supplementary Material for this article can be found online at: https://www.frontiersin.org/articles/10.3389/frobt.2022.949460/full#supplementary-material

Footnotes

¹https://github.com/jeeeunlee/ccroc_magneto

²https://github.com/jeeeunlee/centroidal_sandbox_magneto

³https://github.com/jeeeunlee/ros_pncbranch:mpc-devel-slip

References

Ahn, J., Jorgensen, S. J., Bang, S. H., and Sentis, L. (2021). Versatile locomotion planning and control for humanoid robots. Front. Robot. AI 8, 712239. doi:10.3389/frobt.2021.712239

PubMed Abstract | CrossRef Full Text | Google Scholar

Bandyopadhyay, T., Steindl, R., Talbot, F., Kottege, N., Dungavell, R., Wood, B., et al. (2018). “Magneto: A versatile multi-limbed inspection robot,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE), Madrid, Spain, 01-05 Oct. 2018, 2253–2260.

CrossRef Full Text | Google Scholar

Bellicoso, C. D., Gehring, C., Hwangbo, J., Fankhauser, P., and Hutter, M. (2016). “Perception-less terrain adaptation through whole body control and hierarchical optimization,” in 2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids) (IEEE), Cancun, Mexico, 15-17 Nov. 2016, 558–564.

Google Scholar

Bretl, T. (2006). Motion planning of multi-limbed robots subject to equilibrium constraints: The free-climbing robot problem. Int. J. Robotics Res. 25, 317–342. doi:10.1177/0278364906063979

CrossRef Full Text | Google Scholar

Brown, J. M., Austin, M. P., Kanwar, B., Jonas, T. E., and Clark, J. E. (2018). “Maneuverability in dynamic vertical climbing,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE), Madrid, Spain, 01-05 Oct. 2018, 4340–4347.

CrossRef Full Text | Google Scholar

Caron, S., Pham, Q.-C., and Nakamura, Y. (2015). “Stability of surface contacts for humanoid robots: Closed-form formulae of the contact wrench cone for rectangular support areas,” in 2015 IEEE International Conference on Robotics and Automation (ICRA) (IEEE), Seattle, WA, USA, 26-30 May 2015, 5107–5112.

CrossRef Full Text | Google Scholar

Carpentier, J., and Mansard, N. (2018). Multicontact locomotion of legged robots. IEEE Trans. Robot. 34, 1441–1460. doi:10.1109/tro.2018.2862902

CrossRef Full Text | Google Scholar

Dai, H., Valenzuela, A., and Tedrake, R. (2014). “Whole-body motion planning with centroidal dynamics and full kinematics,” in 2014 IEEE-RAS International Conference on Humanoid Robots (IEEE), Madrid, Spain, 18-20 Nov. 2014, 295–302.

CrossRef Full Text | Google Scholar

Del Prete, A., Tonneau, S., and Mansard, N. (2018). Zero step capturability for legged robots in multicontact. IEEE Trans. Robot. 34, 1021–1034. doi:10.1109/tro.2018.2820687

CrossRef Full Text | Google Scholar

Eich, M., and Vögele, T. (2011). “Design and control of a lightweight magnetic climbing robot for vessel inspection,” in 2011 19th Mediterranean Conference on Control & Automation (MED) (IEEE), Corfu, Greece, 20-23 June 2011, 1200–1205.

CrossRef Full Text | Google Scholar

Escande, A., Mansard, N., and Wieber, P.-B. (2014). Hierarchical quadratic programming: Fast online humanoid-robot motion generation. Int. J. Robotics Res. 33, 1006–1028. doi:10.1177/0278364914521306

CrossRef Full Text | Google Scholar

Feng, S., Whitman, E., Xinjilefu, X., and Atkeson, C. G. (2015). Optimization-based full body control for the darpa robotics challenge. J. Field Robot. 32, 293–312. doi:10.1002/rob.21559

CrossRef Full Text | Google Scholar

Fernbach, P., Tonneau, S., Stasse, O., Carpentier, J., and Taïx, M. (2020). C-croc: Continuous and convex resolution of centroidal dynamic trajectories for legged robots in multicontact scenarios. IEEE Trans. Robot. 36, 676–691. doi:10.1109/tro.2020.2964787

CrossRef Full Text | Google Scholar

Focchi, M., Barasuol, V., Frigerio, M., Caldwell, D. G., and Semini, C. (2018). “Slip detection and recovery for quadruped robots,” in Robotics research (Berlin/Heidelberg, Germany: Springer), 185–199.

CrossRef Full Text | Google Scholar

Goldfarb, D., and Idnani, A. (1983). A numerically stable dual method for solving strictly convex quadratic programs. Math. Program. 27, 1–33. doi:10.1007/bf02591962

CrossRef Full Text | Google Scholar

Jenelten, F., Hwangbo, J., Tresoldi, F., Bellicoso, C. D., and Hutter, M. (2019). Dynamic locomotion on slippery ground. IEEE Robot. Autom. Lett. 4, 4170–4176. doi:10.1109/lra.2019.2931284

CrossRef Full Text | Google Scholar

Jose, J., Dinakaran, D., Ramya, M., and Harris, D. (2018). A survey on magnetic wall-climbing robots for inspection. Int. J. Mech. Prod. Eng. Res. Dev. 8, 59–68. doi:10.24247/ijmperddec20186

CrossRef Full Text | Google Scholar

Kaneko, K., Kanehiro, F., Kajita, S., Morisawa, M., Fujiwara, K., Harada, K., et al. (2005). “Slip observer for walking on a low friction floor,” in 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems (IEEE), Edmonton, AB, Canada, 02-06 Aug. 2005, 634–640.

CrossRef Full Text | Google Scholar

Kermorgant, O. (2018). A magnetic climbing robot to perform autonomous welding in the shipbuilding industry. Robotics Computer-Integrated Manuf. 53, 178–186. doi:10.1016/j.rcim.2018.04.008

CrossRef Full Text | Google Scholar

Khatib, O., Sentis, L., Park, J., and Warren, J. (2004). Whole-body dynamic behavior and control of human-like robots. Int. J. Hum. Robot. 1, 29–43. doi:10.1142/s0219843604000058

CrossRef Full Text | Google Scholar

Kim, D., Di Carlo, J., Katz, B., Bledt, G., and Kim, S. (2019). Highly dynamic quadruped locomotion via whole-body impulse control and model predictive control. arXiv preprint arXiv:1909.06586. Available at: https://arxiv.org/pdf/1909.06586.pdf.

Google Scholar

Kim, D., Jorgensen, S. J., Lee, J., Ahn, J., Luo, J., Sentis, L., et al. (2020). Dynamic locomotion for passive-ankle biped robots and humanoids using whole-body locomotion control. Int. J. Robotics Res. 39, 936–956. doi:10.1177/0278364920918014

CrossRef Full Text | Google Scholar

Kim, S., Spenko, M., Trujillo, S., Heyneman, B., Mattoli, V., and Cutkosky, M. R. (2007). “Whole body adhesion: Hierarchical, directional and distributed control of adhesive forces for a climbing robot,” in Proceedings 2007 IEEE International Conference on Robotics and Automation (IEEE), Rome, Italy, 10-14 Apr. 2007, 1268–1273.

CrossRef Full Text | Google Scholar

Lee, J., Ahn, J., Kim, D., Bang, S. H., and Sentis, L. (2021). Online gain adaptation of whole-body control for legged robots with unknown disturbances. Front. Robot. AI 8, 788902. doi:10.3389/frobt.2021.788902

PubMed Abstract | CrossRef Full Text | Google Scholar

Lin, X., Krishnan, H., Su, Y., and Hong, D. W. (2018). “Multi-limbed robot vertical two wall climbing based on static indeterminacy modeling and feasibility region analysis,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE), Madrid, Spain, 01-05 Oct. 2018, 4355–4362.

CrossRef Full Text | Google Scholar

Lin, X., Zhang, J., Shen, J., Fernandez, G., and Hong, D. W. (2019). “Optimization based motion planning for multi-limbed vertical climbing robots,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE), Macau, China, 03-08 Nov. 2019, 1918–1925.

CrossRef Full Text | Google Scholar

Lynch, G. A., Clark, J. E., Lin, P.-C., and Koditschek, D. E. (2012). A bioinspired dynamical vertical climbing robot. Int. J. Robotics Res. 31, 974–996. doi:10.1177/0278364912442096

CrossRef Full Text | Google Scholar

Miller, T. G., and Rock, S. (2008). Control of a climbing robot using real-time convex optimization. Mechatronics 18, 301–313. doi:10.1016/j.mechatronics.2007.07.009

CrossRef Full Text | Google Scholar

Orin, D. E., Goswami, A., and Lee, S.-H. (2013). Centroidal dynamics of a humanoid robot. Auton. Robots 35, 161–176. doi:10.1007/s10514-013-9341-4

CrossRef Full Text | Google Scholar

Parness, A., Abcouwer, N., Fuller, C., Wiltsie, N., Nash, J., and Kennedy, B. (2017). “Lemur 3: A limbed climbing robot for extreme terrain mobility in space,” in 2017 IEEE international conference on robotics and automation (ICRA) (IEEE), Singapore, 29 May 2017-03 June 2017, 5467–5473.

CrossRef Full Text | Google Scholar

Ponton, B., Herzog, A., Del Prete, A., Schaal, S., and Righetti, L. (2018). On time optimization of centroidal momentum dynamics. In 2018 IEEE International Conference on Robotics and Automation (ICRA) (IEEE), 21-25 May 2018, Brisbane, QLD, Australia, 5776–5782.

CrossRef Full Text | Google Scholar

Sentis, L., Park, J., and Khatib, O. (2010). Compliant control of multicontact and center-of-mass behaviors in humanoid robots. IEEE Trans. Robot. 26, 483–501. doi:10.1109/tro.2010.2043757

CrossRef Full Text | Google Scholar

Spenko, M. J., Haynes, G. C., Saunders, J., Cutkosky, M. R., Rizzi, A. A., Full, R. J., et al. (2008). Biologically inspired climbing with a hexapedal robot. J. Field Robot. 25, 223–242. doi:10.1002/rob.20238

CrossRef Full Text | Google Scholar

Takemura, H., Deguchi, M., Ueda, J., Matsumoto, Y., and Ogasawara, T. (2005). Slip-adaptive walk of quadruped robot. Robotics Aut. Syst. 53, 124–141. doi:10.1016/j.robot.2005.07.002

CrossRef Full Text | Google Scholar

Tavakoli, M., Viegas, C., Marques, L., Pires, J. N., and De Almeida, A. T. (2013). Omniclimbers: Omni-directional magnetic wheeled climbing robots for inspection of ferromagnetic structures. Robotics Aut. Syst. 61, 997–1007. doi:10.1016/j.robot.2013.05.005

CrossRef Full Text | Google Scholar

Tummala, R. L., Mukherjee, R., Xi, N., Aslam, D., Dulimarta, H., Xiao, J., et al. (2002). Climbing the walls [robots]. IEEE Robot. Autom. Mag. 9, 10–19. doi:10.1109/mra.2002.1160067

CrossRef Full Text | Google Scholar

Wiedebach, G., Bertrand, S., Wu, T., Fiorio, L., McCrory, S., Griffin, R., et al. (2016). “Walking on partial footholds including line contacts with the humanoid robot atlas,” in 2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids) (IEEE), Cancun, Mexico, 15-17 Nov. 2016, 1312–1319.

CrossRef Full Text | Google Scholar

Winkler, A. W., Bellicoso, C. D., Hutter, M., and Buchli, J. (2018). Gait and trajectory optimization for legged systems through phase-based end-effector parameterization. IEEE Robot. Autom. Lett. 3, 1560–1567. doi:10.1109/lra.2018.2798285

CrossRef Full Text | Google Scholar

8 Appendix

8.1 Cubic hermite spline

$\begin{array}{ccc}\hfill p_{HS}\left(t;T,{\mathbf{p}}_i,{\mathbf{p}}_g,{\mathbf{v}}_i,{\mathbf{v}}_g\right)& =\hfill & {\mathbf{p}}_i\left(2{(\frac{t}{T})}^3-3{(\frac{t}{T})}^2+1\right)+{\mathbf{v}}_{i}T\left({(\frac{t}{T})}^3-2{(\frac{t}{T})}^2+\frac{t}{T}\right)\hfill \\ \hfill & \hfill & +{\mathbf{p}}_g\left(-2{(\frac{t}{T})}^3+3{(\frac{t}{T})}^2\right)+{\mathbf{v}}_{g}T\left({(\frac{t}{T})}^3-{(\frac{t}{T})}^2\right),\hfill \\ \hfill v_{HS}\left(t;T,{\mathbf{p}}_i,{\mathbf{p}}_g,{\mathbf{v}}_i,{\mathbf{v}}_g\right)& =\hfill & \frac{{\mathbf{p}}_i}{T}\left(6{(\frac{t}{T})}^2-6\frac{t}{T}\right)+{\mathbf{v}}_i\left(3{(\frac{t}{T})}^2-4\frac{t}{T}+1\right)+\frac{{\mathbf{p}}_g}{T}\left(-6{(\frac{t}{T})}^2+6\frac{t}{T}\right)+{\mathbf{v}}_g\left(3{(\frac{t}{T})}^2-2\frac{t}{T}\right),\hfill \\ \hfill a_{HS}\left(t;T,{\mathbf{p}}_i,{\mathbf{p}}_g,{\mathbf{v}}_i,{\mathbf{v}}_g\right)& =\hfill & \frac{{\mathbf{p}}_i}{T^2}\left(12\frac{t}{T}-6\right)+\frac{{\mathbf{v}}_i}{T}\left(6\frac{t}{T}-4\right)+\frac{{\mathbf{p}}_g}{T^2}\left(-12\frac{t}{T}+6\right)+\frac{{\mathbf{v}}_g}{T}\left(6\frac{t}{T}-2\right).\hfill \end{array}$

8.2 Weight matrix design for friction cone

Given the centroidal dynamics, the problem to find contact force that satisfies all the constraints can be formulated as follows:

$\begin{array}{ccc}\hfill \mathbf{F}\mathbf{i}\mathbf{n}\mathbf{d}{\mathbf{f}}_c\text{s.t.\hspace{0.17em}}& \hfill & \mathbf{A}{\mathbf{f}}_c=\mathbf{b},\hfill \\ \hfill & \hfill & \mathbf{D}{\mathbf{f}}_c\ge 0\text{where}\mathbf{D}=\mathrm{diag}\left({\mathbf{U}}_f\left({\mu }^{c_1}\right),\cdots \right),\hfill \\ \hfill & \hfill & {\mathbf{U}}_f\left(\mu \right)=\left(\begin{array}{ccccc}\hfill 0\hfill & \hfill \hfill & \hfill 0\hfill & \hfill \hfill & \hfill 1\hfill \\ \hfill 1\hfill & \hfill \hfill & \hfill 0\hfill & \hfill \hfill & \hfill \tilde{\mu }\hfill \\ \hfill -1\hfill & \hfill \hfill & \hfill 0\hfill & \hfill \hfill & \hfill \tilde{\mu }\hfill \\ \hfill 0\hfill & \hfill \hfill & \hfill 1\hfill & \hfill \hfill & \hfill \tilde{\mu }\hfill \\ \hfill 0\hfill & \hfill \hfill & \hfill -1\hfill & \hfill \hfill & \hfill \tilde{\mu }\hfill \end{array}\right)\text{\hspace{0.17em}with\hspace{0.17em}}\tilde{\mu }=\frac{\mu }{\sqrt{2}}.\hfill \end{array}(26)$

Assuming the contact force ${\mathbf{f}}_c={(f_x^{c_1},f_y^{c_1},f_z^{c_1},\cdots )}^{\top }$ lying in the friction cone, we would want to find f_c that is located as close to the center of the cone as possible as shown in Figure 9. In this manner, we can formulate the problem:

$\begin{array}{ccc}\hfill & \min \hfill & \sum _{c\in \mathcal{C}}\frac{1}{2{\left({\mu }^c\right)}^2}\left({\left(f_x^c-{\tilde{\mu }}^c{f}_z^c\right)}^2+{\left(f_x^c+{\tilde{\mu }}^c{f}_z^c\right)}^2+{\left(f_y^c-{\tilde{\mu }}^c{f}_z^c\right)}^2+{\left(f_y^c+{\tilde{\mu }}^c{f}_z^c\right)}^2\right)\hfill \\ \hfill & \hfill & =\sum _{c\in \mathcal{C}}\left(\frac{{\left(f_x^c\right)}^2+{\left(f_y^c\right)}^2+2{\left({\tilde{\mu }}^c{f}_z^c\right)}^2}{{\left({\mu }^c\right)}^2}\right)\triangleq {\mathbf{f}}_c\mathbf{W}{\mathbf{f}}_c^{\top },\text{where\hspace{0.17em}}\mathbf{W}=\mathrm{diag}\left(\frac{1}{{\left({\mu }^{c_1}\right)}^2},\frac{1}{{\left({\mu }^{c_1}\right)}^2},1,\cdots \right),\hfill \\ \hfill & \text{s.t.\hspace{0.17em}}\hfill & \mathbf{A}{\mathbf{f}}_c=\mathbf{b}.\hfill \end{array}$

Note that we used weighted sum to prioritize the slippery contact. Then solving constrained minimization problem, we get ${\mathbf{f}}_c={\mathbf{W}}^{-1}{\mathbf{A}}^{\top }{({\mathbf{A}\mathbf{W}}^{-1}{\mathbf{A}}^{\top })}^{-1}\mathbf{b}$.

8.3 Proof of proposition 1

Given the bounded function f_min ≤ f(t) ≤ f_max, ∀t ∈ (0, T) and ∃s ∈ (0, 1) s.t. f(t) = sf_min + (1 − s)f_max, ∀t ∈ (0, T). Then s, (1 − s) ≥ 0 and

$\begin{array}{cc}\hfill \left(f\left(t\right){\mathbf{B}}_1+{\mathbf{B}}_2\right)\mathbf{x}+\mathbf{c}& =\left(\left(s{f}_{\text{min}}+\left(1-s\right)f_{\text{max}}\right){\mathbf{B}}_1+{\mathbf{B}}_2\right)\mathbf{x}+\mathbf{c},\hfill \\ \hfill & =s\left(\left(f_{\text{min}}{\mathbf{B}}_1+{\mathbf{B}}_2\right)\mathbf{x}+\mathbf{c}\right)+\left(1-s\right)\left(\left(f_{\text{max}}{\mathbf{B}}_1+{\mathbf{B}}_2\right)\mathbf{x}+\mathbf{c}\right),\hfill \\ \hfill & \ge 0.\hfill \end{array}$