Transition Motion Planning for Multi-Limbed Vertical Climbing Robots Using Complementarity Constraints

Generally, legged robots with $n$ joints will be treated as an underactuated system with a total of $n+6$ degrees of freedom (DOF) since the floating base, which is often the body of a robot, cannot be controlled directly. The additional DOF will be determined by wrenches (force/torque) applied on the robot including contact wrenches and gravity. According to the centroidal dynamics [17], we can represent them as linear and angular momentum at the center of mass (COM). Climbing on the wall only with the friction generated by regular end-effectors, like the point-contact toe, requires large motor torques which is prone to causing over-torque. The additional acceleration (linear/angular) of the body will burden motors significantly. For safety consideration, quasi-static movements where the derivative of linear and angular momentum can be negligible are presumed for our applications. With this assumption, the robot is subject to the static equilibrium constraint for every round $j$ where $j=1,\ldots,M$ and $M$ is the total rounds to be planned.

	$\displaystyle\sum^{N}_{i=1}\boldsymbol{f}_{i,j}+m\boldsymbol{g}=\boldsymbol{0}$		(1a)
	$\displaystyle\sum^{N}_{i=1}(\boldsymbol{p}_{i,j}-\boldsymbol{c}_{j})\times\boldsymbol{f}_{i,j}+\boldsymbol{\lambda}_{i,j}=\boldsymbol{0}$		(1b)

where $m$ is the total mass of the robot, $\boldsymbol{c}_{j}\in\mathbb{R}^{3}$ is the COM position, $\boldsymbol{p}_{i,j}\in\mathbb{R}^{3}$ and $(\boldsymbol{f}_{i,j},\boldsymbol{\lambda}_{i,j})\in\mathbb{R}^{6}$ denote the position and contact wrench for the $i^{th}$ leg ($N$ is the total number of limbs), with respect to the world frame. The contact wrench will be kept at zero if the contact is inactive.

Given a legged robot, the design will put a variety of constraints on the motion of the robot geometrically. For every round $j$, given the COM position $\boldsymbol{c}_{j}\in\mathbb{R}^{3}$ and the body orientation $\boldsymbol{\Theta}_{j}\in\mathbb{R}^{3}$, we can specify the limb reachability as follows:

$$\boldsymbol{p}_{i,j}\in\mathcal{R}(\boldsymbol{c}_{j},\boldsymbol{\Theta}_{j})$$

(2)

where the set $\mathcal{R}(\boldsymbol{c}_{j},\boldsymbol{\Theta}_{j})$ can be expressed explicitly with either analytical expressions by introducing additional decision variables joint angles $\boldsymbol{\theta}_{i,j}$ for each leg $i$, or approximated ranges. With full kinematics model, we can express the constraint (2) exactly as follows:

	$\displaystyle\boldsymbol{p}_{i,j}=\phi_{FK}(\boldsymbol{\theta}_{i,j})$		(3a)
	$\displaystyle\boldsymbol{\theta}_{min}\leq\boldsymbol{\theta}_{i,j}\leq\boldsymbol{\theta}_{max}$		(3b)

where $\phi_{FK}(\cdot)$ and $\boldsymbol{J}(\boldsymbol{\theta}_{i,j})$ denote the forward kinematics and Jacobian matrix for each leg, $\boldsymbol{\theta}_{min}$ and $\boldsymbol{\theta}_{max}$ describe the physical joint limitations. Although full kinematics model enables rich constraints including the position/orientation of every end-effector and explicit joint limitation, it will increase the computational expense with additional decision variables and high nonlinearility introduced by forward kinematics and Jacobian matrix. With proper approximated ranges, we can still capture the physical meanings behind those constraints although it will be more conservative. For the reachability (2), we can approximate the leg reachable workspace as a sphere around the nominal posture which can be expressed as:

$$\left\lVert\boldsymbol{p}_{i,j}-\boldsymbol{c}_{j}-\boldsymbol{R}(\boldsymbol{\Theta}_{j})\boldsymbol{v}\right\rVert_{2}\leq\Delta_{FK}$$

(4)

where $\boldsymbol{v}$ denotes the offset from COM to the housing location of legs on the body, $\boldsymbol{R}(\boldsymbol{\Theta}_{j})$ is the body rotation matrix represented by Euler angles, and $\Delta_{FK}$ is the radius of the approximated sephere. Furthermore, the step size of both the body and limbs is considered to ensure the reachability between rounds as follows:

	$\displaystyle|\Delta\boldsymbol{c}|\leq\Delta\boldsymbol{c}_{max}$		(5a)
	$\displaystyle|\Delta\boldsymbol{\Theta}|\leq\Delta\boldsymbol{\Theta}_{max}$		(5b)
	$\displaystyle|\Delta\boldsymbol{p}_{i}|\leq\Delta\boldsymbol{p}_{max}$		(5c)

