Whole-body dynamic motion planning with centroidal dynamics and full kinematics
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake
Robot Locomotion Group, Massachusetts Institute of Technology
Sep. 18. 2014
Introduction
Linear Inverted Pendulum
•compute ZMP with point-mass model
•linear system, analytical solution
•co-planar contact
•solve kinematics separately
Full body trajectory
optimization
Complexity
Accuracy
Our approach
dynamic constraint for
EVERY degrees of freedom
dynamic constraint for
6 under-actuated DoF
Overview
whole body robot model
admissible contact regions
nonlinear
optimization
solver
robot state trajectory
robot actuator input
contact wrench profile
inverse
dynamics
task constraint
Code can be downloaded from Drake
https://drake.mit.edu
Results
Atlas playing monkey bars
Atlas running
Little Dog running
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Dynamic constraint
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
Consider a robot interacting with the environment, with foot and hand contact. The red dot
r
is the
Center of Mass location.
Dynamic constraint
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
The robot is in contact with the envinronment at point
c
i
, subject to contact wrench
[
F
i
, τ
i
]
and the
gravitational force
mg
at the CoM.
Dynamic constraint
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
The rate of centroidal linear and angular momentum should equal to the total wrench at the CoM.
The rate of centroidal linear momentum is
m
¨
r
. The centroidal angular momentum can be computed
from the robot posture and velocity. [D.E.Orin et al]
m
¨
r
=
X
j
F
j
+
m
g
Newton’s law on CoM acceleration
˙
L
=
X
j
(
c
j
−
r
)
×
F
j
+
τ
j
rate of centroidal angular momentum equals to external torque
r
=
com
(
q
)
compute CoM from posture
L
=
A
(
q
)
v
compute angular momentum from robot state
Kinematic Constraint
Accommodate a variety of kinematic constraints
Position of an end-effector
Orientation of an end-effector
Gaze at a point
Collision avoidance
Quasi-static
Figure:
Solving inverse kinematics problem with different
types of kinematic constraints.
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
Unscheduled contact sequence
Hard to specify the contact sequence when mulitple contact points can be active. Exploit the
complemen-tarity constraint on normal contact force
F
n
and distance to contact
φ
(
c
)
. [M.Posa]
< φ
j
(
c
j
)
,
F
j
n
>
=
0
φ
j
(
c
j
)
≥
0
F
j
n
≥
0
Figure:
Illustration of the contact point
c
j
, its distance
φ
j
to the
contact surface
S
j
, and the local coordinate frame on the
tangential surface, with unit vector
t
x
,
t
y
. The
complementarity condition holds between contact distance
φ
j
and the normal contact force
F
n
j
.
8
9
10
11
12
13
14
l_foot_l_heel
l_foot_r_heel
l_foot_l_toe
l_foot_r_toe
r_foot_l_heel
r_foot_r_heel
r_foot_l_toe
r_foot_l_toe
knot
Contact sequence BEFORE optimizing
with complementarity constraints
active contact
inactive contact
8
9
10
11
12
13
14
knot
Contact sequence AFTER optimizing
with complementarity constraints
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
Go back
Go back
Trajectory optimization
1
Sample the whole trajectory into
N
knot points.
2
In
k
th
point, assign the posture
q
[
k
]
, velocity
v
[
k
]
, contact wrench
F
[
k
]
, τ
[
k
]
and time duration
h
[
k
]
as
decision variables.
3
Solve a nonlinear optimization problem.
min
q
[
k
]
,
v
[
k
]
,
h
[
k
]
r
[
k
]
,
˙r
[
k
]
,
¨r
[
k
]
c
j
[
k
]
,
F
j
[
k
]
,τ
j
[
k
]
L
[
k
]
,
L
˙
[
k
]
N
X
k
=
1
|
q
[
k
]
−
q
nom
[
k
]
|
2
Q
q
+
|
v
[
k
]
|
2
Q
v
+
|
¨r
[
k
]
|
2
+
X
j
(
c
1
F
j
[
k
]
2
+
c
2
|
τ
j
[
k
]
|
2
)
h
[
k
]
s.t
Dynamic constraint
m
¨r
[
k
] =
P
j
F
j
[
k
] +
m
g
˙
L
[
k
] =
P
j
(
c
j
[
k
]
−
r
[
k
])
×
F
j
[
k
] +
τ
j
[
k
]
L
[
k
] =
A
(
q
[
k
])
v
[
k
]
r
[
k
] =
com
(
q
[
k
])
Backward-Euler integration
(
q
[
k
]
−
q
[
k
−
1
] =
v
[
k
]
h
[
k
]
L
[
k
]
−
L
[
k
−
1
] =
L
˙
[
k
]
h
[
k
]
Quadratic interpolation on CoM
(
r
[
k
]
−
r
[
k
−
1
] =
˙r
[
k
]+
˙r
2
[
k
−
1
]
h
[
k
]
˙r
[
k
]
−
˙r
[
k
−
1
] =
¨r
[
k
]
h
[
k
]
Kinematic constraint for contact
(
c
j
[
k
] =
p
j
(
q
[
k
])
c
j
[
k
]
∈ S
j
[
k
]
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
Trajectory optimization
1
Sample the whole trajectory into
N
knot points.
2
In
k
th
point, assign the posture
q
[
k
]
, velocity
v
[
k
]
, contact wrench
F
[
k
]
, τ
[
k
]
and time duration
h
[
k
]
as
decision variables.
3
Solve a nonlinear optimization problem.
min
q
[
k
]
,
v
[
k
]
,
h
[
k
]
r
[
k
]
,
˙r
[
k
]
,
¨r
[
k
]
c
j
[
k
]
,
F
j
[
k
]
,τ
j
[
k
]
L
[
k
]
,
L
˙
[
k
]
N
X
k
=
1
|
q
[
k
]
−
q
nom
[
k
]
|
2
Q
q
+
|
v
[
k
]
|
2
Q
v
+
|
¨r
[
k
]
|
2
+
X
j
(
c
1
F
j
[
k
]
2
+
c
2
|
τ
j
[
k
]
|
2
)
h
[
k
]
s.t
Dynamic constraint
m
¨r
[
k
] =
P
j
F
j
[
k
] +
m
g
˙
L
[
k
] =
P
j
(
c
j
[
k
]
−
r
[
k
])
×
F
j
[
k
] +
τ
j
[
k
]
L
[
k
] =
A
(
q
[
k
])
v
[
k
]
r
[
k
] =
com
(
q
[
k
])
Backward-Euler integration
(
q
[
k
]
−
q
[
k
−
1
] =
v
[
k
]
h
[
k
]
L
[
k
]
−
L
[
k
−
1
] =
L
˙
[
k
]
h
[
k
]
Quadratic interpolation on CoM
(
r
[
k
]
−
r
[
k
−
1
] =
˙r
[
k
]+
˙r
2
[
k
−
1
]
h
[
k
]
˙r
[
k
]
−
˙r
[
k
−
1
] =
¨r
[
k
]
h
[
k
]
Kinematic constraint for contact
(
c
j
[
k
] =
p
j
(
q
[
k
])
c
j
[
k
]
∈ S
j
[
k
]
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
Trajectory optimization
1
Sample the whole trajectory into
N
knot points.
2
In
k
th
point, assign the posture
q
[
k
]
, velocity
v
[
k
]
, contact wrench
F
[
k
]
, τ
[
k
]
and time duration
h
[
k
]
as
decision variables.
3
Solve a nonlinear optimization problem.
min
q
[
k
]
,
v
[
k
]
,
h
[
k
]
r
[
k
]
,
˙r
[
k
]
,
¨r
[
k
]
c
j
[
k
]
,
F
j
[
k
]
,τ
j
[
k
]
L
[
k
]
,
L
˙
[
k
]
N
X
k
=
1
|
q
[
k
]
−
q
nom
[
k
]
|
2
Q
q
+
|
v
[
k
]
|
2
Q
v
+
|
¨r
[
k
]
|
2
+
X
j
(
c
1
F
j
[
k
]
2
+
c
2
|
τ
j
[
k
]
|
2
)
h
[
k
]
s.t
Dynamic constraint
m
¨r
[
k
] =
P
j
F
j
[
k
] +
m
g
˙
L
[
k
] =
P
j
(
c
j
[
k
]
−
r
[
k
])
×
F
j
[
k
] +
τ
j
[
k
]
L
[
k
] =
A
(
q
[
k
])
v
[
k
]
r
[
k
] =
com
(
q
[
k
])
Backward-Euler integration
(
q
[
k
]
−
q
[
k
−
1
] =
v
[
k
]
h
[
k
]
L
[
k
]
−
L
[
k
−
1
] =
L
˙
[
k
]
h
[
k
]
Quadratic interpolation on CoM
(
r
[
k
]
−
r
[
k
−
1
] =
˙r
[
k
]+
˙r
2
[
k
−
1
]
h
[
k
]
˙r
[
k
]
−
˙r
[
k
−
1
] =
¨r
[
k
]
h
[
k
]
Kinematic constraint for contact
(
c
j
[
k
] =
p
j
(
q
[
k
])
c
j
[
k
]
∈ S
j
[
k
]
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
Trajectory optimization
1
Sample the whole trajectory into
N
knot points.
2
In
k
th
point, assign the posture
q
[
k
]
, velocity
v
[
k
]
, contact wrench
F
[
k
]
, τ
[
k
]
and time duration
h
[
k
]
as
decision variables.
3
Solve a nonlinear optimization problem.
min
q
[
k
]
,
v
[
k
]
,
h
[
k
]
r
[
k
]
,
˙r
[
k
]
,
¨r
[
k
]
c
j
[
k
]
,
F
j
[
k
]
,τ
j
[
k
]
L
[
k
]
,
L
˙
[
k
]
N
X
k
=
1
|
q
[
k
]
−
q
nom
[
k
]
|
2
Q
q
+
|
v
[
k
]
|
2
Q
v
+
|
¨r
[
k
]
|
2
+
X
j
(
c
1
F
j
[
k
]
2
+
c
2
|
τ
j
[
k
]
|
2
)
h
[
k
]
s.t
Dynamic constraint
m
¨r
[
k
] =
P
j
F
j
[
k
] +
m
g
˙
L
[
k
] =
P
j
(
c
j
[
k
]
−
r
[
k
])
×
F
j
[
k
] +
τ
j
[
k
]
L
[
k
] =
A
(
q
[
k
])
v
[
k
]
r
[
k
] =
com
(
q
[
k
])
Backward-Euler integration
(
q
[
k
]
−
q
[
k
−
1
] =
v
[
k
]
h
[
k
]
L
[
k
]
−
L
[
k
−
1
] =
L
˙
[
k
]
h
[
k
]
Quadratic interpolation on CoM
(
r
[
k
]
−
r
[
k
−
1
] =
˙r
[
k
]+
˙r
2
[
k
−
1
]
h
[
k
]
˙r
[
k
]
−
˙r
[
k
−
1
] =
¨r
[
k
]
h
[
k
]
Kinematic constraint for contact
(
c
j
[
k
] =
p
j
(
q
[
k
])
c
j
[
k
]
∈ S
j
[
k
]
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
Trajectory optimization
1
Sample the whole trajectory into
N
knot points.
2
In
k
th
point, assign the posture
q
[
k
]
, velocity
v
[
k
]
, contact wrench
F
[
k
]
, τ
[
k
]
and time duration
h
[
k
]
as
decision variables.
3
Solve a nonlinear optimization problem.
min
q
[
k
]
,
v
[
k
]
,
h
[
k
]
r
[
k
]
,
˙r
[
k
]
,
¨r
[
k
]
c
j
[
k
]
,
F
j
[
k
]
,τ
j
[
k
]
L
[
k
]
,
L
˙
[
k
]
N
X
k
=
1
|
q
[
k
]
−
q
nom
[
k
]
|
2
Q
q
+
|
v
[
k
]
|
2
Q
v
+
|
¨r
[
k
]
|
2
+
X
j
(
c
1
F
j
[
k
]
2
+
c
2
|
τ
j
[
k
]
|
2
)
h
[
k
]
s.t
Dynamic constraint
m
¨r
[
k
] =
P
j
F
j
[
k
] +
m
g
˙
L
[
k
] =
P
j
(
c
j
[
k
]
−
r
[
k
])
×
F
j
[
k
] +
τ
j
[
k
]
L
[
k
] =
A
(
q
[
k
])
v
[
k
]
r
[
k
] =
com
(
q
[
k
])
Backward-Euler integration
(
q
[
k
]
−
q
[
k
−
1
] =
v
[
k
]
h
[
k
]
L
[
k
]
−
L
[
k
−
1
] =
L
˙
[
k
]
h
[
k
]
Quadratic interpolation on CoM
(
r
[
k
]
−
r
[
k
−
1
] =
˙r
[
k
]+
˙r
2
[
k
−
1
]
h
[
k
]
˙r
[
k
]
−
˙r
[
k
−
1
] =
¨r
[
k
]
h
[
k
]
Kinematic constraint for contact
(
c
j
[
k
] =
p
j
(
q
[
k
])
c
j
[
k
]
∈ S
j
[
k
]
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
Trajectory optimization
1
Sample the whole trajectory into
N
knot points.
2
In
k
th
point, assign the posture
q
[
k
]
, velocity
v
[
k
]
, contact wrench
F
[
k
]
, τ
[
k
]
and time duration
h
[
k
]
as
decision variables.
3
Solve a nonlinear optimization problem.
min
q
[
k
]
,
v
[
k
]
,
h
[
k
]
r
[
k
]
,
˙r
[
k
]
,
¨r
[
k
]
c
j
[
k
]
,
F
j
[
k
]
,τ
j
[
k
]
L
[
k
]
,
L
˙
[
k
]
N
X
k
=
1
|
q
[
k
]
−
q
nom
[
k
]
|
2
Q
q
+
|
v
[
k
]
|
2
Q
v
+
|
¨r
[
k
]
|
2
+
X
j
(
c
1
F
j
[
k
]
2
+
c
2
|
τ
j
[
k
]
|
2
)
h
[
k
]
s.t
Dynamic constraint
m
¨r
[
k
] =
P
j
F
j
[
k
] +
m
g
˙
L
[
k
] =
P
j
(
c
j
[
k
]
−
r
[
k
])
×
F
j
[
k
] +
τ
j
[
k
]
L
[
k
] =
A
(
q
[
k
])
v
[
k
]
r
[
k
] =
com
(
q
[
k
])
Backward-Euler integration
(
q
[
k
]
−
q
[
k
−
1
] =
v
[
k
]
h
[
k
]
L
[
k
]
−
L
[
k
−
1
] =
L
˙
[
k
]
h
[
k
]
Quadratic interpolation on CoM
(
r
[
k
]
−
r
[
k
−
1
] =
˙r
[
k
]+
˙r
2
[
k
−
1
]
h
[
k
]
˙r
[
k
]
−
˙r
[
k
−
1
] =
¨r
[
k
]
h
[
k
]
Kinematic constraint for contact
(
c
j
[
k
] =
p
j
(
q
[
k
])
c
j
[
k
]
∈ S
j
[
k
]
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
Trajectory optimization
1
Sample the whole trajectory into
N
knot points.
2
In
k
th
point, assign the posture
q
[
k
]
, velocity
v
[
k
]
, contact wrench
F
[
k
]
, τ
[
k
]
and time duration
h
[
k
]
as
decision variables.
3
Solve a nonlinear optimization problem.
min
q
[
k
]
,
v
[
k
]
,
h
[
k
]
r
[
k
]
,
˙r
[
k
]
,
¨r
[
k
]
c
j
[
k
]
,
F
j
[
k
]
,τ
j
[
k
]
L
[
k
]
,
L
˙
[
k
]
N
X
k
=
1
|
q
[
k
]
−
q
nom
[
k
]
|
2
Q
q
+
|
v
[
k
]
|
2
Q
v
+
|
¨r
[
k
]
|
2
+
X
j
(
c
1
F
j
[
k
]
2
+
c
2
|
τ
j
[
k
]
|
2
)
h
[
k
]
s.t
Dynamic constraint
m
¨r
[
k
] =
P
j
F
j
[
k
] +
m
g
˙
L
[
k
] =
P
j
(
c
j
[
k
]
−
r
[
k
])
×
F
j
[
k
] +
τ
j
[
k
]
L
[
k
] =
A
(
q
[
k
])
v
[
k
]
r
[
k
] =
com
(
q
[
k
])
Backward-Euler integration
(
q
[
k
]
−
q
[
k
−
1
] =
v
[
k
]
h
[
k
]
L
[
k
]
−
L
[
k
−
1
] =
L
˙
[
k
]
h
[
k
]
Quadratic interpolation on CoM
(
r
[
k
]
−
r
[
k
−
1
] =
˙r
[
k
]+
2
˙r
[
k
−
1
]
h
[
k
]
˙r
[
k
]
−
˙r
[
k
−
1
] =
¨r
[
k
]
h
[
k
]
Kinematic constraint for contact
(
c
j
[
k
] =
p
j
(
q
[
k
])
c
j
[
k
]
∈ S
j
[
k
]
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
Trajectory optimization
1
Sample the whole trajectory into
N
knot points.
2
In
k
th
point, assign the posture
q
[
k
]
, velocity
v
[
k
]
, contact wrench
F
[
k
]
, τ
[
k
]
and time duration
h
[
k
]
as
decision variables.
3
Solve a nonlinear optimization problem.
min
q
[
k
]
,
v
[
k
]
,
h
[
k
]
r
[
k
]
,
˙r
[
k
]
,
¨r
[
k
]
c
j
[
k
]
,
F
j
[
k
]
,τ
j
[
k
]
L
[
k
]
,
L
˙
[
k
]
N
X
k
=
1
|
q
[
k
]
−
q
nom
[
k
]
|
2
Q
q
+
|
v
[
k
]
|
2
Q
v
+
|
¨r
[
k
]
|
2
+
X
j
(
c
1
F
j
[
k
]
2
+
c
2
|
τ
j
[
k
]
|
2
)
h
[
k
]
s.t
Dynamic constraint
m
¨r
[
k
] =
P
j
F
j
[
k
] +
m
g
˙
L
[
k
] =
P
j
(
c
j
[
k
]
−
r
[
k
])
×
F
j
[
k
] +
τ
j
[
k
]
L
[
k
] =
A
(
q
[
k
])
v
[
k
]
r
[
k
] =
com
(
q
[
k
])
Backward-Euler integration
(
q
[
k
]
−
q
[
k
−
1
] =
v
[
k
]
h
[
k
]
L
[
k
]
−
L
[
k
−
1
] =
L
˙
[
k
]
h
[
k
]
Quadratic interpolation on CoM
(
r
[
k
]
−
r
[
k
−
1
] =
˙r
[
k
]+
2
˙r
[
k
−
1
]
h
[
k
]
˙r
[
k
]
−
˙r
[
k
−
1
] =
¨r
[
k
]
h
[
k
]
Kinematic constraint for contact
(
c
j
[
k
] =
p
j
(
q
[
k
])
c
j
[
k
]
∈ S
j
[
k
]
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
Monkey bars
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
Go back
Go back
Atlas running
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
Monkey bars
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
Go back
Go back
Little dog running
Playback in 1/8x speed
Playback in real speed.
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
Little dog walking
Side view
Front view
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
Little dog bounding
Playback in 1/4x speed
Play back in real speed.
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
Whole-body dynamic motion planning with centroidal dynamics and full kinematics
Little dog rotary
Playback in 1/4x speed
Play back in real speed.
Hongkai Dai, Andr ´es Valenzuela and Russ Tedrake Robot Locomotion Group, Massachusetts Institute of Technology
