\replaced[id=3, remark=5]A FastAn Online Trajectory Optimization Framework for Legged Robots unifying ZMP and Capture Point Approaches

\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 Vertex-based ZMP Constraints

Alexander W. Winkler    Farbod Farshidian    Diego Pardo    Michael Neunert    Jonas Buchli Agile & Dexterous Robotics Lab, Institute of Robotics and Intelligent Systems, ETHZ Zurich, Switzerland. email: {winklera, farbodf, depardo, neunertm, buchlij}@ethz.ch

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 vertex-based representation of the support-area constraint, which can treat arbitrarily oriented point-, line-, and area-contacts uniformly. This generalization allows us to create motions such quadrupedal walking, trotting, bounding, pacing, combinations and transitions between these, limping, bipedal walking and push-recovery 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.

\newacronym

hyqHyQHydraulically Actuated Quadruped \newacronymcomCoMCenter of Mass \newacronymzmpZMPZero-Moment-Point \newacronymcpCPCapture Point \newacronymcopCoPCenter of Pressure \newacronymocOCOptimal Control \newacronymtoTOTrajectory Optimization \newacronymnlpNLPNonlinear Programming Problem \newacronymqpQPQuadratic Program \newacronymcmaesCMA-ESCovariance 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

\IEEEpeerreviewmaketitle

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 full-body \glsto, in which an optimal body and endeffector motion plus the appropriate inputs are discovered to achieve a high-level 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 straight-forward 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 higher-level 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 foot-trajectories that the body motion planner cannot modify, constraints such as stability or kinematic reachability become purely the responsibility of the lower-level 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 high-level 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 support-area with multiple feet in contact is often avoided, by not sampling the constraint during the double-support phase. For biped robots neglecting the constraint in the double-support 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 vertex-based 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 double-support phase remains. Through our proposed formulation, double- and single-stance support areas can be represented for arbitrary foot geometry, including point-feet. Additionally, it allows to represent arbitrarily oriented 1D-support 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 \glszmp-based approaches were so far only used for quadrupedal walking, where 2D-support areas were present.