where $\Delta\boldsymbol{c}_{max}$, $\Delta\boldsymbol{\Theta}_{max}$ and $\Delta\boldsymbol{p}_{max}$ denote maximum stepsizes for the body’s linear translation, rotation and maximum limb stride length respectively.

Since wall climbing is a high-risk task, being conservative on the choice of contact wrenches is necessary for safety of both the robot and the operator. Two safety factors are defined as follows in order to deal with the friction coefficient $\mu$ which is hard to be measured precisely and the motor torque limit $\tau_{max}$ which can degenerate by heating. More details can be seen in our previous work [11].

	$\displaystyle S_{\mu}$	$\displaystyle=\mu/\mu_{c}$		(6)
	$\displaystyle S_{\tau}$	$\displaystyle=\tau_{max}/\tau_{c}$		(7)

where $\mu_{c}$ and $\tau_{c}$ denote the critical values for safety. For our algorithm, critical values are not determined experimentally. Instead, we accept $S_{\mu}$ and $S_{\tau}$ as two main tuning parameters and being conservative can be achieved via keeping $S_{\mu}\geq 1$ and $S_{\tau}\geq 1$. Similarly, the constraint on contact wrench can include two parts, one from the motor torque limits and another from the friction cone.

$$\boldsymbol{f}_{i,j}\in\mathcal{F}_{\tau}(\tau_{max},\boldsymbol{J}(\boldsymbol{\theta}_{i,j}),S_{\tau})\cap\mathcal{F}_{\mu}(\mu,S_{\mu})$$

(8)

where the contact torque is not included for consideration since most legged robots are assumed point contact generally (multiple point contacts at corners of the sole are used for humanoids). If the torque can be generated by the end-effector and is controllable, it is easy to modify the contraint (8). Actually, the contact torque is very important for stability of a few contact sequences which we will see in Section III. The friction cone can be expressed as follows:

$$\mathcal{F}_{\mu}(\mu,S_{\mu})=\left\{\boldsymbol{f}_{i,j}\mid\left\lVert(\boldsymbol{f}^{c}_{i,j})_{x,y}\right\lVert^{2}_{2}\leq\mu(\boldsymbol{f}^{c}_{i,j})_{z}/S_{\mu}\right\}$$

(9)

where $\boldsymbol{f}^{c}_{i,j}$ denotes the contact force for the $i^{th}$ leg in the $j^{th}$ round with respect to the contact frame as shown in Fig. 2. $(\cdot)_{x,y}$ extracts the $x$ and $y$ components while $(\cdot)_{z}$ is the $z$ component of the vector. In terms of the set $\mathcal{F}_{\tau}(\tau_{max},\boldsymbol{J}(\boldsymbol{\theta}_{i,j}),S_{\tau})$, two explicit options are provided similar to Section II-B. Based on full kinematics model, we can express it as:

	$\displaystyle\boldsymbol{\tau}_{i,j}=\boldsymbol{J}(\boldsymbol{\theta}_{i,j})^{\top}\boldsymbol{f}_{i,j}$		(10a)
	$\displaystyle\boldsymbol{\tau}_{i,j}\leq\boldsymbol{\tau}_{max}/S_{\tau}$		(10b)

where $\boldsymbol{\tau}_{i,j}$ denotes required torques of motors on the $i^{th}$ leg. We can also assume all actuators have the same torque limit $\tau_{max}$ which is a scalar value so that we require the maximum of joint torques not to exceed the relaxed limit during operation,

$$\left\lVert\boldsymbol{\tau}_{i,j}\right\rVert_{\infty}\leq\tau_{max}/S_{\tau}$$

(11)

With the inequality $\left\lVert\boldsymbol{\tau}_{i,j}\right\rVert_{\infty}\leq\left\lVert\boldsymbol{J}(\boldsymbol{\theta}_{i,j})^{\top}\right\rVert_{\infty}\left\lVert\boldsymbol{f}_{i,j}\right\rVert_{\infty}$ from Eq. (10a), we can set the right-hand side of Eq. (11) as a conservative upper bound for $\boldsymbol{J}(\boldsymbol{\theta}_{i,j})^{\top}_{\infty}\left\lVert\boldsymbol{f}_{i,j}\right\rVert_{\infty}$, and the following relation can be obtained:

