In this section, we demonstrate the feasibility of the proposed approach on an increasingly challenging task of hopping which includes switching of different mode of dynamics (flight and stance) and more complex discontinuous state transition arising from impact at touch-down. Additional difficulty in this task is to find the flight and stance time for successful task execution which is highly restricted by the underactuated nature of the intrinsic dynamics and the desired task specifications.
We consider the hopping robot model in (Hyon and Emura 2004) with an augmentation of variable compliance elements in the hip and the leg actuators. The objective of optimization is to find appropriate leg and hip stiffness to exploit the passive dynamics and also the required flight and stance time during one locomotion cycle in a periodic movement based on a time-based switching approximation. The obtained controller is then applied to achieve multiple hopping cycles of locomotion on event based switching dynamics. Robustness of the obtained optimal feedback controller will be evaluated by applying external disturbances during the multiple hopping cycles to demonstrate the feasibility of the optimized controller.
Dynamics model of a hopping robot
The hopping dynamics switch between the flight phase and stance phase (with \(i=1\): flight and \(i=2\): stance) at the switching instance \(T_j\) when either touch-down or lift-off conditions are met. The configuration vector of the system is defined as \(\mathbf{q}=[x_{ com },y_{ com },\theta ,\phi ,r]^T\) (see Fig. 9).
The flight dynamics (\(i=1\)) are given as
$$\begin{aligned} \left\{ \begin{array}{ll} M \ddot{x}_{ com } = 0, &{} M \ddot{y}_{ com } = - Mg \\ J_l\ddot{\theta } = -\tau _{ hip }, &{}J_b\ddot{\phi } = \tau _{ hip } \end{array}\right. \end{aligned}$$
(45)
with \(r=r_0\). The stance dynamics (\(i=2\)) are given as
$$\begin{aligned} \left\{ \begin{array}{l} (J_l+Mr^2) \ddot{\theta }+2 M r \dot{r}\dot{\theta }-Mrg\sin \theta = -\tau _{ hip }\\ J_b\ddot{\phi } = \tau _{ hip } \\ M \ddot{r} - M r \dot{\theta }^2 + Mg\cos \theta = \tau _{ leg } \end{array}\right. \end{aligned}$$
(46)
with \(x_{ com }=-r \sin \theta + x_{ foot }\) and \(y_{ com } = r \cos \theta + y_{ foot }\). M is the mass of the body, \(J_l\) is the leg inertia, \(J_b\) is the body inertia, g is the gravitational constant, \(r_0\) is the nominal length of the leg spring. \(\tau _{ hip }\) and \(\tau _{ leg }\) are the torque applied to the hip joint and the force applied to the leg by the VSAs as given in (48) and (49) below, respectively. We use the parameters \(M=11.0\) kg, \(J_b=2.5\) kgm\(^2\), \(J_l=0.25\) kgm\(^2\) and \(r_0=0.7\) m adopted from (Ahmadi and Buehler 1997).
In a general form, the flight and stance dynamics can be written as
$$\begin{aligned} \mathbf{M}_i\ddot{\mathbf{q}}_i + \mathbf{C}_i(\mathbf{q}_i,\dot{\mathbf{q}}_i)\dot{\mathbf{q}}_i + \mathbf{g}_i(\mathbf{q}_i) = \varvec{\tau }_i(\mathbf{q}_i,\mathbf{u}_i) \end{aligned}$$
(47)
where \(\mathbf{q}_1 = [x_{ com }, y_{ com }, \theta , \phi ]^T\) and \(\mathbf{q}_2 = [\theta , \phi , r]^T\) are the partial configuration vector for the flight and stance phase, respectively.
\(\varvec{\tau }_1(\mathbf{q},\mathbf{u}) = [0, 0, -\tau _{ hip }, \tau _{ hip }]^T\) and \(\varvec{\tau }_2(\mathbf{q},\mathbf{u}) = [-\tau _{ hip }, \tau _{ hip }, \tau _{ leg }]^T\) are the VSA torque/force applied to each degree of freedom where
$$\begin{aligned} \tau _{ hip }(\mathbf{q},\mathbf{u})= & {} u_1 - u_3(\phi -\theta ) \end{aligned}$$
(48)
$$\begin{aligned} \tau _{ leg }(\mathbf{q},\mathbf{u})= & {} u_2 - u_4(r - r_0) \end{aligned}$$
(49)
and \(\mathbf{u}=[u_1,u_2,u_3,u_4]^T\) is the control command vector defined, where \(u_1\) is the hip torque, \(u_2\) is the leg force, \(u_3\) is the hip joint stiffness and \(u_4\) is the leg stiffness. The range of the control and stiffness is limited as \(\mathbf{u}_{ min } \preceq \mathbf{u}\preceq \mathbf{u}_{ max }\) where \(\mathbf{u}_{ min } =[\;-100,\;-100,\;10,\;2000\;]^T\) and \(\mathbf{u}_{ max } =[\;100,\;100,\;150,\;25000\;]^T\).
For the purpose of optimization, the dynamics will be formulated in a state space representation of the form of \(\dot{\mathbf{x}}=\mathbf{f}_i(\mathbf{x}, \mathbf{u})\) as in (5) with the full state vector \(\mathbf{x}=[\;\mathbf{q}^T,\;\dot{\mathbf{q}}^T\;]^T\) and \(\mathbf{q}=[x_{ com },y_{ com },\theta ,\phi ,r]^T\). In this hopping robot, we consider a simplified parallel elastic VSA model with direct force/torque and stiffness control as in (Hyon and Emura 2004), which does not include the motor dynamicsFootnote 6 (2).
The discontinuous impact map at touch-down is defined as
$$\begin{aligned} \mathbf{x}^{+}=\varvec{\Delta }^{1,2}(\mathbf{x}^{-}) = \left[ \begin{array}{cc} \mathbf{I} &{} \mathbf{0}\\ \mathbf{0} &{} \varvec{\Lambda }(\mathbf{q})\\ \end{array} \right] \mathbf{x}^{-} \end{aligned}$$
(50)
where \(\mathbf{x}=[\mathbf{q}^T,\dot{\mathbf{q}}^T]^T\), \(\mathbf{q}^+=\mathbf{q}^-\) and \(\dot{\mathbf{q}}^+=\varvec{\Lambda }(\mathbf{q}) \dot{\mathbf{q}}^-\). The matrix \(\varvec{\Lambda }(\mathbf{q})\) is the transition map between the pre-impact to post-impact velocities based on a rigid body collision model (Grizzle et al. 2001; Rosa et al. 2012). The specific form of the velocity transition map is given in the study of passive running with an additional analysis of energy dissipation at impact (Hyon and Emura 2004). At lift-off, the velocity of the leg is reset to zero as \(\dot{r} =0\) at \(r=r_0\).
Design of composite cost function
In this paper, we consider a task of achieving periodic movement of continuous hopping which is a repetition of one hopping cycle while exploiting the passive dynamics and the benefits of stiffness modulation. For this purpose, first, we design a composite cost function for one hopping cycle including both the flight and stance phases and the desirable touch-down condition. Then, the obtained controller is applied to achieve multiple cycles.
Consider the following cost function:
$$\begin{aligned} J= & {} (\mathbf{x}(T_f)-\mathbf{x}_0)^T \mathbf{Q}_{T_f}(\mathbf{x}(T_f)-\mathbf{x}_0)+ \Psi \left( \mathbf{x}\left( T_1^-\right) \right) \nonumber \\&+\int _{0}^{T_f}\mathbf{u}^T\mathbf{R}\mathbf{u}\,dt \end{aligned}$$
(51)
where \(\mathbf{Q}_{T_f}\) is a positive semi-definite matrix and \(\mathbf{R}\) is a positive definite matrix. The purpose of the first term is to achieve periodicity of the trajectory where \(\mathbf{x}_0\) denotes the initial state. The second term consists of two criteria:
$$\begin{aligned} \Psi \left( \mathbf{x}\left( T_1^-\right) \right) = Q_{T_1,1} \left( y^{-}_{ foot } - y_{ ground }\right) ^2 + Q_{T_1,2} (\mu ^{-})^2 \end{aligned}$$
(52)
where \(Q_{T_1,1}\) and \(Q_{T_1,2}\) are positive weights. The first term \(Q_{T_1,1} (y^{-}_{ foot } - y_{ ground })^2\) penalizes the height of the foot from the ground \(y_{ ground }=0\) to approximately encode the touch-down condition in a time-based formulation. In order to minimize this term, it is important to find an appropriate flight time \(T_1\) as the trajectory of the center of mass cannot be directly controlled in the flight phase for a given initial lift-off condition. The second term \(Q_{T_2,2} (\mu ^{-})^2\) minimizes energy loss at touch-down where \(\mu ^-\) is called the energy dissipation coefficient
$$\begin{aligned} \mu ^{-}&= \dot{x}^{-}_{ com } \cos \theta ^{-} + \dot{y}^{-}_{ com } \sin \theta ^{-}+ r_0 \dot{\theta }^{-}\nonumber \\&=\dot{x}^{-}_{ foot } \cos \theta ^- + \dot{y}^{-}_{ foot } \sin \theta ^- \end{aligned}$$
(53)
motivated from the study of passive running by Hyon and Emura (2004) to find an appropriate leg angle and foot velocity relative to the ground at touch-down. Note that if \(\mu ^{-}=0\), there is no energy loss at impact. In the running cost, we minimize the control effort \(u_1\) and \(u_2\) while small weights are used for penalizing the magnitude of stiffness \(u_3\) and \(u_4\) for regularization.
The initial conditions are obtained by choosing the desired initial horizontal velocity and lift-off angle of the leg \((\dot{x}_0, \theta _0)\) and computing the remaining parameters with the assumption of synchronization between the oscillatory movements of the leg swing and compression in the passive running model (Ahmadi and Buehler 1997; Hyon and Emura 2004).
Simulation results
We choose the desired initial condition at lift off as \(\dot{x}_0=2.0\)m and \(\theta _0=-6.0\) deg (\(-0.105\) rad). The rest is obtained based on an approximated condition of passive running (Hyon and Emura 2004) for the nominal model of Ahmadi and Buehler (1997). With this initial condition, the passive dynamics (no control) of the robot can achieve several steps of running as reported by Ahmadi and Buehler (1997) and Hyon and Emura (2004). However, eventually, it will fail since passive running is intrinsically unstable.
Using the proposed method, we simultaneously obtained the optimal feedback control for the control commands \((u_1,u_2)\) and stiffness \((u_3, u_4)\), and found the flight time \(T_1\) and the period for one complete cycle \(T_f\) for one hopping cycle. The optimized flight time and one hopping cycle were \(T_1 = 0.410\) s and \(T_f = 0.487\) s. Since the obtained controller is based on an assumption of time-based switching, there could be some mismatch in the exact timing in the switching condition when applied to realistic event based switching dynamics (flight to stance at touch-down \(y_{ foot }= y_{ ground }\), stance to flight when \(r=r_0\)) to achieve multiple cycles of locomotion. One of the benefits of our approach is that it provides a locally optimal feedback control, deviations from the optimal trajectory can be corrected, which will be illustrated in the following examples. These simulation results are presented in the accompanied video.
Comparison to individual phase optimization As a comparison, we optimized the control command, stiffness and the movement duration in a sequential manner individually for the flight phase subsequently followed by the stance phase for one cycle of the movement. The optimized movement duration for the flight phase was \(T_{1, ind } = 0.410\) s and for the stance phase was \(T_{ stance,ind }=0.080\) s, i.e., the total duration was \(T_{ f,ind }=0.490\) s. The total cost for this individual optimization was \(J_{ ind }=1.686\) which is comparable to the complete optimization case \(J_{ comp }=1.624\) mentioned above. The optimized trajectories, control commands and stiffness profiles are similar between these two cases. However, interestingly, there are notable difference in the robustness of the controller when these two were applied to the event based switching dynamics where the role of the feedback control becomes prominent.
The controller with complete cycle optimization was able to achieve continuous stable running over multiple cycles. However, with the controller obtained by individual optimization, the robot failed to continue to run after 25 steps of hopping. Although this is an empirical observation, this difference presumably came from the difference in the optimal feedback gains. In the complete cycle optimization, the optimal feedback gains take the future goal until the end of the hopping cycle into account including both the flight and stance phases with the via-point and terminal costs. However, in the individual optimization, corrections are made only considering the immediate goal specified by the terminal cost in each phase. This result highlights the benefits of optimizing the whole cycle of the movement in comparison to individually optimizing the movement in a sequential manner. This comparison is demonstrated in the accompanied video.
Robustness against perturbations In this simulation, we evaluate the robustness of the obtained optimal feedback controller by applying external perturbations while the robot is running. At \(t=1.0\) s, the robot is pushed forward with \(F_x=150\) N and at \(t=2.0\) s, a backward perturbation is applied \(F_x=-250\) N for the duration of 0.05 s, respectively. Figure 10a depicts the movement of the robot from \(t=0.7\) to \(t=3.9\) s. Figure 10b show the forward velocity \(\dot{x}\) (top), body height \(y_{ com }\) (middle), and leg angle \(\theta \) and hip angle \(\phi \) (bottom) from \(t=0\) to \(t=6\) s. Figure 10c show the control commands \(u_1\) and \(u_2\) (top), hip stiffness \(u_3\) (center) and leg stiffness \(u_4\) (bottom). The simulation result illustrate that after the perturbations, the robot was able to stabilize the periodic running behavior without falling over demonstrating the robustness of the optimal feedback controller and the feasibility of the proposed approach in this problem setting. This result is illustrated in the accompanied video.