\replaced[id=3, remark=5]A FastAn Online Trajectory Optimization Framework for Legged Robots unifying ZMP and Capture Point Approaches
Fast Trajectory Optimization for Legged Robots using Vertexbased ZMP Constraints
Abstract
This paper combines the fast \glszmp approaches that work well in practice with the broader range of capabilities of a \acrlongto formulation, by optimizing over body motion, footholds and \acrlongcop simultaneously. We introduce a vertexbased representation of the supportarea constraint, which can treat arbitrarily oriented point, line, and areacontacts uniformly. This generalization allows us to create motions such quadrupedal walking, trotting, bounding, pacing, combinations and transitions between these, limping, bipedal walking and pushrecovery all with the same approach. This formulation constitutes a minimal representation of the physical laws (unilateral contact forces) and kinematic restrictions (range of motion) in legged locomotion, which allows us to generate various motion in less than a second. We demonstrate the feasibility of the generated motions on a real quadruped robot.
hyqHyQHydraulically Actuated Quadruped \newacronymcomCoMCenter of Mass \newacronymzmpZMPZeroMomentPoint \newacronymcpCPCapture Point \newacronymcopCoPCenter of Pressure \newacronymocOCOptimal Control \newacronymtoTOTrajectory Optimization \newacronymnlpNLPNonlinear Programming Problem \newacronymqpQPQuadratic Program \newacronymcmaesCMAESCovariance Matrix Adaptation Evolution Strategy \newacronymdofDoFDegrees of Freedom \newacronymmpcMPCModel Predictive Control \newacronymeomEoMEquation of Motion \newacronymipLIPLinear Inverted Pendulum \newacronymimuIMUInertial Measurement Unit \newacronymodeODEOrdinar Differential Equation \newlistmylistenumerate*1 \setlist[mylist]label=() \setauthormarkup \setremarkmarkup[#1.#2] \definechangesauthor[name=General, color=blue]gen \definechangesauthor[name=Editor, color=blue]1 \definechangesauthor[name=Reviewer 2, color=blue]2 \definechangesauthor[name=Reviewer 3, color=orange]3 \definechangesauthor[name=Reviewer 4, color=blue]4 \definechangesauthor[name=Reviewer 5, color=blue]5 \IEEEoverridecommandlockouts\overrideIEEEmargins \glsresetall
1 Introduction
Planning and executing motions for legged systems is a complex task. A central difficulty is that legs cannot pull on the ground, e.g. the forces acting on the feet can only push upwards. Since the motion of the body is mostly generated by these constrained (=unilateral) contact forces, this motion is also restricted. When leaning forward past the tip of your tows, you will fall, since your feet cannot pull down to generate a momentum that counteracts the gravity acting on your \glscom. Finding motions that respect these physical laws can be tackled by various approaches described in the following.
A successful approach to tackle this problem is through fullbody \glsto, in which an optimal body and endeffector motion plus the appropriate inputs are discovered to achieve a highlevel goal. This was demonstrated by [1, 2, 3, 4, 5, 6, 7] resulting in an impressive range of motions for legged systems. These \glsto approaches have shown great performance, but are often time consuming to calculate and not straightforward to apply on a real robot. In [8] the authors generate an wide range of quadruped gaits, transitions and jumps based on a parameterized controller and periodic motions. While the resulting motions are similar to ours, the methods are very different: While our approach is based on \glsto with physical constraints, [8] optimizes controller parameters based mainly on motion capture data.
Previous research has shown that to generate feasible motions to execute on legged systems, non\glsto approaches also work well, although the motions cannot cover the range of the approaches above. One way is to model the robot as a \glsip and keep the \glszmp [9] inside the convex hull of the feet in stance. This approach has been successfully applied to generate motions for biped and quadruped walking [10, 11, 12, 13]. However, these hierarchical approaches use predefined footholds, usually provided by a higherlevel planner beforehand that takes terrain information (height, slope) into account. Although this decoupling of foothold planning and body motion generation reduces complexity, it is unnatural, as the main intention of the footholds is to assist the body to achieve a desired motion. By providing fixed foottrajectories that the body motion planner cannot modify, constraints such as stability or kinematic reachability become purely the responsibility of the lowerlevel body motion planner, artificially constraining the solution. A somewhat reverse view of the above are \acrfullcp [14] approaches, which have been successfully used to generate dynamic trotting and push recovery motions for quadruped robots [15, 16]. A desired body motion (usually a reference \glscom velocity) is given by a highlevel planner or heuristic, and a foothold/\glscop trajectory must be found that generates it.
Because of the dependency between footholds and body motion, approaches that optimize over both these quantities simultaneously, while still using a simplified dynamics model, have been developed [17, 18, 19, 20, 21, 22]. This reduces heuristics while increasing the range of achievable motions, but still keeps computation time short compared to full body \glsto approaches. These approaches are most closely related to the work presented in this paper.
The approaches [18, 19, 20, 21] demonstrate impressive performance on biped robots. One common difficulty in these approaches however is the nonlinearity of the \glscop constraint with respect to the orientation of the feet. In [19, 20] the orientation is either fixed or solved with a separate optimizer beforehand. In [21] the nonlinearity of this constraint is accepted and the resulting nonlinear optimization problem solved. However, although the orientation of the individual feet can be optimized over in these approaches, a combined supportarea with multiple feet in contact is often avoided, by not sampling the constraint during the doublesupport phase. For biped robots neglecting the constraint in the doublesupport phase is not so critical, as these take up little time during normal walking. For quadruped robots however, there are almost always two or more feet in contact at a given time, so the correct representation of the dynamic constraint in this phase is essential.
We therefore extend the capabilities of the approaches above by using a vertexbased representation of the \glscop constraint, instead of hyperplanes. In [23] this idea is briefly touched, however the connection between the corners of the foot geometry and the convexity variables is not made and thereby the restriction of not sampling in the doublesupport phase remains. Through our proposed formulation, double and singlestance support areas can be represented for arbitrary foot geometry, including pointfeet. Additionally, it allows to represent arbitrarily oriented 1Dsupport lines, which wasn’t possible with the above approaches. Although not essential for biped walk on flat feet, this is a core necessity for dynamic quadruped motions (trot, pace, bound). This is a reason why \glszmpbased approaches were so far only used for quadrupedal walking, where 2Dsupport areas were present.
The approach presented in this paper combines the \glsipbased \glszmp approaches that are fast and work well in practice with the broader range of capabilities of a \glsto formulation. A summary of the explicit contributions with respect to the papers above are:

We reformulate the traditional \glszmpbased legged locomotion problem [10] into a standard \glsto formulation with the \glscop as input, clearly identifying state, dynamic model and path and boundaryconstraints, which permits easier comparison with existing methods in the \glsto domain. Push recovery behavior also naturally emerges from this formulation.

We introduce a vertexbased representation of the \glscop constraint, instead of hyperplanes. which allows us to treat arbitrarily oriented point, line, and areacontacts uniformly. This enables us to generate motions that are difficult for other \glszmpbased approaches, such as bipedal walk with doublesupport phases, pointfeet locomotion, various gaits as well as arbitrary combinations and transitions between these.

Instead of the heuristic shrinking of support areas, we introduce a cost term for uncertainties that improve the robustness of the planned motions.
We demonstrate that the problem can be solved for multiple steps in less than a second to generate walking, trotting, bounding, pacing, combinations and transitions between these, limping, biped walking and pushrecovery motions for a quadruped robot. Additionally, we verify the physically feasibility of the optimized motions through demonstration of walking and trotting on a real \SI80\kg hydraulic quadruped.
2 Method
2.1 Physical Model
We model the legged robot as a \acrfullip, with its \glscom located at a constant height . The touchdown position of the pendulum with the ground (also known as \glszmp or \glscop) is given by as seen in Fig. 1. The \glscom acceleration is predefined by the physics of a tipping pendulum
(1) 
The secondorder dynamics are influenced by the \glscom position , the \glscop and gravity . This model can be used to describe a legged robot, since the robot can control the torques in the joints, thereby the contact forces and through these the position of the \glscop. Looking only at the xdirection (left image in Fig. 1), if the robot decides to lift the hind leg, the model describing the system dynamics is a pendulum in contact with the ground at the front foot , so . Since this pendulum is nearly upright, the \glscom will barely accelerate in x; the robot is balancing on the front leg. However, lifting the front leg can be modeled as placing the pendulum at , which is strongly leaning and thereby must accelerate forward in x. By distributing the load between the legs, the robot can generate motions corresponding to a pendulum anchored anywhere between the contact points, e.g. (see Fig. 1). Therefore, the \glscop is considered the input to the system and an abstraction of the joint torques and contact forces.
2.2 Trajectory Optimization Problem
We want to obtain the inputs that generate a motion from an initial state to a desired goal state in time for a robot described by the system dynamics , while respecting some constraints and optimizing a performance criteria . This can be formulated as a continuoustime \glsto problem
(2a)  
(2b)  
(2c)  
(2d)  
(2e)  
(2f) 
The dynamics are modeled as those of a \glsip (1), whereas the state and input for the legged system model are given by
(3)  
(4) 
which includes the \glscom position and velocity and the position and orientation of the feet. The input to move the system is the generated \glscop, abstracting the usually used contact forces or joint torques.
2.3 Specific Case: Capture Point
We briefly show that this general \glsto formulation, using the \glsip model, also encompasses Capture Point methods to generate walking motions. Consider the problem of finding the position to step with a pointfoot robot to recover from a push. With the initial position and the initial velocity generated by the force of the push we have . The robot should come to, and remain, at a stop at the end of the motion, irrespective of where and when, so we have . We parametrize the input by the constant parameter , as we only allow one step with a pointfoot. We allow the \glscop to be placed anywhere, e.g. no path constraints (2d) and do not have a preference as to how the robot achieves this task, e.g. .
Such a simple \glsto problem can be solved analytically, without resorting to a mathematical optimization solver (see Appendix .1). The point on the ground to generate and hold the \glscop in order to achieve a final steadystate maintaining zero \glscom velocity becomes
(5) 
This is the onestep \acrfullcp, originally derived by [14] and the solution of our general \glsto formulation (2) for a very specific case (e.g one step/control input, zero final velocity).
2.4 General Case: Legged Locomotion Formulation
Compared to the above example, our proposed formulation adds the capabilities to represent motions of multiple steps, timevarying \glscop, physical restrictions as to where the \glscop can be generated and preferences which of the feasible motions to choose. This \glsto formulation is explained on a highlevel in the following, corresponding to Fig. 2, whereas more specific details of the implementation are postponed to the next section.
Unilateral Forces
We clearly differentiate between the \glscop and the feet positions , which only coincide for a pointfoot robot with one leg in contact. Generally, the footholds affect the input bounds of . We use to control the body, but must at the same time choose appropriate footholds to respect the unilateral forces constraint. Traditional \glszmp approaches fix the footholds in advance, as the combination of both the \glscop and the footholds make this constraint nonlinear. We accept this nonlinearity and the higher numerical complexity associated with it. This gives us a much larger range of inputs , as we can “customize” our bounds by modifying the footholds according to the desired task. Therefore, the first path constraint of our \glsto problem is given by
(6) 
where represents the convex hull of the feet in contact as seen in Fig. 2 and is the indicator if foot is in contact.
We implement this convex hull constraint by weighing the vertices/corners of each foot in contact. This extends the capabilities of traditional representations by line segments/hyperplanes to also model point and linecontacts of arbitrary orientation. We use predefined contact sequences and timings , to only optimize over realvalued decision variables and not turn the problem into a mixedinteger \glsnlp. Simply by adapting this contact schedule , the optimizer generates various gaits as well as combinations and transitions between these, for which previously separate frameworks were necessary.
Kinematic Reachability
When modifying the footholds to enclose the \glscop, we must additionally ensure that these stay inside the kinematic range of the legs (Reachability). This constraint that depends on both the \glscom and foothold positions is formulated for every leg as
(7) 
Allowing the modification of both these quantities simultaneously characterizes the legged locomotion problem more accurately and reduces heuristics used in hierarchical approaches.
Robust Motions
With the above constraints the motion will comply to physics and the kinematics of the system. This feasible motion is assuming a simplified model, a perfect tracking controller and an accurate initial state. To make solutions robust to real world discrepancies where these assumptions are violated, it is best to avoid the borders of feasible solutions, where the inequality constraints are tight (). This can be achieved by artificially shrinking the solutions space by a stability margin (e.g. ). For legged locomotion this is often done by shrinking the support area to avoid solutions were the \glscop is placed at the marginallystable border [11].
We do not restrict the solution space, but choose the more conservative of the feasible motions through a performance criteria . This soft constraint expresses “avoid boundaries when possible, but permit if necessary”. The robot is allowed to be at marginally stable states, but since there are many uncertainties in our model and assumptions, it is safer to avoid them. This cost does not require a handtuned stability margin and the solution can still be at the boundaries when necessary. However, especially for slow motions (e.g. walking) where small inaccuracies can accumulate and cause the robot to fall, this cost term is essential to generate robust motions for real systems.
3 Implementation
There exist different methods to solve Optimal Control problems (2), namely Dynamic Programming (Bellman Optimality Equation), indirect (Maximum Principle) and direct methods [24]. In direct methods the continuous time \glsto problem is represented by a finite number of decision variables and constraints and solved by a nonlinear programming solver. If the decision variables fully describe the input and state over time, the method is further classified as a simultaneous direct method, with flavors Direct Transcription and Multiple Shooting. In our approach we chose a Direct Transcription formulation, e.g. optimizing state and controls together. This has the advantage of not requiring an ODE solver, constraints on the state can be directly formulated and the sparse structure of the Jacobian often improves convergence. The resulting discrete formulation to solve the continuous problem in (2) is given by
find  
subject to  
where are the parameters describing the \glscom motion, the feet motion (swing and stance) and the position of the \glscop. This section describes in detail how we parametrize the state and input , formulate the pathconstraints and defined the cost (21).
3.1 CenterofMass Motion
This section explains how the continuous motion of the \glscom can be described by a finite number of variables to optimize over, while ensuring compliance with the \glsip dynamics.
CoM Parametrization
The \glscom motion is described by a spline, strung together by quarticpolynomials as
(8)  
(9) 
with coefficients and describing the global time at the start of polynomial .
We ensure continuity of the spline by imposing equal position and velocity at each of the junctions between polynomial and , so . Using we enforce
(10) 
Dynamic Constraint
In order to ensure consistency between the parametrized motion and the dynamics of the system (1), the integration of our approximate solution must resemble that of the actual system dynamics, so
(11) 
Simpson’s rule states that if is chosen as a order polynomial (which is why is chosen as order) that matches the system dynamics at the beginning, the center and at the end, then (11) is bounded by an error proportional to . Therefore we add the following constraints for each polynomial
(12) 
(see Appendix .2 for a more detailed formulation). By keeping the duration of each polynomial short (\SI50ms), the error of Simpson’s integration stays small and the order polynomial solution is close to an actual solution of the \glsode in (1).
This formulation is similar to the ”collocation” constraint [25]. Collocation implicitly enforces the constraints (12) at the boundaries through a specific parametrization of the polynomial, while the above formulation achieves this through explicit constraints in the \glsnlp. Reversely, collocation enforces that through the explicit constraint, while our formulation does this through parametrization in (8).
3.2 Feet Motion
Feet Parametrization
We impose a constant position and orientation if leg is in stance. We use a cubic polynomial in the ground plane to move the feet between two consecutive contacts
(13) 
where is the elapsed time since the beginning of the swing motion. The vertical swingleg motion does not affect the \glsnlp and is therefore not modeled. The coefficients and are fully determined by the predefined swing duration and the position and orientation of the enclosing contacts and . Therefore the continuous motion of all feet can be parametrized by the \glsnlp decision variables
(14) 
are the parameters to fully describe the motion of a single leg taking steps.
RangeofMotion Constraint
To ensure a feasible kinematic motion, we must enforce , which is the gray area in Fig. 3. We approximate the area reachable by each foot through a rectangle , representing the allowed distance that a foot can move from its nominal position (center of gray area). The foothold position for each foot is therefore constrained by
(15) 
Contrary to hierarchical approaches, this constraint allows the optimizer to either move the body to respect kinematic limits or place the feet at different positions. A constraint on the foot orientation can be formulated equivalently.
3.3 Center of Pressure Motion
To represent the continuous \glscop trajectory, we parameterize it through the load carried by each endeffector. This parametrization is used to formulate a novel convexity constraint based on vertices instead of hyperplanes. Finally this section introduces a cost that keeps the \glscop from marginally stable regions and improves robustness of the motion.
CoP Parametrization
The \glscop is not parametrized by polynomial coefficients or discrete points, but by the relative load each corner of each foot is carrying. This load is given by
(16) 
represents the number of vertices/corners of foot . For the square foot in Fig. 3, four lambda values represent one foot and distribute the load amongst the corners. These multipliers represent the percentage of vertical force that each foot is carrying, e.g. implies that leg is carrying 90% of the weight of the robot at time . Using these values, the \glscop is parameterized by
(17) 
where represents the rotation matrix corresponding to the optimized rotation of foot (13). represents the fixed position (depending on the foot geometry) of corner of the foot expressed in the foot frame. For a pointfoot robot with , (17) simplifies to .
We represent for the duration of the motion by piecewiseconstant values discretized every \SI20ms, resulting in nodes. Therefore the \glscop can be fully parameterized by and the additional \glsnlp decision variables
(18) 
Unilateral Forces Constraint
We represent the essential input constraint (6), which ensures that only physically feasible forces inside the convex hull of the contacts are generated, for as
(19a)  
(19b) 
where is the indicator if foot is in contact. The constraints (17) and (19a) allow to be located anywhere inside the convex hull of the vertices of the current foot positions, independent of whether they are in contact. However, since only feet in contact can actually carry load, (19b) enforces that a leg that is swinging () must have all the corners of its foot unloaded. These constraints together ensure that the \glscop lies inside the green area shown in Fig. 3.
Robust Walking Cost
To keep the \glscop away from the edges of the supportarea we could constrain of each leg in stance to be greater than a threshold, causing these legs in contact to never be unloaded. This conceptually corresponds to previous approaches that heuristically shrink support areas and thereby reduce the solutionspace for all situations. We propose a cost that has similar effect, but still permits the solver to use the limits of the space if necessary.
The most robust state to be in, is when the weight of the robot is equally distributed amongst all the corners in contact, so
(20) 
where is the total number of vertices in contact at time , predefined by the contact sequence . This results in the \glscop to be located in the center of the support areas. The deviation of the input values from the optimal values over the entire discretized trajectory (18) is then given by
(21) 
For a support triangle () this cost tries to keep the \glscop in the center and for a line () in the middle. For quadruped walking motions this formulation generates a smooth transition of the \glscop between diagonally opposite swinglegs, while still staying away from the edges of supportareas whenever possible.
4 Tracking the Motion
The motion optimization part of our approach is largely robot independent. The only robot specific information needed to run the framework are the robot height, the number of feet, their geometry and their kinematic range. For execution however, the optimized motion must be translated into joint torques using a fullybody dynamics model. This section discusses this generation summarized by Fig. 4.
4.1 Generating fullbody reference accelerations
The 6–\glsdof base pose is reconstructed using zero desired orientation (in Euler angles x,y,z), the optimized \glscom motion (assuming the geometric center of the base coincides with the \glscom), and the constant base height as
In order to cope with uncertainties it is essential to incorporate feedback into the control loop. We do this by adding an operational space PDcontroller on the base that creates desired 6D base accelerations according to
The derivate of the pose, the base twist represents the base angular and linear velocities and is the optimized \glscom acceleration from the \glsnlp. This controller modifies the planned body motion if the current state deviation from the reference state.
In order to obtain the desired joint accelerations that correspond to the planned Cartesian motion of the feet we can use the relationship where represent the full body state (base + joints) and the Jacobian that maps fullbody velocities to linear foot velocities in world frame. Rearranging this equation, and using the Moore–Penrose pseudoinverse , gives us the reference joint acceleration
(22) 
4.2 Inverse Dynamics
The inverse dynamics controller is responsible for generating required joint torques to track the reference acceleration , which is physically feasible based on the \glsip model. This is done based on the rigid body dynamics model of the system, which depends on the joint torques, but also the unknown contact forces. To eliminate the contact forces from the equation, we project it into the space of joint torques by where is the contact Jacobian that maps Cartesian contact forces to joint torques [26, 27]. This allows us to solve for the required joint torques through
(23) 
where is the joint space inertia matrix, the effect of Coriolis forces on the joint torques and the selection matrix which prohibits from actuating the floating base state directly. We found it beneficial to also add a lowgain PDcontroller on the joint position and velocities. This can mitigate the effects of dynamic modeling errors and force tracking imperfections.
5 Results
We demonstrate the performance of this approach on the hydraulically actuated quadruped robot HyQ [28]. The robot weighs approximately \SI80\kg, moves at a height of about \SI0.6\m and is torque controlled. Base estimation [29] is performed onboard, fusing \glsimu and joint encoder values. Torque tracking is performed at \SI1000\Hz, while the reference position, velocity and torque setpoints are provided at \SI250\Hz. The C++ dynamics model is generated by [30].
5.1 Discussion of generated motions
This section analyses the different motions generated by changing the sequence and timings of contacts . There is no highlevel footstep planner; the footholds are chosen by the optimizer to enable the body to reach a user defined goal state . The results where obtained using C++ code interfaced with Interior Point Method (Ipopt [31]) or Sequential Quadratic Programming (Snopt [32]) solvers on an Intel Core i7/\SI2.8GHz Quadcore laptop. The Jacobians of the constraint and the gradient of the cost function are provided to the solver analytically, which is important for performance. We initialize the decision variables with the quadruped standing in default stance for a given duration. The shown motions correspond to the first columns (e.g. 16 steps) in Table 1. The reader is encouraged to view the video at https://youtu.be/5WLeQMBuv30, as it very intuitively demonstrates the performance of this approach. Apart from the basic gaits, the video shows the capability of the framework to generate gradual transitions between them, bipedal walking, limping and pushrecovery.
Walk
Fig. a shows a walk of multiple steps, with the two support areas highlighted for swinging RFLH. The effect of the cost term is clearly visible, as the \glscop is accumulated away from the support area borders by leftright swaying of the body. Only when switching diagonally opposite legs the \glscop lies briefly at the marginally stable border, but then immediately shifts to a more conservative location. Without the cost term, the \glscom motion is a straight line between and , causing the real system to fail.
Trot
Fig. b shows a completely different pattern of support areas and \glscop distribution. During trotting only linecontacts exist, so the possible places to generate the \glscop is extremely restricted compared to walking. Notice how the \glscop lies close to the \glscom trajectory during the middle of the motion, but deviates quite large back/forward during the start/end of the motion (e.g. the robot pushing off from the rightfront (green) leg in the second to last step). This is because the distance between the \glscop and the \glscom generates the acceleration necessary for starting and stopping, whereas in the middle the robot is moving with nearly constant velocity.
Pace/Bound/Biped Walk
Specifying legs on the same side to be in contact, with a short fourleg transition period between them produces the motion shown in Fig. c. This can also be viewed as biped walking with linefeet (e.g. skis), with the constraint enforced also during the doublestance phase. The first observation is the sideways swaying motion of the \glscom. This is necessary because the support areas do not intersect (as in the trot) the \glscom trajectory. Since the \glscop always lies inside these left and right support areas, they will accelerate the body away from that side until the next step, which then reverses the motion. We found that the \glsip model with fixed zero body orientation does not describe such a motion very well, as the inherent rotation (rolling) of the body is not taken into account. In order to also demonstrate these motions on hardware, the \glsip model must be extended by the angular body motion. Specifying the front and hind legs to alternate between contact generates a bound Fig. d. The lateral shifting motion of the pace is now transformed to a forward backward motion of the \glscom due to support areas. In case of an omni directional robot a bounding gait can simply be considered a sideways pace.
(16 steps, \SI1\m) (4 steps, \SI0.2\m)  
Walk  Trot  Pace  Bound  
Horizon [s]  6.4 1.6  2.4 0.6  3.2 0.8  3.2 0.8 
Variables []  646 202  387 162  1868 728  1868 728 
Constraints []  850 270  548 255  2331 939  2331 939 
[s]  0.1  0.05  0.02  0.02 
Cost term        
Time Ipopt [s]  0.25 0.06  0.02 0.01  0.21 0.12  0.17 0.04 
Time Snopt [s]  0.35 0.04  0.04 0.01  0.54 0.18  0.42 0.29 
6 Conclusion
This paper presented a \glsto formulation using vertexbased supportarea constraints, which enables the generation of a variety of motions for which previously separate methods were necessary. In the future, more decision variables (e.g. contact schedule, body orientation, foothold height for uneven terrain), constraints (e.g. friction cone, obstacles) and more sophisticated dynamic models can be incorporated into this formulation. Additionally, we plan to utilize the speed of the optimization for \glsmpc.
.1 Derivation of Capture Point
Consider the differential equation describing a \glsip (linear, constant coefficients, second order) in xdirection
(24) 
The general solution to the homogeneous part of the equation can be construct by the Ansatz which leads to the characteristic equation resulting in Assuming constant input leads to the partial solution , and the space of solutions for the entire \glsode is given by
(25) 
where are the free parameters describing the motion. Imposing the initial position and velocity we obtain
(26) 
As we require the velocity to remain at zero (pendulum at rest). Since , and we have
(27)  
(28) 
which is known as the onestep \acrlongcp originally derived in [14].
.2 Dynamic Constraint
The system dynamics constraint (11) enforced through , with the local polynomial time , are formulated as
References
 G. Schultz and K. Mombaur, “Modeling and optimal control of humanlike running,” IEEE/ASME Transactions on Mechatronics, vol. 15, no. 5, pp. 783–792, 2010.
 H. Dai, A. Valenzuela, and R. Tedrake, “Wholebody Motion Planning with Simple Dynamics and Full Kinematics,” IEEERAS International Conference on Humanoid Robots, 2015.
 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, 2013.
 I. Mordatch, E. Todorov, and Z. Popović, “Discovery of complex behaviors through contactinvariant optimization,” ACM Transactions on Graphics, vol. 31, no. 4, pp. 1–8, 2012.
 M. Posa, S. Kuindersma, and R. Tedrake, “Optimization and stabilization of trajectories for constrained dynamical systems,” Proceedings  IEEE International Conference on Robotics and Automation, vol. 2016June, no. 724454, pp. 1366–1373, 2016.
 M. Neunert, F. Farshidian, A. W. Winkler, and J. Buchli, “Trajectory optimization through contacts and automatic gait discovery for quadrupeds,” to appear in IEEE Robotics and Automation Letters (RAL), 2017.
 D. Pardo, M. Neunert, A. W. Winkler, and J. Buchli, “Hybrid direct collocation and control in the constraintconsistent subspace for dynamic legged robotlegged locomotion,” RSS Science and Systems Conference, 2017.
 S. Coros, A. Karpathy, B. Jones, L. Reveret, and M. van de Panne, “Locomotion skills for simulated quadrupeds,” ACM SIGGRAPH, p. 1, 2011.
 M. Vukobratović and B. Borovac, “Zeromoment point  thirty five years of its life,” International Journal of Humanoid Robotics, vol. 1, no. 01, pp. 157–173, 2004.
 S. Kajita, F. Kanehiro, K. Kaneko, K. Fujiwara, K. Harada, K. Yokoi, and H. Hirukawa, “Biped walking pattern generation by using preview control of zeromoment point,” IEEE International Conference on Robotics and Automation, pp. 1620–1626, 2003.
 M. Kalakrishnan, J. Buchli, P. Pastor, M. Mistry, and S. Schaal, “Fast, robust quadruped locomotion over challenging terrain,” IEEE International Conference on Robotics and Automation, pp. 2665–2670, 2010.
 J. Z. Kolter, M. P. Rodgers, and A. Y. Ng, “A control architecture for quadruped locomotion over rough terrain,” IEEE International Conference on Robotics and Automation, pp. 811–818, 2008.
 J. Carpentier, S. Tonneau, M. Naveau, O. Stasse, and N. Mansard, “A versatile and efficient pattern generator for generalized legged locomotion,” IEEE International Conference on Robotics and Automation, vol. 2016June, no. 3, pp. 3555–3561, 2016.
 J. Pratt, J. Carff, S. Drakunov, and A. Goswami, “Capture point: A step toward humanoid push recovery,” in IEEERAS International Conference on Humanoid Robots, 2006, pp. 200–207.
 C. Gehring, S. Coros, M. Hutter, M. Bloesch, M. A. Hoepflinger, and R. Siegwart, “Control of dynamic gaits for a quadrupedal robot,” Proceedings  IEEE International Conference on Robotics and Automation, pp. 3287–3292, 2013.
 V. Barasuol, J. Buchli, C. Semini, M. Frigerio, E. R. De Pieri, and D. G. Caldwell, “A reactive controller framework for quadrupedal locomotion on challenging terrain,” IEEE International Conference on Robotics and Automation, pp. 2554–2561, 2013.
 I. Mordatch, M. de Lasa, and A. Hertzmann, “Robust physicsbased locomotion using lowdimensional planning,” ACM Transactions on Graphics, vol. 29, no. 4, p. 1, 2010.
 B. J. Stephens and C. G. Atkeson, “Push recovery by stepping for humanoid robots with force controlled joints,” in IEEERAS International Conference on Humanoid Robots, Humanoids 2010, 2010, pp. 52–59.
 H. Diedam, D. Dimitrov, P.b. Wieber, K. Mombaur, and M. Diehl, “Online Walking Gait Generation with Adaptive Foot Positioning through Linear Model Predictive Control,” IEEE International Conference on Intelligent Robots and Systems, 2009.
 A. Herdt, N. Perrin, and P. B. Wieber, “Walking without thinking about it,” IEEE International Conference on Intelligent Robots and Systems, pp. 190–195, 2010.
 M. Naveau, M. Kudruss, O. Stasse, C. Kirches, K. Mombaur, and P. Souères, “A ReactiveWalking Pattern Generator Based on Nonlinear Model Predictive Control,” IEEE Robotics and Automation Letters, vol. 2, no. 1, pp. 10–17, 2017.
 A. W. Winkler, F. Farshidian, M. Neunert, D. Pardo, and J. Buchli, “Online walking motion and foothold optimization for quadruped locomotion,” in IEEE International Conference on Robotics and Automation, 2017.
 D. Serra, C. Brasseur, A. Sherikov, D. Dimitrov, and P.b. Wieber, “A Newton method with always feasible iterates for Nonlinear Model Predictive Control of walking in a multicontact situation,” 2016 IEEERAS International Conference on Humanoid Robots (Humanoids 2016), no. 1, pp. 6–11, 2016.
 M. Diehl, H. G. Bock, H. Diedam, and P. B. Wieber, “Fast direct multiple shooting algorithms for optimal robot control,” Lecture Notes in Control and Information Sciences, vol. 340, pp. 65–93, 2006.
 C. Hargraves and S. Paris, “Direct trajectory optimization using nonlinear programming and collocation,” Journal of Guidance, Control, and Dynamics, 1987.
 F. Aghili, “A unified approach for inverse and direct dynamics of constrained multibody systems based on linear projection operator: Applications to control and simulation,” IEEE Transactions on Robotics, vol. 21, no. 5, pp. 834–849, 2005.
 M. Mistry, J. Buchli, and S. Schaal, “Inverse dynamics control of floating base systems using orthogonal decomposition,” IEEE International Conference on Robotics and Automation, no. 3, pp. 3406–3412, 2010.
 C. Semini, “HyQ  Design and development of a hydraulically actuated quadruped robot,” Ph.D. dissertation, Istituto Italiano di Tecnologia, 2010.
 M. Bloesch, M. Hutter, M. A. Hoepflinger, S. Leutenegger, C. Gehring, C. D. Remy, and R. Siegwart, “State Estimation for Legged Robots  Consistent Fusion of Leg Kinematics and IMU,” Robotics: Science and Systems, p. 2005, 2012.
 M. Frigerio, J. Buchli, D. G. Caldwell, and C. Semini, “RobCoGen : a code generator for efficient kinematics and dynamics of articulated robots , based on Domain Specific Languages,” Journal of Software Engineering for Robotics, vol. 7, no. July, pp. 36–54, 2016.
 A. Waechter and L. T. Biegler, On the implementation of an interiorpoint filter linesearch algorithm for largescale nonlinear programming, 2006, vol. 106, no. 1.
 P. E. Gill, W. Murray, and M. A. Saunders, “SNOPT: An SQP Algorithm for LargeScale Constrained Optimization,” pp. 979–1006, 2002.