$$\left\lVert\boldsymbol{f}_{i,j}\right\rVert_{\infty}\leq\frac{\tau_{max}}{S_{\tau}\underset{\boldsymbol{\theta}_{i,j}}{\textit{max}}\left\lVert\boldsymbol{J}(\boldsymbol{\theta}_{i,j})^{\top}\right\rVert_{\infty}}$$

(12)

and for a robot, we can treat the value of $\underset{}{\textit{max}}\left\lVert\boldsymbol{J}(\boldsymbol{\theta}_{i,j})^{\top}\right\rVert_{\infty}$ as a constant by searching all possible $\boldsymbol{\theta}_{i,j}$ offline.

As discussed previously in Section I, we apply the same idea of [14] exploiting the complementarity condition between the contact distance and the contact force to our motion planning algorithm. As shown in Fig. 2, the contact force can be generated only when the contact distance is zero. Therefore, either the contact distance $(\boldsymbol{p}^{c}_{i,j})_{z}$ or the normal contact force $(\boldsymbol{f}^{c}_{i,j})_{z}$ will be zero which can be express as:

	$\displaystyle(\boldsymbol{f}^{c}_{i,j})_{z}\geq 0,\quad(\boldsymbol{p}^{c}_{i,j})_{z}\geq 0$		(13a)
	$\displaystyle(\boldsymbol{f}^{c}_{i,j})_{z}(\boldsymbol{p}^{c}_{i,j})_{z}=0$		(13b)

It is noted that the complementarity condition between the contact distance and the contact torque which can be expressed as $\lVert{\lambda}^{c}_{i,j}\rVert^{2}(\boldsymbol{p}^{c}_{i,j})_{z}=0$ is ignored here for the point contact assumption. In this way, we can use only continuous variables in the optimization problem and search over all possible contact sequences at once.

Furthermore, we also provide an optional complentarity constraint for limb switchability. If we use the same leg for supporting the weight during the transition phase for too many rounds which means a long period, it is likely for motors of that leg to over torque due to heating. In order to reduce this risk, we can force the motion planner to switch supporting legs between rounds, expressed as follows:

$$\text{for $j=2,\dots,M$},\quad(\boldsymbol{f}^{c}_{i,j-1})_{z}(\boldsymbol{f}^{c}_{i,j})_{z}=0$$

(14)

If one leg is used to generate nonzero contact force in the prior round, the complentraity condition will force the normal contact force of the same leg for the current round to be zero which means this leg will not be one of supporting legs. The observation here is that it will also sacrifice some richness in the possible transition motions. Without the constraint (14), we can generate more feasible contact sequences which can be seen from our results in Section III.

With all constraints described previously, we can choose the decision variables for the $j^{th}$ round as:

$$\mathit{\Gamma}=\left\{\boldsymbol{p}_{i,j},\boldsymbol{f}_{i,j},\boldsymbol{c}_{j},\boldsymbol{\Theta}_{j}\right\}$$

(15)

If the explicit form of constraints on reachability and the motor torque limits is chosen as Eq. (3) and Eq. (10), additional decision variable $\boldsymbol{\theta}_{i,j}$ will be required. Similarly, $\boldsymbol{\lambda}_{i,j}$ can be introduced if consider contact torques explicitly.

In terms of the cost function, a terminal cost and intermediate costs are included. To prepare the robot ready for wall-climbing during transition motion planning, we command the robot to reach the initial posture of the following climbing trajectory which is our terminal condition. Intermediate costs are composed of stepsize penalty and the $L_{1}$ norm of contact force. Unlike the $L_{2}$ norm of contact force which would encourage evenly distributed forces, the $L_{1}$ norm would favor sparse solutions which is what we expect from complementarity constraints. The full NLP formulation can be expressed as:

$\begin{aligned} &\begin{aligned} \underset{\Gamma}{\textbf{minimize}}\quad&\sum^{N}_{i=1}|\boldsymbol{p}_{i,M}-\boldsymbol{p}_{i,d}|^{2}_{Q_{p}}+\sum^{M}_{j=2}(|\Delta\boldsymbol{c}_{j}|^{2}_{Q_{c}}\\ &+|\Delta\boldsymbol{\Theta}_{j}|^{2}_{Q_{\Theta}}+\sum^{N}_{i=1}|\Delta\boldsymbol{p}_{i,j}|^{2}_{Q_{\Delta}}+|\boldsymbol{f}_{i,j}|^{2}_{L_{1}})\end{aligned}\\ &\text{subject to \eqref{const:static}, \eqref{const:reachability}, \eqref{const:stepsiz}, \eqref{const:wrench}, \eqref{const:comple}, \eqref{const:switchability} (optional)}\\ \\ \end{aligned}$

