PaperlayerView exact revision on arXiv

Source-linked AI summary

Trajectory Optimization Through Contacts and Automatic Gait Discovery for Quadrupeds

Michael Neunert, Farbod Farshidian, Alexander W. Winkler, Jonas Buchli

arXiv:1607.04537v1cs.RO

TL;DR

The paper addresses high-level robot tasks that leave the behavior needed to solve them unspecified. It uses whole-body trajectory optimization to discover contact-rich motions and gaits, with first hardware results showing optimized trajectories can transfer to the robot.

  • Problem

    A major challenge is specifying a high-level robot task without specifying how to solve it, including the behaviors needed for legged locomotion.

    Source: §I · p. 1

  • Method

    The framework optimizes whole-body robot dynamics through contact changes without priors on contact points, sequences, or timings.

    Source: §VI · p. 9

  • Results

    The approach produced motions across tasks, discovered galloping, trotting, and two-legged walking, and yielded first results transferring optimized trajectories to hardware.

    Source: §VI · p. 9

  • Takeaways & Limitations

    The motions may serve as initialization for other algorithms or provide insight into gait parameters such as stepping frequencies and stride lengths.

    Source: §VI · p. 9

    The broad solution space leaves users to select motions through cost-function weights or added terms, and motions may be vulnerable to model inaccuracies, disturbances, or noise.

    Source: §VI · p. 9

Abstract

from arXiv · show

In this work we present a trajectory Optimization framework for whole-body motion planning through contacts. We demonstrate how the proposed approach can be applied to automatically discover different gaits and dynamic motions on a quadruped robot. In contrast to most previous methods, we do not pre-specify contact switches, timings, points or gait patterns, but they are a direct outcome of the optimization. Furthermore, we optimize over the entire dynamics of the robot, which enables the optimizer to fully leverage the capabilities of the robot. To illustrate the spectrum of achievable motions, here we show eight different tasks, which would require very different control structures when solved with state-of-the-art methods. Using our trajectory Optimization approach, we are solving each task with a simple, high level cost function and without any changes in the control structure. Furthermore, we fully integrated our approach with the robot's control and estimation framework such that optimization can be run online. By demonstrating a rough manipulation task with multiple dynamic contact switches, we exemplarily show how optimized trajectories and control inputs can be directly applied to hardware.

I. INTRODUCTION

The paper addresses the difficulty of planning contact-rich motions without prescribing how the robot should perform them, while also seeking to run trajectory optimization online on hardware. Its approach optimizes whole-body dynamics and lets contact events emerge from the optimization.

I. INTRODUCTION

  • Earlier quadruped hardware work discovered dynamic gaits but prespecified contact sequences and optimized control parameters for a fixed structure.This work instead optimizes whole-body trajectories and control inputs.Source: §I · p. 2, A. Contributions · p. 2

A. Contributions

  • Whole-body optimization on HyQ discovers contact events without predefining contact switches, timings, points, or gait patterns.The framework is integrated with control, state estimation, and ground estimation; optimization runs online, though not in a control loop.Source: A. Contributions · p. 2

A. Optimal Control Problem

  • Trajectory optimization indirectly changes state and input trajectories through a time-varying feedback and feedforward controller to minimize a finite-horizon cost.The described SLQ algorithm iteratively simulates dynamics, linearizes them, solves a linear-quadratic problem backward, and updates the control.Source: A. Optimal Control Problem · p. 2

B. Sequential Linear Quadratic Control

Sequential Linear Quadratic Control (SLQ) iteratively approximates and solves an optimal-control problem around a trajectory. It computes feedforward actions and a time-varying regulator, but cannot enforce hard state constraints.

B. Sequential Linear Quadratic Control

  • SLQ rolls out the dynamics, linearizes them and quadratically approximates the cost, then solves the resulting linear-quadratic problem backward.It computes both feedforward control and a time-varying linear-quadratic regulator.Source: B. Sequential Linear Quadratic Control · p. 2
  • SLQ cannot handle hard state constraints, and soft state constraints can cause poor numerical behavior.Although constraints are not considered, most tasks produce trajectories within joint position and torque limits.Source: B. Sequential Linear Quadratic Control · p. 2

