Whole-body dynamic motion planning with centroidal dynamics and full kinematics

Transcription

1 Sep Introduction Linear Inverted Pendulum compute ZMP with point-mass model linear system, analytical solution co-planar contact solve kinematics separately Our approach dynamic constraint for 6 under-actuated DoF Full body trajectory optimization dynamic constraint for EVERY degrees of freedom Accuracy Complexity Overview task constraint nonlinear robot state trajectory whole body robot model optimization solver contact wrench profile inverse dynamics robot actuator input admissible contact regions Code can be downloaded from Drake Results Atlas playing monkey bars Atlas running Little Dog running

2 Dynamic constraint Consider a robot interacting with the environment, with foot and hand contact. The red dot r is the Center of Mass location.

3 Dynamic constraint The robot is in contact with the envinronment at point c, subject to contact wrench [F, τ ] and the i i i gravitational force mg at the CoM.

4 Dynamic constraint 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 = j F j + mg Newton s law on CoM acceleration L = j (c j r) F j + τ j rate of centroidal angular momentum equals to external torque r = com(q) L = A(q)v compute CoM from posture compute angular momentum from robot state

5 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.

6 Unscheduled contact sequence Hard to specify the contact sequence when mulitple contact points can be active. Exploit the complementarity constraint on normal contact force F n and distance to contact φ(c). [M.Posa] < φ j (c j ), F n j >= 0 φ j (c j ) 0 F n j 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. r_foot_l_toe r_foot_l_toe Contact sequence BEFORE optimizing with complementarity constraints active contact inactive contact Contact sequence AFTER optimizing with complementarity constraints r_foot_r_heel r_foot_l_heel l_foot_r_toe l_foot_l_toe l_foot_r_heel l_foot_l_heel knot knot

7 Trajectory optimization 1 Sample the whole trajectory into N knot points.

8 Trajectory optimization Sample the whole trajectory into N knot points. 1 In k th point, assign the posture q[k], velocity v[k], contact wrench F[k], τ[k] and time duration h[k] as 2 decision variables.

9 Trajectory optimization Sample the whole trajectory into N knot points. 1 In k th point, assign the posture q[k], velocity v[k], contact wrench F[k], τ[k] and time duration h[k] as 2 decision variables. Solve a nonlinear optimization problem. 3

10 Trajectory optimization Sample the whole trajectory into N knot points. 1 In k th point, assign the posture q[k], velocity v[k], contact wrench F[k], τ[k] and time duration h[k] as 2 decision variables. Solve a nonlinear optimization problem. 3 N min q[k],v[k],h[k] r[k],ṙ[k], r[k] c j [k],f j [k],τ j [k] k=1 q[k] q nom [k] 2 Q q + v[k] 2 Q v + r[k] 2 + j (c Fj 1 [k] 2 + c 2 τ [k] 2 ) h[k] j L[k], L[k]

11 Trajectory optimization Sample the whole trajectory into N knot points. 1 In k th point, assign the posture q[k], velocity v[k], contact wrench F[k], τ[k] and time duration h[k] as 2 decision variables. Solve a nonlinear optimization problem. 3 N min q[k],v[k],h[k] r[k],ṙ[k], r[k] c j [k],f j [k],τ j [k] k=1 q[k] q nom [k] 2 Q q + v[k] 2 Q v + r[k] 2 + j (c Fj 1 [k] 2 + c 2 τ [k] 2 ) h[k] j L[k], L[k] m r[k] = j F j[k] + mg L[k] = j (c j[k] r[k]) F j [k] + τ j [k] s.t Dynamic constraint L[k] = A(q[k])v[k] r[k] = com(q[k])

12 Trajectory optimization Sample the whole trajectory into N knot points. 1 In k th point, assign the posture q[k], velocity v[k], contact wrench F[k], τ[k] and time duration h[k] as 2 decision variables. Solve a nonlinear optimization problem. 3 N min q[k],v[k],h[k] r[k],ṙ[k], r[k] c j [k],f j [k],τ j [k] k=1 q[k] q nom [k] 2 Q q + v[k] 2 Q v + r[k] 2 + j (c Fj 1 [k] 2 + c 2 τ [k] 2 ) h[k] j L[k], L[k] m r[k] = j F j[k] + mg L[k] = j (c j[k] r[k]) F j [k] + τ j [k] s.t Dynamic constraint L[k] = A(q[k])v[k] r[k] = com(q[k]) { q[k] q[k 1] = v[k]h[k] Backward-Euler integration L[k] L[k 1] = L[k]h[k]

13 Trajectory optimization Sample the whole trajectory into N knot points. 1 In k th point, assign the posture q[k], velocity v[k], contact wrench F[k], τ[k] and time duration h[k] as 2 decision variables. Solve a nonlinear optimization problem. 3 N min q[k],v[k],h[k] r[k],ṙ[k], r[k] c j [k],f j [k],τ j [k] k=1 q[k] q nom [k] 2 Q q + v[k] 2 Q v + r[k] 2 + j (c Fj 1 [k] 2 + c 2 τ [k] 2 ) h[k] j L[k], L[k] m r[k] = j F j[k] + mg L[k] = j (c j[k] r[k]) F j [k] + τ j [k] s.t Dynamic constraint L[k] = A(q[k])v[k] r[k] = com(q[k]) { q[k] q[k 1] = v[k]h[k] Backward-Euler integration Quadratic interpolation on CoM L[k] L[k 1] = L[k]h[k] { r[k] r[k 1] = ṙ[k]+ṙ[k 1] h[k] 2 ṙ[k] ṙ[k 1] = r[k]h[k]

14 Trajectory optimization Sample the whole trajectory into N knot points. 1 In k th point, assign the posture q[k], velocity v[k], contact wrench F[k], τ[k] and time duration h[k] as 2 decision variables. Solve a nonlinear optimization problem. 3 N min q[k],v[k],h[k] r[k],ṙ[k], r[k] c j [k],f j [k],τ j [k] k=1 q[k] q nom [k] 2 Q q + v[k] 2 Q v + r[k] 2 + j (c Fj 1 [k] 2 + c 2 τ [k] 2 ) h[k] j L[k], L[k] m r[k] = j F j[k] + mg L[k] = j (c j[k] r[k]) F j [k] + τ j [k] s.t Dynamic constraint L[k] = A(q[k])v[k] r[k] = com(q[k]) { q[k] q[k 1] = v[k]h[k] Backward-Euler integration Quadratic interpolation on CoM L[k] L[k 1] = L[k]h[k] { r[k] r[k 1] = ṙ[k]+ṙ[k 1] h[k] 2 ṙ[k] ṙ[k 1] = r[k]h[k] { c j [k] = p j (q[k]) Kinematic constraint for contact c j [k] S j [k]

15 Monkey bars

16 Atlas running

17 Monkey bars

18 Little dog running Playback in 1/8x speed Playback in real speed.

19 Little dog walking Side view Front view

20 Little dog bounding Playback in 1/4x speed Play back in real speed.

21 Little dog rotary Playback in 1/4x speed Play back in real speed.