where $|\cdot|^{2}_{Q}$ is the abbreviation for the quadratic cost with the weight matrix as $Q\geq 0$, $\boldsymbol{p}_{i,M}$ and $\boldsymbol{p}_{i,d}$ denote the position at the last round $M$ and desired position for the $i^{th}$ leg respectively. Note that we could also add the contact position assignment as a constraint represented by $\boldsymbol{A}\boldsymbol{p}_{i,j}\leq\boldsymbol{b}$ where $\boldsymbol{A}$ and $\boldsymbol{b}$ describe the convex hull of feasible contact regions.

After solving our transition motion planner with SNOPT, the robot is able to accomplish the transtion task from the ground to the wall round by round by commanding the optimized leg positions and contact forces. If the robot has force feedback on the end-effector of each leg, direct force control would work easily. Unfortunately, our robot SiLVIA is position controlled. For multi-limbed robots like SiLVIA, contact forces are statically indeterminate if more than 3 contacts are active. In [18], the virtual penetration into the wall $\boldsymbol{\delta_{wall}}$ and the body deformation $\boldsymbol{\delta_{com}}$ as shown in Fig. 2 are considered to determine contact forces indirectly. The optimized contact force $\boldsymbol{f}_{i,j}$ from our transition motion planner can be treated as the spring force using the Virtual Joint Method (VJM) [19]. In order to achieve the contact force at the $i^{th}$ leg for the $j^{th}$ round, we need to solve a feasibility problem as follows:

	find	$\displaystyle\boldsymbol{\delta_{wall}},\boldsymbol{\delta_{com}}$		(16)
		$\displaystyle\boldsymbol{f}_{i,j}=\boldsymbol{K}_{i,j}(\boldsymbol{\delta_{wall}}-\boldsymbol{\delta_{com}})$		(16a)
		$\displaystyle\boldsymbol{K}_{i,j}=(\boldsymbol{J}(\boldsymbol{\theta}_{i,j})\boldsymbol{k}^{-1}\boldsymbol{J}(\boldsymbol{\theta}_{i,j})^{\top})^{-1}$		(16b)

where $\boldsymbol{K}_{i,j}$ is the stiffness matrix of the $n$ DOF leg, $\boldsymbol{k}$ is the diagonal matrix with the virtual spring coefficients $[k_{1},k_{2},\ldots,k_{n}]$ of the position controller motors. Since $\boldsymbol{\theta}_{i,j}$ can be find after transition motion planner, only $\boldsymbol{\delta_{wall}}$ and $\boldsymbol{\delta_{com}}$ are unknown. Problem (16) can be solved efficiently. Therefore indirect force control can be achieved by adding $\boldsymbol{\delta_{wall}}$ to optimized leg positions $\boldsymbol{p}_{i,j}$ which is what we have done for the following experiments.

In this scenario, we let the robot stand between two parallel walls at a distance of 1230 mm. Initially, the robot stands on the ground with the body height as 210 mm. The desired configuration on the wall is the starting point of the climbing trajectory from our previous work [11] indicated in Fig. 3 as the translucent configuration.

In the hardware experiment, the walls are covered by rubber pads and the robot toes are covered by anti-slip tapes, which gives a frictional coefficient $\mu$ around 1. With the nominal values for safety factors ($S_{\tau}=1.8$, $S_{\mu}=1.1$), the tripod gait (3-3 contact sequence) which lifts the front and rear legs on one side, the middle leg on the other side firstly, and then lifts the remaining 3 legs, is generated and validated in the robot as seen in Fig. 1. In the following, the two safety factors are tuned to simulate different experimental setups although the real environment for hardware demonstrations is kept the same. For the planner, we make the wall surface more slippery by increasing the value of $S_{\mu}$. When $S_{\mu}\approx 1.46$, the resulting solution shows the robot would lift two front and two rear legs for the first round, and then lift two remaining middle legs to the wall (4-2 contact sequence in Fig. 3). We also increase the motor torque limit by decreasing $S_{\tau}$ to pretend that the robot has stronger motors. 2-4 contact sequence is generated when $S_{\tau}$ becomes around 0.85. Actually, this is an aggressive setting for our robot since the torque density needs to be $1/0.85$ times larger in order to execute the resulting solution successfully in the hardware. It is noted that with smaller $S_{\tau}$, both 3-3 contact sequence and 4-2 contact sequence are also feasible local minimas of the NLP planner. With a different initial guess, the planner would converge to a different local minima.

