The objective of this work is to get a first touch of optimal control The tutorial will guide you to generate a dynamically balanced walk motion for a humanoid robot using a LQR to compute the robot center-of-mass trajectory.
A humanoid robot models, with at least two legs.
An inverse geometry solver based on BFGS.
Yet, the inverse geometry only solves the motion of the robot for a constant task, like reaching a specific position of the hand, or a constant position of the center of mass.
It is possible to modify the BFGS call to perform an inverse kinematics by (i) limiting the number of iteration of BFGS to a small value e.g 10 iterations maximum, (ii) initializing the non-linear search from the previous configuration of the robot, and (iii) turning off the default verbose output of BFGS. For example, the robot can track a target moving vertically using the following example:
Implement a motion of the right foot of the robot tracking a straight line from the initial position of the robot to a position 10cm forward, while keeping a constant rotation of the foot.
The input of the walk generation algorithm is a sequence of steps with a given timing. This input will be represented by two sequences as in the examples below. The class FootSteps
provided here can be used to define, store and access to the footstep plan.
A phase 'none' defines a double support phase (no foot moving). A phase 'left' (resp. 'right') defines a simple support phase while indicating the flying foot. The time is a duration. The position is absolute.
Each interval corresponds to the following constant support phases:
interval | ||
---|---|---|
\(\left[t_0,t_1\right]\) | double support phase | left foot in steps [0], right foot in steps [1] |
\(\left[t_1,t_2\right]\) | left foot support phase | right foot moving to steps [2] |
\(\left[t_2,t_3\right]\) | double support phase, | |
\(\left[t_3,t_4\right]\) | right foot support phase, | left foot moving to steps [3] |
\(\vdots\) | \(\vdots\) | \(\vdots\) |
\(\left[t_{m-2}, t_{m-1}\right]\) | double support phase, | left foot in steps [p-2], right foot in steps [p-1] |
Implement a python class called ZmpRef
that takes as input a sequence of times and a sequence of steps. Objects of this class behave as a function of time that returns a 2 dimensional vector:
The function should be a piecewise affine function
You can use the template below.
For the inputs provided above, the graph of zmp
is given below.
Using the reference zmp trajectory implemented above, implement a class ComRef
that computes the reference trajectory of the center of mass by optimal control.
To write the underlying optimization problem, you can use a factor graph. A simple implementation is available in this file. An example of use is the following. Try to guess the solution before executing it.
Using the same method as in 6.2, implement two classes RightAnkleRef
and LeftAnkleRef
that return reference positions of the ankles as homogeneous matrices. Unlike zmp reference, trajectories of the feet should be continuously differentiable.
Use the classes defined in the previous sections to generate a walk motion using the inverse kinematics solver of Lab 2.