C. Cost Function

The cost function uses a quadratic form, with waypoint costs available to guide the trajectory.

C. Cost Function

  • The approach assumes a quadratic cost function, which the authors say can improve convergence and is often sufficient for the desired behavior.The objective weights final state cost, intermediate state cost, and input cost; waypoint costs add intermediate state guidance.Source: C. Cost Function · pp. 2–3

III. SYSTEM MODEL AND ROBOT DESCRIPTION

The system model represents HyQ as a rigid-body system with contact forces and actuator torques, using a state that combines base and joint variables. The authors use analytical and numerical differentiation to linearize the dynamics for SLQ.

III. SYSTEM MODEL AND ROBOT DESCRIPTION

  • HyQ is modeled as a rigid-body system because its torque-tracking performance is considered sufficient for assuming perfect torque control.The robot has four hydraulically actuated legs, each with three degrees of freedom.Source: A. Rigid Body Dynamics · p. 3, §III · p. 3

A. Rigid Body Dynamics

  • The dynamics include inertia, Coriolis and gravity terms, with contact forces acting through the contact Jacobian and actuator torques through a selection matrix.The state contains the six-degree-of-freedom base state and joint positions and velocities.Source: A. Rigid Body Dynamics · p. 3
  • Base pose and twist are expressed in different frames, while the model uses analytical derivatives for some linearization terms and numerical differentiation for others.Base orientation and position use the world frame; base angular and linear velocities use the body frame.Source: A. Rigid Body Dynamics · p. 3

B. Contact Model

The contact model balances physical accuracy against numerical stability by using smooth, nonlinear spring-damper forces in normal and tangential directions. Contact offsets are treated as hidden states, while friction limits are represented through contact-force saturation.

B. Contact Model

  • A smooth nonlinear spring-damper model represents contact forces in both the surface-normal and tangential directions.The normal spring displacement is ground penetration; the tangential displacement is the offset from where contact was established.Source: B. Contact Model · p. 3
  • Tangential contact uses spring and damping forces, with displacement and displacement-velocity directions represented as hidden states to keep the SLQ problem size constant.The tangential force is composed of a spring force along the displacement direction and a damping force along its velocity direction.Source: B. Contact Model · p. 3
  • The models become nonlinear near zero penetration to avoid discontinuities during contact changes.The smoothing coefficient sets this transition; parameter choices trade off physical correctness and convergence and depend on the task.Source: B. Contact Model · pp. 3–4
  • Friction cones let trajectory optimization account for possible slippage and contact-force saturation, assuming static and dynamic friction coefficients are equal.The contact models are visualized by plotting normal force against penetration and its derivative, and tangential force against penetration and tangential displacement.Source: B. Contact Model · p. 4, Figure 2 · p. 4

IV. STATE ESTIMATION AND TRACKING CONTROL

The trajectory-optimization approach is integrated with the robot’s estimation and control framework. Estimated state and contact information support trajectory tracking through joint and base control loops.

IV. STATE ESTIMATION AND TRACKING CONTROL

  • The optimized trajectory is sent to task-space base and joint controllers, whose corrective outputs are added to SLQ’s feedforward control signal.Both control loops run at 200 Hz.Source: §IV · p. 4
  • The framework combines estimation and control around SLQ, which optimizes the trajectory that the controllers track.The optimized feedforward signal is combined with the controllers’ corrective output before being sent to the robot.Source: §IV · p. 4
  • Base, stance, and ground estimators provide location and contact-configuration information, while joint and base controllers jointly control the robot’s state.The pipeline is shown in Figure 3.Source: Figure 3 · p. 4

A. State Estimation

State estimation supplies the state feedback and contact information needed by SLQ. The implementation estimates the base state and infers contact from ground-reaction-force estimates.

