参考资料

Optimization-based Control of Legged Robots

Modeling

definitions: state , control , matrix, fully actuated, under actuated

robot mainpulator

fixed-base robot, with end-effectors, configuration is of joint angles and velocity is of joint velocity

typically each joint is driven by 1 actuator (e.g. electric, hydraulic, pneumatic) and can be modeled as velocity source, acceleration source, torque source and so on. appropriate model depends on robot and task

actuator as velocity source:

actuator as acceleration source:

actuator as torque source:

bias forces decomposed as

nonlinear state-space dynamic

forward dynamics: given to compute , solved by simulators

inverse dynamics: given to compute , solved by controllers

robot in contact

if robot in contact with surrounding, add contact force to dynamics

for point contact, is 1 or 3; for surface contact, is 6, contact Jacobian is given by

where is forward geometry of contact points (mapping joint angles to contact point positions)

rigid contacts constrain motion besides introducing contact force: , i.e. contact points do not move, as we cannot change joint angles instantaneously, so use constraints on velocity or acceleration

introduce acceleration constraints in dynamics

dynamics is linear to acceleration, contact force and torque

forward dynamics with constraints: given to compute and , solved by bilateral rigid contact simulators

inverse dynamics with constraints: given to compute and , assumed satisify constraints

legged robot

floating-base robot, joint angles not enough to describe robot configuration, so add pose (position + orientation) of one link (base) w.r.t inertial frame

where comprising combination of translations (in ) and rotations (in )

elements are represented with:

in this lession, elements are represented as 7d vectors: 3d for position and 4d for orientation (quaternion)

unit quaternion: any 3d rotation is a single rotation by angle about fixed axis

robot configuration is

robot velocity is

angular velocity related to associated rotation matrix by

so and have different size ()

configuration is ordered as , where is passive (unactuated) joints (configuration) and is actuated joints

velocity is ordered as , and selection matrix is

for legged robot, typically all joints are actuated and base is passive,

dynamics of under-actuated mechanical system is

decomposed into unactuated and actuated parts

Joint-Space Control

joint-space inverse dynamics control

for nonlinear mainpulator dynamics

find so that follows reference based on dynamics and measure on and

solution is to set , substituting it to dynamics is , select so that follows :

where are diagonal positive-definite gain matrix, the closd-loop dynamics is

if and are diagonal positive-definite, then is Hurwitz matrix, then which means

the control law

is known as

another variant is (work better):

simpler control laws for mainpulators

both control laws are stable and ensure convergence, in theory, ID control > “PD + gravity” > PID, but in practice, more model errors

inverse dynamics control as optimization problem

optimization problem is defined as as least-squares program/problem (LSP)

with , the solution is exactly ID control law

least-squares program has

so it is a subclass of convex quadratic program (QP), which has

both can be solved extremely fast with off-the-shelf softwares

however, it can result in high acceleration, typically incompatible with torque/current limits -> unfeasible LSP, there are better approaches in papers (not in this course)

Task-Space Control

task-space inverse dynamics

joint-space control needs reference joint trajectory , which is difficult to know when we have reference trajectory for end-effector

option 1: compute joint trajectory corresponding to , then apply joint-space control

where is forward geometry function of end-effector and is a function that , the joint torque is computed as

the issue is the inverse geometry is nonconvex problem with infinitely many solutions, tracking is sufficient but not necessary to track , controller rejects perturbations affecting without affecting

option 2: feedback directly end-effector configuration

desired joint acceleration should be

then compute joint torque as

option 2 is typically preferred: gains defined in Cartesian space, no pre-computations, online specification of reference trajectory but more complex controller

option 2 is written as LSP

task model

task is control objective, described as a function to minimize, assume measures error between real and reference output

where depends on instantaneous state-control value, in optimal control, depends on state-control trajectory

the idea is for a given , we find affine function of and to minimize three kinds of task function :

and are not decision variables in ID LSP, so we need impose dynamics of (e.g. ) which should be affine function of such that

for velocity task-function , impose first-order linear dynamics:

is affine function of

for configuration task-function , impose second-order linear dynamics:

is affine function of

all three kinds of task function end up with affine function of and

under-actuation and contacts

fully-actuated case in task-space inverse dynamics is to find that minimize task function

for under-actuated system,

for under-actuated robots in soft contact, use estimated force

for under-actuated robots in rigid contact, which constrain motion, contact force is also decision variable