The approach presented in this paper combines the \glsip-based \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 \glszmp-based legged locomotion problem [10] into a standard \glsto formulation with the \glscop as input, clearly identifying state, dynamic model and path- and boundary-constraints, which permits easier comparison with existing methods in the \glsto domain. Push recovery behavior also naturally emerges from this formulation.

  • We introduce a vertex-based representation of the \glscop constraint, instead of hyperplanes. which allows us to treat arbitrarily oriented point-, line-, and area-contacts uniformly. This enables us to generate motions that are difficult for other \glszmp-based approaches, such as bipedal walk with double-support phases, point-feet 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 push-recovery 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 second-order 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 x-direction (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.

Figure 1: Modeling of a quadruped robot by a \glsip with the right-front and left-hind legs in contact. Through joint torques the robot can control the center of pressure and thereby the motion of the \glscom . However, can only lie inside the convex hull (green line) of the contact points.

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 continuous-time \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 point-foot 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 point-foot. 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 steady-state maintaining zero \glscom velocity becomes

(5)

This is the one-step \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, time-varying \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 high-level in the following, corresponding to Fig. 2, whereas more specific details of the implementation are postponed to the next section.

Figure 2: Overview of the \glsto problem: A point-foot quadruped robot trotting forward in x-direction, first swinging right-front and left-hind legs , then left-front and right-hind . The \glscom motion (black line) is generated by shifting the \glscop (red dots). However, can only lie in the convex hull (green area) of the legs in contact at that time . Additionally, the position of each leg must always be inside its range of motion (gray areas for front and hind legs) relative to the \glscom. The optimization problem consist of varying the position of the footholds , to allow inputs that drive the robot from an initial position to a desired goal position in time .

Unilateral Forces

We clearly differentiate between the \glscop and the feet positions , which only coincide for a point-foot 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 line-contacts of arbitrary orientation. We use predefined contact sequences and timings , to only optimize over real-valued decision variables and not turn the problem into a mixed-integer \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 marginally-stable 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 hand-tuned 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 path-constraints and defined the cost (21).

3.1 Center-of-Mass 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 quartic-polynomials 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.

Range-of-Motion 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.

Figure 3: Top down view of a biped for both feet in contact at inside the range of motion (gray), which moves with the \glscom position. For square feet with corners , rotated by , the support area is shown by (light green area). This is the area to which the \glscop is constrained. If the biped controls its \glscop to lie on the tip of the right foot, the corresponding corner carries all the load (), while the other seven lambdas are zero. In case of point-feet the support area is simply a straight line between and .

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 point-foot robot with , (17) simplifies to .

We represent for the duration of the motion by piecewise-constant 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 support-area 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 solution-space 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 swing-legs, while still staying away from the edges of support-areas 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 fully-body dynamics model. This section discusses this generation summarized by Fig. 4.

Figure 4: The controller that generates the required torques to execute a planned motion. Given the current state of the system and a user defined goal state , the optimizer generates a reference motion. We augment this reference through a body feedback acceleration based on how much the body deviates from the desired motion. Inverse dynamics is used to generate the torques to achieve the reference base and joint accelerations.

4.1 Generating full-body 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 PD-controller 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 full-body 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 low-gain PD-controller 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 on-board, fusing \glsimu and joint encoder values. Torque tracking is performed at \SI1000\Hz, while the reference position, velocity and torque set-points are provided at \SI250\Hz. The C++ dynamics model is generated by [30].

(a) Quadruped Walk: swinging one leg at a time.
(b) Quadruped Trot: swinging diagonally opposite legs.
(c) Quadruped Pace: swinging left, then right legs.
(d) Quadruped Bound: swinging front, then hind legs.
Figure 5: Top down view of the generated motions for a quadruped robot moving from left to right, swinging the legs left-hind (blue), left-front (purple), right-hind (brown), right-front (green) in the sequence shown. The initial stance is shown by the squares, the optimized steps by the circles. The \glscom motion is shown by the solid line, where the color corresponds to the swingleg(s) at that moment. If all legs are in contact during, the motion and corresponding \glscop is shown in gray. The support area for each phase is shown by the transparent areas. The optimized \glscop positions that drive the system are shown in red and always lie inside the support area.

5.1 Discussion of generated motions

This section analyses the different motions generated by changing the sequence and timings of contacts . There is no high-level 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 push-recovery.

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 left-right 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 line-contacts 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 right-front (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 four-leg transition period between them produces the motion shown in Fig. c. This can also be viewed as biped walking with line-feet (e.g. skis), with the constraint enforced also during the double-stance 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 side-ways 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
Table 1: Specs of the \glsnlp for 16- and 4-step motions

6 Conclusion

This paper presented a \glsto formulation using vertex-based support-area 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 x-direction

(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 one-step \acrlongcp originally derived in [14].

.2 Dynamic Constraint

The system dynamics constraint (11) enforced through , with the local polynomial time , are formulated as

References

  1. G. Schultz and K. Mombaur, “Modeling and optimal control of human-like running,” IEEE/ASME Transactions on Mechatronics, vol. 15, no. 5, pp. 783–792, 2010.
  2. H. Dai, A. Valenzuela, and R. Tedrake, “Whole-body Motion Planning with Simple Dynamics and Full Kinematics,” IEEE-RAS International Conference on Humanoid Robots, 2015.
  3. 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.
  4. I. Mordatch, E. Todorov, and Z. Popović, “Discovery of complex behaviors through contact-invariant optimization,” ACM Transactions on Graphics, vol. 31, no. 4, pp. 1–8, 2012.
  5. 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. 2016-June, no. 724454, pp. 1366–1373, 2016.
  6. 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 (RA-L), 2017.
  7. D. Pardo, M. Neunert, A. W. Winkler, and J. Buchli, “Hybrid direct collocation and control in the constraint-consistent subspace for dynamic legged robot-legged locomotion,” RSS Science and Systems Conference, 2017.
  8. S. Coros, A. Karpathy, B. Jones, L. Reveret, and M. van de Panne, “Locomotion skills for simulated quadrupeds,” ACM SIGGRAPH, p. 1, 2011.
  9. 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.
  10. S. Kajita, F. Kanehiro, K. Kaneko, K. Fujiwara, K. Harada, K. Yokoi, and H. Hirukawa, “Biped walking pattern generation by using preview control of zero-moment point,” IEEE International Conference on Robotics and Automation, pp. 1620–1626, 2003.
  11. 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.
  12. 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.
  13. 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. 2016-June, no. 3, pp. 3555–3561, 2016.
  14. J. Pratt, J. Carff, S. Drakunov, and A. Goswami, “Capture point: A step toward humanoid push recovery,” in IEEE-RAS International Conference on Humanoid Robots, 2006, pp. 200–207.
  15. 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.
  16. 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.
  17. I. Mordatch, M. de Lasa, and A. Hertzmann, “Robust physics-based locomotion using low-dimensional planning,” ACM Transactions on Graphics, vol. 29, no. 4, p. 1, 2010.
  18. B. J. Stephens and C. G. Atkeson, “Push recovery by stepping for humanoid robots with force controlled joints,” in IEEE-RAS International Conference on Humanoid Robots, Humanoids 2010, 2010, pp. 52–59.
  19. 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.
  20. 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.
  21. 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.
  22. 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.
  23. 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 multi-contact situation,” 2016 IEEE-RAS International Conference on Humanoid Robots (Humanoids 2016), no. 1, pp. 6–11, 2016.
  24. 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.
  25. C. Hargraves and S. Paris, “Direct trajectory optimization using nonlinear programming and collocation,” Journal of Guidance, Control, and Dynamics, 1987.
  26. 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.
  27. 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.
  28. C. Semini, “HyQ - Design and development of a hydraulically actuated quadruped robot,” Ph.D. dissertation, Istituto Italiano di Tecnologia, 2010.
  29. 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.
  30. 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.
  31. A. Waechter and L. T. Biegler, On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming, 2006, vol. 106, no. 1.
  32. P. E. Gill, W. Murray, and M. A. Saunders, “SNOPT: An SQP Algorithm for Large-Scale Constrained Optimization,” pp. 979–1006, 2002.
72367
This is a comment super asjknd jkasnjk adsnkj
Upvote
Downvote
Edit
-  
Unpublish
""
The feedback must be of minumum 40 characters
The feedback must be of minumum 40 characters
Submit
Cancel
Comments 0
Request comment
""
The feedback must be of minumum 40 characters
Add comment
Cancel
Loading ...

You are asking your first question!
How to quickly get a good answer:
  • Keep your question short and to the point
  • Check for grammar or spelling errors.
  • Phrase it like a question
Test
Test description