A fundamental ability of humanoid robots consists in moving over steps and stairs, where they can exploit the legged configuration. A possible approach for the generation of humanoid motions over stairs is to tackle the problem as an extension of the planar walking motion generation [1, 2, 3]. This paper presents methods and validations for extending the planar walking generation algorithms to large step-up behaviors of humanoid robots.
Assuming the angular momentum to be constant and restricting the robot center of mass (CoM) on a plane at fixed height, it is possible to derive simple and effective control laws based on the well known Linear Inverted Pendulum (LIP) model . By exploiting the linearity of the model, it is possible to determine analytical stability conditions, enabling for reactive motions in case of disturbances [5, 6]. However, the linearity of the model is based on the assumption of constant CoM height, resulting in a fixed pendulum constant . Instead, when considering as a tuning parameter, the model linearity can be preserved even in case of varying CoM height . If it is left unconstrained, it is possible to generate walking trajectories with a straight knee configuration . In other works, balance is maintained by varying only the CoM height , hence completely removing the assumption for it to be constant. The robustness properties of this strategy have been further studied through the use of Sums-of-Squares optimization . A similar model has been used also in a multi-contact scenario 
and extended considering zero-moment point (ZMP) motions to determine stability conditions [13, 14]. These approaches have been used to control a humanoid robot considering only single support phases. In addition, the CoM height variations still remain very limited.
In case of large step-ups, the significant and required variation of the CoM height is no more negligible and it cannot be considered as constant. In addition, it becomes important to perform such motion dynamically, exploiting the robot momentum to avoid excessive effort from the robot actuators. In literature, this problem is faced by using non-linear trajectory optimization techniques. Apart from the problems arising when using non-linear, and potentially non-convex, models, an additional hurdle is the determination of contact timings and locations. For example, kinematic and dynamic quantities can be computed in an iterative fashion, assuming the contact sequence to be known . Authors of  adopt a phase-based approach, where the number of contact phases are predetermined, but their duration and the contact location are a result of the optimization problem. Integer variables can be used to describe the activation/deactivation of contacts . Other contributions include complementarity conditions as constraints [18, 19], thus contact locations and timings are an output of the optimization problem. The time needed to find a solution ranges from seconds to several minutes, while results are shown only in simulation. On the other hand, methods that rely on iterative Linear Quadratic Regulators usually require small computational times and are successfully applied to robots with point contacts, like quadrupeds or a single leg hopping robot [20, 21].
The aim of this paper is to leverage trajectory optimization techniques to generate motions which allow a humanoid robot to perform large step-ups. In particular, we exploit a reduced model of the centroidal dynamics, where the forces on both feet are considered. The angular momentum is assumed constant. By adopting a phase-based trajectory optimization
technique, we plan over a fixed series of contacts. Through a particular heuristic, we are able to avoid generating trajectories which would require a high torque expenditure. Real world experiments are performed on the Atlas humanoid robot, showing a reduction of the maximal knee torque up to 20%.
This paper uses the following notation.
component of a vectoris denoted as .
The transpose operator is denoted by .
is a fixed inertial frame with respect to (w.r.t.) which the robot’s absolute pose is measured. Its axis is supposed to point against gravity, while the direction defines the forward direction.
Given a function of time the dot notation denotes the time derivative, i.e. . Higher order derivatives are denoted with a corresponding amount of dots.
denotes a zero matrix.
The weighted L2-norm of a vector is denoted by , where is a weight matrix.
and denote the rotation and transformation matrices which transform a vector expressed in the frame, , into a vector expressed in the frame, .
Ii-B QP-based whole-body controller
The robot motion is controlled by means of a QP-based whole-body controller, presented in . It determines a set of desired joint accelerations, including the spatial acceleration of the floating-base joint with respect to . We denote this quantity with where is the number of joints under control. In addition, the whole-body controller generates desired contact wrenches.
The controller handles different motion tasks, amongst which Cartesian and center of mass (CoM) tasks. The former are achieved by minimizing the following quantity:
where , is the task Jacobian, with
being the number of degrees of freedom of task. is the task desired acceleration, while are the measured
base and joint velocities (obtained through an internal estimator). The quantityis a vector which does not depend on the decision variables and it is referred with the symbol .
The CoM task is achieved by controlling the robot momentum . In particular, we exploit the following relation:
where is the Centroidal Momentum Matrix , is the wrench due to gravity. represents the summation of external wrenches applied to the robot, except those exerted on the robot body as a result of a contact, i.e. .
A different point force is considered applied at each vertex of a contact patch. In particular, these forces are defined through a coordinate vector , whose basis are extreme rays of the friction cone. By constraining each component of in the range , we avoid excessively high wrenches. More importantly, friction and Center of Pressure  (CoP) constraints are implicitly satisfied . Trivially, also the resulting normal force is bounded to be positive. It is possible to express the corresponding wrench in the inertial frame , , as a function of :
where depends on the position of contact vertex and on the basis vectors. By concatenating all the and , we obtain .
The Cartesian and CoM task are grouped as follows:
with and being the concatenation of all the tasks Jacobians and respectively. is a desired momentum rate of change.
At every control cycle, the solved QP problem writes:
The desired joint accelerations and contact wrenches are used to compute a set of desired joint torques to be commanded to the robot.
Ii-C The variable height double pendulum
The linear momentum rate of change can be written according to Newton’s second law:
where is the center of mass position expressed in the inertial frame . is the total mass of the robot, is the gravity acceleration while is an external force expressed in . We can express the wrenches applied at the feet in a frame parallel to , having the origin coincident with the corresponding CoP. We assume that no moment is exerted along the axis perpendicular to the foot, passing through the CoP. Thanks to these two choices, the moments are null. It is possible to enforce the angular momentum to be constant by constraining these forces along a line passing through the CoM, i.e.:
The symbol is used as a placeholder for either the left or the right foot. is the force applied on the foot, measured in coordinates. is a multiplier. Since the contacts are considered unilateral, is constrained to be greater than zero for guaranteeing the positivity of normal forces. is the position of the foot CoP expressed in , in particular:
where is the foot position. is the CoP expressed in foot coordinates, thus having the coordinate equal to zero. For the sake of simplicity, from now on we will drop the subscript in front of the rotation matrix , which expresses the relative rotation between the inertial frame and a frame attached to the foot.
Assuming the forces exerted on the feet to be the only external forces, we can rewrite Eq. (4) as:
Dividing by the total mass of the robot, we can finally write:
Iii Dynamic Planning for Large Step-Ups
The step-up planner presented in this paper leverages the idea of phase-based trajectory optimization explored in [16, 26, 27]. We assume to know the contact sequence beforehand, simplifying the handling of their activation and deactivation. This section presents constraints, tasks and methodologies used to solve the corresponding non-linear trajectory optimization problem.
We start by simplifying Eq. (8) as follows:
assumes different value depending on the contact state of the corresponding phase :
one foot in contact ( is either or )
both feet in contact
We can obtain an approximated discrete form of Eq. (9) through a second order Taylor expansion:
where is used to indicate a generic discrete instant, while is the CoM velocity at instant . The duration of each phase is not defined a-priori, but it is considered as an optimization variable. Nevertheless, each phase consists of a fixed number of instants, thus , where is the number of instants per phase. In addition, each is bounded in .
Iii-a Force and leg constraints
The forces applied at the feet need to satisfy some conditions in order to be attainable by the robot. These are embedded as constraints in the optimization problem.
First of all, the CoP has to lie inside the foot polygon. This can be obtained by specifying a set of linear inequalities:
where the matrix and the vector can be computed according to the location of the foot vertices, whose number is . Since is defined in foot coordinates, these quantities do not depend on the foot pose.
The contact forces are supposed to lie within the friction cone in order to avoid slipping. This is imposed by
is the static friction coefficient and is the force exerted on a foot, expressed in foot coordinates, namely . We assume the foot frame to be parallel to a frame attached to the ground, with the axis perpendicular to the walking surface. Using Eq. (5) and (6), we can rewrite Eq. (13) as follows:
Notice that when , friction is automatically satisfied as every component goes to zero. Thus, we can simplify Eq. (14) as follows:
In order to consider the torsional friction, we impose the equivalent normal torque , applied at the origin of the foot frame, to be bounded, i.e. , with the torsional friction coefficient. Given that the external force is applied in , we can rewrite this constraint as follows:
Notice that corresponds to the component of the cross product . By substituting as for the static friction constraint, we write the following set of constraints:
When planning the robot motion, we have to take into consideration its kinematic limits. We can approximate the robot leg length with the distance between the CoM and the corresponding foot position, thus limiting excessively wide motions or movements which could cause a leg to collapse. This can be achieved with the following constraint:
with the lower and upper bound, respectively. These constraints can be applied on each leg separately.
The cost function is designed to generate a CoM trajectory for the robot to perform large step-ups exploiting its momentum. It is composed of several tasks. The full cost function is shown at the top of Optimization Problem III-C.
The first task weights the distance of the terminal states from the desired position and velocity :
where is a tunable gain. contains a certain portion of the last phase, including the terminal state. Thus, it is possible to generate trajectories which reach the end point in advance while defining control inputs able to maintain such position.
The model defined by Eq. (8) does not carry any information about the joint torques necessary to achieve the planned motion. Nevertheless, with the aim of reducing the torques required at the leading knee to step-up, we introduce the following task:
is used as a heuristic to reduce joint torques. is again a placeholder for and , while is a reference height difference. Intuitively, the robot knees undergo higher stress when they are highly bent. In this configuration, the height difference between the CoM and the foot gets small. This heuristic prevents the solver from requiring high forces (characterized by a high multiplier ) on a collapsed leg. Thus weights the summation over each time instant of this quantity squared. The second term is employed to prevent this quantity to have an impulsive behavior. and are weights allowing to specify the relative priority for each task.
Considering the control inputs and , it is preferable to generate trajectories which require small control variations. This reduces the need for large torque variations when tracking the trajectories on the real robot. To this end, we consider the following task:
where is the task weight and .
Finally, we consider a series of regularization terms:
This task allows selecting solutions which minimize the use of control inputs and the duration of each phase is close to a desired one . are the tasks weights.
Iii-C The optimization problem
The trajectory optimization problem constituted by the dynamic constraint defined in Eq. (10a), the constraints listed in Sec. III-A and the tasks of Sec. III-B, is casted into an optimization problem via a Direct Multiple Shooting method . The optimization variables correspond to the following set:
with equal to the number of phases. and are state variables and need to be initialized with the measurements coming from the robot , , i.e. and .
The complete formulation is shown in Optimization Problem III-C. Notice that the subscript refers to quantities which depend on a specific phase and they are fixed for its entire duration. It is implemented using CasADi , adopting Ipopt  as solver.
The optimization problem is non-convex due to the non-linearities of the model equation, of the friction constraints and the chosen tasks. In order to facilitate the finding of a solution, we initialize the phases duration
with their desired value. The CoM position trajectory is initialized to a linear interpolation fromto . All other variables are initialized to zero.
The trajectories generated with the method described in Sec. III are stabilized by the controller presented in Sec. II-B. The desired accelerations and contact wrenches are turned into a set of desired joint torques commanded to the Atlas humanoid robot.
The task consists in stepping onto a platform whose height is about 31, as showed in Figure 1. We noticed that this height is already enough to have the robot pushing the leg joint limits during the swing phase. The robot starts at a distance of about 30 from the platform and performs a 55 step forward on the platform. Once the robot is close to the platform, the optimization is started using the current robot state as initialization. The optimization problem is solved using a desktop computer equipped with a 5 generation Intel® Core firstname.lastname@example.orgGHz processor and 64GB RAM memory, running Ubuntu 16.04. Ipopt is set up to use the mumps  solver. A solution is generated after about 1.5. This time can be reduced up to a factor 3 by using the linear solver ma27  instead of mumps. Nevertheless, considering that the trajectories are generated only once before the beginning of the motion, this improvement is not necessary at this stage.
The length of each phase is set to instants. We noticed that this value is a good trade-off between the computational time and the smoothness of the generated trajectories. The maximum duration of a phase is equal to , thus corresponding to a maximum .
Amongst the set of variables constituting the solution provided by the solver, we pack the CoM state into a desired trajectory. In addition, the timings are used to determine the single and double support phase durations.
Iv-a Real robot experiments
Fig. 6 contains snapshots of a step-up executed by the Atlas humanoid robot when using the planner. Fig. (a)a and (b)b show the corresponding CoM position tracking along the and direction. Figure (c)c presents the tracking of the CoM height. Note that the CoM is lowered right before performing the step-up. This instant is shown in Fig. (b)b. We believe such desired motion arises to achieve sufficiently high vertical velocity, see Fig. 11, while considering the limits on the leg length. By performing this motion, the robot gains momentum to perform the step-up motion requiring a lower torque on the leading knee.
In order to provide a comparison, we performed the same task without prescribing any CoM trajectory, thus adopting the motion generation techniques presented in . To facilitate the comparison, we impose the same phases duration. Fig. 12 shows the torque profile measured on the left knee when performing the step-up. We focus on this joint since is the one witnessing the highest torque expenditure during the step-up motion. In particular, using the controller presented in , the maximum value of this torque reaches , while it is reduced to when using the method presented in this paper. This corresponds to a nearly maximal torque reduction. Such result comes at the cost of a higher torque on the trailing knee. In order to facilitate the step-up motion, the right leg is much more exploited compared to the baseline, with a knee torque of nearly .
Since the trajectories are computed offline, small disturbances can induce the robot to a fall. Robustness is achieved by limiting the CoP to a smaller region compared to the real robot foot, in the planning phase. In this way there is margin in the underlying QP controller for counteracting disturbances while executing those trajectories. During the experiment described above, the foot dimension is set to of the original size.
The planned step-up is a dynamic motion which requires a high vertical velocity of the CoM. We noticed that the static friction coefficient has a particular effect in determining how “dynamic” is the planned motion. Intuitively, it limits the angle between the normal to the ground and the pendulum. A small angle corresponds to a conservative motion where the projection of the CoM on the ground lies well inside the support polygon. In the experiment shown above, the static friction coefficient is set to 0.7. By reducing it to the motion is more conservative and robust, even though the effectiveness of the method is reduced to a maximum torque reduction of 10%. As shown in Fig. 13, the knee torque reaches a peak of .
When performing experiments on the real robot, the poor tracking of the desired trajectories limits the robustness and reproducibility of the results. Since the trajectories are computed offline, a poor tracking limits their efficacy. In the majority of the experiments, the reduction of the maximum torque consisted of about . Anyhow, we believe this still represents an interesting achievement. It shows that trajectory optimization techniques have an effect in reducing the maximum effort required to a robot when performing a motion at the limits of its reachable workspace.
Iv-B Simulation results
When performing the same test in simulation, the tracking on the planar directions is much better, as shown in Fig. (a)a and (b)b, while along the direction, it is somehow comparable, Fig. (c)c. The improved tracking helps in maintaining balance, allowing more dynamic motions. The figures shown are generated with a static friction coefficient equal to 1.0, while the timings are kept equal to the real robot experiments. The baseline is obtained in the same way, leaving to the controller the generation of desired trajectories given a defined sequence of footsteps. As it can be noticed from Fig. 18, when the robot switches from double to single support the knee torque has a spike of
. Given the coincidence with the change of support configuration, the source of such spike is most probably the controller itself. Since in simulation the torque control is almost perfect, this spike is reflected also in the measured data. Conversely, on the real robot, any spike on the desired torques is smoothed due to the actuator dynamics. Nevertheless, when using the planner there is no spike and the maximum torque is at, corresponding to a maximum torque reduction.
Table I provides the weights adopted in the cost function tasks of Sec. III-B. They are the same in simulation and in real robot experiments. The tuning process consists in adding one task at a time starting from the most important, i.e. , which is assigned an arbitrary weight. It is possible to notice that the three highest weights are those related to the terminal state tracking, the maximum and the control variations.
This paper presents a method to generate trajectories for large step-up motions with humanoid robots. Its effectiveness has been tested on the IHMC Atlas humanoid robot. Experiments carried on the real platform showed that such trajectories can reduce the maximum torque required to the knee up to .
The method has been studied for large step-ups, but its applicability is not limited to this domain. Indeed, its formulation enables the planning of motions involving flight phases, hence requiring dynamic movements. This represents a fascinating future work.
The desired trajectories are computed offline right before starting the step-up motion. This reduces the robustness of the planned trajectories, strongly relying on the tracking performances of the low-level controller. Poor tracking of the planned trajectories limits also their efficacy in reducing the maximum knee torque. A possible future work consists in re-planning the trajectories every time a walking phase is completed. However, the computational time would need to be reduced by at least an order of magnitude.
-  K. Hirai, M. Hirose, Y. Haikawa, and T. Takenaka, “The development of honda humanoid robot,” in Proceedings. 1998 IEEE International Conference on Robotics and Automation (Cat. No. 98CH36146), vol. 2. IEEE, 1998, pp. 1321–1326.
-  Y. Hu, J. Eljaik, K. Stein, F. Nori, and K. Mombaur, “Walking of the icub humanoid robot in different scenarios: implementation and performance analysis,” in Humanoid Robots (Humanoids), 2016 IEEE-RAS 16th International Conference on, 2016, pp. 690–696.
-  S. Caron, A. Kheddar, and O. Tempier, “Stair climbing stabilization of the hrp-4 humanoid robot using whole-body admittance control,” in 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019, pp. 277–283.
-  S. Kajita, F. Kanehiro, K. Kaneko, K. Yokoi, and H. Hirukawa, “The 3d linear inverted pendulum mode: A simple modeling for a biped walking pattern generation,” in Proceedings 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems. Expanding the Societal Role of Robotics in the the Next Millennium (Cat. No. 01CH37180), vol. 1. IEEE, 2001, pp. 239–246.
-  J. Pratt, J. Carff, S. Drakunov, and A. Goswami, “Capture point: A step toward humanoid push recovery,” in Humanoid Robots, 2006 6th IEEE-RAS International Conference on, Dec 2006, pp. 200–207.
-  R. J. Griffin, G. Wiedebach, S. Bertrand, A. Leonessa, and J. Pratt, “Walking stabilization using step timing and location adjustment on the humanoid robot, atlas,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017, pp. 667–673.
-  J. Englsberger, C. Ott, and A. Albu-Schäffer, “Three-dimensional bipedal walking control using divergent component of motion,” in 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2013, pp. 2600–2607.
-  R. J. Griffin, G. Wiedebach, S. Bertrand, A. Leonessa, and J. Pratt, “Straight-leg walking through underconstrained whole-body control,” in 2018 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2018, pp. 1–5.
-  T. Koolen, M. Posa, and R. Tedrake, “Balance control using center of mass height variation: limitations imposed by unilateral contact,” in 2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids). IEEE, 2016, pp. 8–15.
-  M. Posa, T. Koolen, and R. Tedrake, “Balancing and step recovery capturability via sums-of-squares optimization.” in Robotics: Science and Systems. Cambridge, MA, 2017, pp. 12–16.
-  N. Perrin, D. Lau, and V. Padois, “Effective generation of dynamically balanced locomotion with multiple non-coplanar contacts,” in Robotics Research. Springer, 2018, pp. 201–216.
-  M. Vukobratović and B. Borovac, “Zero-moment point: thirty five years of its life,” International Journal of Humanoid Robotics, vol. 1, no. 01, pp. 157–173, 2004.
-  S. Caron and A. Kheddar, “Dynamic walking over rough terrains by nonlinear predictive control of the floating-base inverted pendulum,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017, pp. 5017–5024.
-  S. Caron and B. Mallein, “Balance control using both zmp and com height variations: A convex boundedness approach,” in 2018 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2018, pp. 1779–1784.
-  A. Herzog, N. Rotella, S. Schaal, and L. Righetti, “Trajectory generation for multi-contact momentum control,” in Humanoid Robots (Humanoids), 2015 IEEE-RAS 15th International Conference on. IEEE, 2015, pp. 874–880.
-  A. W. Winkler, D. C. Bellicoso, M. Hutter, and J. Buchli, “Gait and trajectory optimization for legged systems through phase-based end-effector parameterization,” IEEE Robotics and Automation Letters (RA-L), vol. 3, pp. 1560–1567, July 2018.
-  B. Aceituno-Cabezas, C. Mastalli, H. Dai, M. Focchi, A. Radulescu, D. G. Caldwell, J. Cappelletto, J. C. Grieco, G. Fernández-López, and C. Semini, “Simultaneous contact, gait, and motion planning for robust multilegged locomotion via mixed-integer convex optimization,” IEEE Robotics and Automation Letters, vol. 3, no. 3, pp. 2531–2538, 2018.
-  H. Dai, A. Valenzuela, and R. Tedrake, “Whole-body motion planning with centroidal dynamics and full kinematics,” in 2014 IEEE-RAS International Conference on Humanoid Robots, 2014, pp. 295–302.
-  M. Posa, C. Cantu, and R. Tedrake, “A direct method for trajectory optimization of rigid bodies through contact,” The International Journal of Robotics Research, vol. 33, no. 1, pp. 69–81, 2014.
-  M. Neunert, M. Stäuble, M. Giftthaler, C. D. Bellicoso, J. Carius, C. Gehring, M. Hutter, and J. Buchli, “Whole-body nonlinear model predictive control through contacts for quadrupeds,” IEEE Robotics and Automation Letters, vol. 3, no. 3, pp. 1458–1465, 2018.
-  J. Carius, R. Ranftl, V. Koltun, and M. Hutter, “Trajectory optimization with implicit hard contacts,” IEEE Robotics and Automation Letters, vol. 3, no. 4, pp. 3316–3323, 2018.
-  T. Koolen, S. Bertrand, G. Thomas, T. De Boer, T. Wu, J. Smith, J. Englsberger, and J. Pratt, “Design of a momentum-based control framework and application to the humanoid robot atlas,” International Journal of Humanoid Robotics, vol. 13, no. 01, p. 1650007, 2016.
-  D. E. Orin and A. Goswami, “Centroidal momentum matrix of a humanoid robot: Structure and properties,” Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on, pp. 653 – 659, 2008.
-  P. Sardain and G. Bessonnet, “Forces acting on a biped robot. center of pressure-zero moment point,” IEEE Transactions on Systems, Man, and Cybernetics-Part A: Systems and Humans, vol. 34, no. 5, pp. 630–637, 2004.
-  N. S. Pollard and P. S. Reitsma, “Animation of humanlike characters: Dynamic motion filtering with a physically plausible contact model,” in Yale workshop on adaptive and learning systems, vol. 2. New Haven, CT, USA, 2001.
-  J. Carpentier, S. Tonneau, M. Naveau, O. Stasse, and N. Mansard, “A versatile and efficient pattern generator for generalized legged locomotion,” in Robotics and Automation (ICRA), 2016 IEEE International Conference on. IEEE, 2016, pp. 3555–3561.
-  S. Caron and Q.-C. Pham, “When to make a step? tackling the timing problem in multi-contact locomotion by topp-mpc,” in 2017 IEEE-RAS 17th International Conference on Humanoid Robotics (Humanoids). IEEE, 2017, pp. 522–528.
-  J. T. Betts, Practical Methods for Optimal Control and Estimation Using Nonlinear Programming. Society for Industrial and Applied Mathematics, 2010, vol. 19.
-  J. A. E. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, “CasADi – A software framework for nonlinear optimization and optimal control,” Mathematical Programming Computation, 2018.
-  A. Wächter and L. T. Biegler, “On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming,” Mathematical Programming, vol. 106, no. 1, pp. 25–57, 2006.
-  P. R. Amestoy, I. S. Duff, J.-Y. L’Excellent, and J. Koster, “Mumps: a general purpose distributed memory sparse solver,” in International Workshop on Applied Parallel Computing, 2000, pp. 121–130.
-  A. HSL, “collection of fortran codes for large-scale scientific computation,” See http://www. hsl. rl. ac. uk, 2007.