A. State Estimation

  • Because SLQ assumes full state feedback, the base pose and twist are estimated rather than directly measured.Joint positions and velocities are measured by HyQ’s encoders; the base state is estimated using.Source: A. State Estimation · p. 4
  • A leg is assumed to be in contact when its estimated normal ground-reaction force exceeds a fixed threshold.Joint torques are mapped to estimated ground-reaction forces to infer contact state.Source: A. State Estimation · p. 4
  • The ground is modeled as a plane fitted to the last contact point of each foot, providing elevation and surface-normal estimates without assuming level terrain.The implementation allows a ground map to be incorporated in the future.Source: A. State Estimation · p. 4

B. Tracking Controller

The tracking controller combines task-space base control with joint-space control. The authors use this simpler tracking controller instead of SLQ’s optimized feedback gains because those gains depend on the linearization point and cost regularization.

B. Tracking Controller

  • The authors do not use SLQ’s optimized feedback gains because contact nonlinearities can move the robot away from their TVLQR linearization point, and cost regularization also affects the gains.Online recomputation is noted as one possible solution, but the authors use a tracking controller with reduced complexity.Source: B. Tracking Controller · p. 4
  • The controller combines a task-space base PD controller with independent joint-space position and velocity tracking.With fixed stance, no slippage, and at least three stance legs, base and stance-leg states are coupled; swing legs still require joint control.Source: B. Tracking Controller · p. 4
  • The desired body wrench is converted to contact forces and joint torques, then added to SLQ’s feedforward action.The feedforward action already includes gravity-compensating torques, so no additional gravity-compensation term is added.Source: B. Tracking Controller · p. 4

V. EXPERIMENTS

The experiments use simple task costs to produce varied quadruped gaits and dynamic motions without prescribing contact sequences. The resulting trajectories include galloping, trotting, jumping, balancing, and standing-and-walking on the hind legs.

V. EXPERIMENTS

  • The gallop uses 9 steps to reach x = 2.0 m, with acceleration and deceleration driven especially by the hind legs and HFE joints.The cost uses final base- and leg-position targets with motion regularization, but no gait or contact-sequence terms.Source: Figure 5 · p. 5, 1) Galloping · p. 5, Figure 6 · p. 6
  • Increasing the base-motion penalty yields a pure trot; other cost weightings produce gallop-to-trot transitions, sidestepping, or turning in place.The diagonal leg pairs move together in the trotting variants, and the contact sequence and gait are not specified in advance.Source: 2) Trotting · p. 6
  • 0.8004 m is the measured squat-jump apex against a 0.8 m target, while the trajectory minimizes unnecessary base motion in x and y.The torque cost promotes an even distribution across the legs; torque spikes occur at take-off and landing.Source: Figure 7 · p. 6, 3) Squat Jump · p. 6, Figure 8 · p. 6
  • With a 30° pitched base target and 0.7 m apex, optimization produces a rearing motion that lifts the front legs and returns to full contact.Contact and stance configurations are not specified.Source: 4) Rearing · p. 7
  • Symmetric costs for diagonal balance produce asynchronous leg contacts: the front-left leg lifts later and touches down earlier, likely reflecting body CoG and inertia asymmetry.Source: 5) Diagonal Balance · p. 7
  • For the hind-leg stand-and-reach task, HyQ tucks a hind leg and uses the front-left leg to rise, then transitions from a brief hop to coordinated stepping.The front limbs help maintain balance before retracting to a target pose; the robot stabilizes upright at the end.Source: 6) Humanoid Walk · p. 7

B. Hardware

The pallet-pushing motion was optimized as a sequence of weight shifting, front-left-leg lift-off, pushing, and touchdown, then adjusted for hardware. Hardware tracking followed the planned trajectories closely, and overlapping contact events produced a dynamically stable motion without requiring static stability.