The interesting observation in the hardware experiment is that the robot tips over for those two contact sequences (2-4 and 4-2). In terms of the 4-2 contact sequence, falling down happens when lifting those 4 legs as indicated in Fig. 4. The reason is that when the robot only has 2 legs generating contact forces, they need to be perfectly aligned in order to avoid any torques applied on the body. The better strategy is to include the contact torque in the planner. However, SiLVIA only has 3 DOF each leg which means the contact torque on the end-effector cannot be controlled directly. We simply replace the point-contact toe with a bar to be robust passively and it performs the 4-2 contact sequence successfully. For the 2-4 contact sequence, the robot suffers from not only the same issue as noted above but also the over-torque problems for the two legs on the wall.

Furthermore, we ignore the limb switchability constraint (14) in order to explore more possible contact sequences. A new contact sequence (2-2-2 contact sequence) is generated by our planner. The robot would lift the left middle leg (LM) and the right middle leg (RM) initially, then lift the left front leg (LF) and the right rear leg (RR) with 4 legs for supporting (2 on the ground and 2 on the wall), and finally lift the remaining two legs to the wall. From Fig. 5, the complementarity constraint (13) is always satisfied. For example, the contact distance of the leg RM at the round 1 is 0 while the contact force is 0 for the round 2. Note that the leg RM is already on the wall after the round 2 (the desired height of the leg on the wall is 180 mm). For rounds 3, 4 and 5, the contact distance of the leg RM is 0 with respect to the contact frame which still keeps the complementarity condition. The hardware demonstration shows that the 2-2-2 contact sequence provides stronger stability of the transition motion since the robot always keeps at least 4 contact points.

This scenario focuses on investigating whether our planner can generate the contact sequence taking advantage of objects presented in order to overcome the transition phase. We put two bricks next to the left wall on the ground and intuitively the robot would be able to step on the brick for the intermediate round. To take advantage of the brick, we decrease the value of $\Delta_{FK}$ in the constraint (4) for the legs on the left side until the leg is unable to reach the desired positions on the wall in one step. The 3-3-3 contact sequence is generated which puts LF and LR on the brick and RM on the right wall, then moves RF, RR and LM on the wall, finally moves LF, LR and RM again to the desired positions on the wall, as shown in Fig. 6. However, in the hardware experiment, the robot tips over when it is trying to lift 3 legs based on the supporting from 2 legs on the brick and 1 leg on the wall. The two legs on the brick (LF and LR) slip causing the failure. The reason is that the friction coefficient between the brick and the toe is much smaller than the one between the toe and the wall. By realizing this, we correct the setting but the problem becomes infeasible. Then we add one more round to enrich the possible contact sequence that the planner can search over. From the bottom row of Fig. 6. We can see that for the second round, the robot would not lift 3 legs together as what the robot would do in the 3-3-3 contact sequence. Instead, it is divided into two rounds. The robot would lift the leg LM to create 4 contact points and then lift the right two legs (RF and RR) to avoid too large horizontal forces required for the two legs on the brick. The robot successfully overcomes the transition phase with steps by performing the 3-1-2-2 contact sequence.

Besides the scenarios described previously, we also simulate two more cases. The first one is that we replace the two bricks with an incline to investigate the capability of changing the body orientation. We use $L_{2}$ norm of contact force in the cost function to encourage forces to be distributed more evenly which is also one possible incentive to change the body orientation. We also reduce the weight $Q_{\Theta}$ of the body orientation stepsize. The second one is to investigate how to transit from the wall to the ground. Obviously, reversing the order of our previous contact sequence would work since the robot is under the static equilibrium for every round. However, when you want to climb out of a circular wall or a tube, the situation is totally different since the flat surface the robot can place the leg is around the tube externally. Both simulation results are visualized in Fig. 7.

Without losing generality, we also apply our transition motion planner for ground walking. With different initial guesses, our planner converges to different local minimas and generates the standard tripod gait (repeated 3-3 contact sequence actually) and repeated 2-4 gait. The hardware demonstration is shown in Fig. 8.

Generally, a big concern about NLP, especially with complementarity constraints, is the running time. Since only a small number of rounds $M$ (generally $\leq 5$) required for the transition motion, our MATLAB code using SNOPT as the solver usually generates motions described previously within 10 secs. Also, NLP is very sensitive to the initial guess. A warm start that ignores highly nonlinear complementarity constraints can always help find a better initial guess leading to less computational time. Currently, for the parallel wall task, the average running time varies from 1.054s to 3.083s. These numbers are reported by running on an Intel Core i7-6500U CPU. We believe the implementation with the C++ code can accelerate the planner and make it run online which is also one of our future works.