we can introduce inequality constraints to optimization problems, if they are affine in :

multi-task control

complex robots are redundant w.r.t task they perform, can use redundancy to execute secondary task

for N tasks, each defined by task function

the simplest strategy is sum functions using user-defined weights (weighted multi-objective optimization)

problem remains LSP, but hard to find weights, too large/small weights lead to numerical issue

alternative approach is order tasks according to priority (hierarchical multi-objective optimization), solve sequence (cascade) of N problems from

it is easier to find priorities than weights, but more computationally expensive to solve several LSPs

computational aspects

TSID solves LSP at each loop, which leads to limited computation time, for DoFs, motors, and contact constraints, there are variables, equality constraints and inequality constraints

idea: exploit problem structure to speed up computation

for equality constraints,

the identity matrix is easy to invert, so express as affine function of other variables

origin problem

is rewritten as

remove variables and equality constraints

more:

but these tricks are either limit expressiveness or lead to small improvements (while making more complex), not worth it

so far, the output (only position), to represent orientation, we need , the elements in are represented using homogeneous matrix and error function is redefined as

where is inverse operation of exponential map

Task-Space Inverse Dynamics Implementation (Joint Space)

begin at Lesson 2 40:31

third party library: TSID - Task Space Inverse Dynamics: TSID is a C++ library for optimization-based inverse-dynamics control based on the rigid multi-body dynamics library Pinocchio.

key concepts in library:

exercise 0 begins from Lesson 2 58:00

set of posture task is equal to critical damping

the optimization is to minimize difference of and , in most time, it does well, but will jump to zero when hit the joint velocity limits

Task-Space Inverse Dynamics Implementation

begin at Lesson 2 1:31:13

key concepts in library:

force-generator matrix : maps force variables to motion constraints representation, in dynamic

for a unilateral plane contact (a rectangle on plane), gives 6d motion constraints, the minimal force representation is 6d (3d force + 3d moment)

exercise 1 UR5 begin at Lesson 2 1:55:35

exercise 1 biped begin at Lesson 2 2:17:00

Trajectory Optimization for Walking

begin at Lesson 3 36:49

TSID is good for tracking trajectory, but no notion of future state, hard to anticipate constraint violations

trajectory optimization is TSID + preview horizon, account for future constraints/cost, but more computationally expensive, so use traj-opt offline to compute reference trajectory and use TSID online to track reference trajectory

options for traj-opt through contacts:

even with fixed contacts, traj-opt is hard because high-dimensional, use simplified models to speed it up, for locomotion: inverted pendulum, linear inverted pendulum and centroidal dynamics (i.e. single rigid body dynamics)

linear inverted pendulum model (LIPM)

Newton-Euler equations

where is center of mass (CoM), is angular momentum expressed at CoM, is gravity acceleration, is i-th contact force, is i-th contact point

assume: contacts with flat ground (); constant angular momentum (); constant CoM height (), then we get

CoP is a weighted average of contact points by contact pressure, in convex polygon of contact points, denoted as , dynamics is rewritten as

CoM acc is given by force pushing CoM away from CoP, is an unstable system, always needs control

rewrite this system as

discretize with time step

CoM Trajectory Optimization wiht LIPM

follow reference trajectory of

foot steps and timing P predefined by user, keep CoP close to foot center for robustness, could be straight line

optimization problem is defined as (assume robot has only one foot at time)

where is foot size in x and y direction and . legged robot can not move at a constant velocity, the real velocity will go up and down slightly, but average tracking reference

foot-step planning

optimize for foot step positions but foot step remains fixed by add into decision variable, bound distance between successive foot steps

usually only need to provide reference velocity

python implementation

begin at Lesson 3 1:56:55

orc/lipm/lipm_ocp.py

connection with TSID

begin at Lesson 3 2:27:33

two issues:

interpolation: given CoM and CoP trajectory with traj-opt time step, output CoM with control time step, the intermediate value is computed by discrete-time dynamics and use CoM and CoP from traj-opt as initial state and control input

foot trajectory is generated by polynomials (in xy plane), e.g. 3rd order with constraints: initial pos, initial vel, (zero) final pos, final vel (zero), for height z, split two trajectory or add more constraints at middle (top) point

code in orc/lipm/lipm_to_tsid.py

code in orc/tsid/biped_walking.py

add constraints in foot step generation orc/lipm/lipm_footstep_ocp.py, in some time steps, is constant