Hardware test

  • The pallet push emerged from one cost function with a single intermediate front-left-leg term, but required slight alteration for hardware because the deterministic solution's small CoG shift left it vulnerable to model inaccuracies or disturbances.The motion shifts the CoG over the three-leg support polygon, lifts the front-left leg, pushes the pallet, then sets the foot down and shifts the CoG back.Source: B. Hardware · p. 7

Hardware tracking

  • Planned and executed base pose and twist trajectories were similar, while joint tracking was better because the joint controller was more aggressive.Joint tracking also contributes indirectly to tracking the base trajectory.Source: Figure 9 · p. 8, Figure 10 · p. 8

Hardware test

  • Lift-off and touchdown partially overlapped the CoG shift, yielding a dynamically stable trajectory without requiring static stability.The task was run online after approaching the pallet with a separate walking controller, so initial conditions varied between runs.Source: B. Hardware · p. 8

Hardware tracking

  • During front-left-leg lift-off and touchdown, its joint torques were almost zero; neighboring legs carried increased load, moderated by the earlier CoG shift.The plotted torques combine trajectory-optimization feedforward torques and task-space control input.Source: B. Hardware · p. 8, Figure 11 · p. 8

C. Runtime and Convergence

Across tasks, most optimizations converged in 10–40 iterations and usually took less than one minute. Runtime per iteration increased linearly with the number of trajectory steps, while convergence varied with task duration and complexity.

Runtime and convergence

  • The runtime and convergence measurements serve as indicators of task complexity and of how far the approach is from model-predictive control use.The section tracks both iterations per task and runtime per iteration.Source: C. Runtime and Convergence · p. 8

Convergence

  • Manipulation tasks converged fastest, while trotting converged slowest; the authors relate convergence to task duration and complexity.The trotting task had a long horizon and high complexity, whereas rearing was simpler and guided by waypoint costs.Source: C. Runtime and Convergence · p. 8, Figure 12 · p. 9
  • Most tasks converged within 10–40 iterations and, given runtime per iteration, usually finished optimization in less than one minute.Tasks were initialized from a standing pure-joint-position controller, and costs were normalized by initial cost.Source: C. Runtime and Convergence · pp. 8–9

Runtime

  • Runtime per iteration scaled linearly with the number of trajectory steps, supporting the expected relationship with time horizon.Timings were averaged across iterations on a standard quadcore laptop using a custom multithreaded solver and optimized generated dynamics code.Source: Figure 13 · p. 9, C. Runtime and Convergence · p. 9

VI. DISCUSSION, CONCLUSION AND OUTLOOK

The framework plans fully dynamic whole-body motions with multiple contact changes without requiring contact priors, and demonstrates diverse tasks and discovered gaits. The authors also identify robustness, parameter tuning, and state constraints as limitations, while proposing shorter-horizon MPC as an outlook.

Conclusion

  • The framework creates fully dynamic whole-body motions with multiple contact changes without priors or initial guesses for contact points, sequences, or timings.Demonstrations include squat jumps, rearing, balancing, rough manipulation, and discovered galloping, trotting, and two-legged walking.Source: §VI · p. 9
  • The approach uses one cost function per task and a task-independent structure, while leveraging full system dynamics rather than heuristics or simplified models.Some motions may serve as initializations for other algorithms or provide insight into gait parameters; cost-function tuning remains manual.Source: §VI · p. 9
  • Optimized trajectories are usually available in less than one minute, compared with reported literature solving times of several minutes or hours.The paper says some motions may not transfer directly to hardware.Source: §VI · p. 9

Limitations

  • Deterministic motions may be vulnerable to model inaccuracies, disturbances, or noise, and SLQ cannot handle state constraints, which may limit complex optimization problems.The authors say hardware execution may require cost-function alterations and that robustness requires considering problem stochasticity.Source: §VI · p. 9

Outlook

  • The authors propose shorter-horizon SLQ-MPC, using warm starts and fewer iterations, with input constraints as a planned next step.They envision combining SLQ with a higher-level planner or optimizer that handles complex state constraints.Source: §VI · p. 9
Loading 1607.04537v1…