1 # 6) Take a walk (aka optimal control)
5 The objective of this work is to get a first touch of optimal control
6 The tutorial will guide you to generate a dynamically balanced walk
7 motion for a humanoid robot using a LQR to compute the robot
8 center-of-mass trajectory.
13 A humanoid robot models, with at least two legs.
16 An inverse geometry solver based on BFGS.
18 Yet, the inverse geometry only solves the motion of the robot for a
19 constant task, like reaching a specific position of the hand, or a
20 constant position of the center of mass.
22 It is possible to modify the BFGS call to perform an inverse kinematics
23 by (i) limiting the number of iteration of BFGS to a small value e.g 10
24 iterations maximum, (ii) initializing the non-linear search from the
25 previous configuration of the robot, and (iii) turning off the default
26 verbose output of BFGS. For example, the robot can track a target moving
27 vertically using the following example:
30 cost.Mdes = se3.SE3(eye(3), np.matrix([0.2, 0, 0.1 + t / 100.])) # Reference target at time 0.
33 cost.Mdes.translation = np.matrix([0.2, 0, 0.1 + t / 100.])
34 q = fmin_bfgs(cost, q, maxiter=10, disp=False)
38 Implement a motion of the right foot of the robot tracking a straight
39 line from the initial position of the robot to a position 10cm forward,
40 while keeping a constant rotation of the foot.
42 ## 6.1) defining input
44 The input of the walk generation algorithm is a sequence of steps with a
45 given timing. This input will be represented by two sequences as in the
46 examples below. The class `FootSteps` [provided here](foot__steps_8py_source.html)
47 can be used to define, store and access to the footstep plan.
50 # Define 6 steps forward, starting with the left foot and stoping at the same forward position.
52 footsteps = FootSteps([.0, -.1] ,[.0, .1])
53 footsteps.add_phase(.3, 'none')
54 footsteps.add_phase(.7, 'left', [.1, .1])
55 footsteps.add_phase(.1, 'none')
56 footsteps.add_phase(.7, 'right', [.2, -.1])
57 footsteps.add_phase(.1, 'none')
58 footsteps.add_phase(.7, 'left', [.3, .1])
59 footsteps.add_phase(.1, 'none')
60 footsteps.add_phase(.7, 'right', [.4, -.1])
61 footsteps.add_phase(.1, 'none')
62 footsteps.add_phase(.7, 'left', [.5, .1])
63 footsteps.add_phase(.1, 'none')
64 footsteps.add_phase(.7, 'right', [.5, -.1])
65 footsteps.add_phase(.5, 'none')
68 A phase 'none' defines a double support phase (no foot moving). A phase
69 'left' (resp. 'right') defines a simple support phase while indicating
70 the flying foot. The time is a duration. The position is absolute.
72 Each interval corresponds to the following constant support phases:
75 ----------------------------------- | ------------------------- | ----------------------------------------------------
76 \f$\left[t_0,t_1\right]\f$ | double support phase | left foot in steps [0], right foot in steps [1]
77 \f$\left[t_1,t_2\right]\f$ | left foot support phase | right foot moving to steps [2]
78 \f$\left[t_2,t_3\right]\f$ | double support phase, |
79 \f$\left[t_3,t_4\right]\f$ | right foot support phase, | left foot moving to steps [3]
80 \f$\vdots\f$ | \f$\vdots\f$ | \f$\vdots\f$
81 \f$\left[t_{m-2}, t_{m-1}\right]\f$ | double support phase, | left foot in steps [p-2], right foot in steps [p-1]
86 footsteps.get_phase_type(.4) # return 'left'
87 footsteps.get_left_position(0.4) # return 0,0.1
88 footsteps.get_left_next_position(0.4) # return 0.1,0.1
89 footsteps.get_phase_start(0.4) # return 0.3
90 footsteps.get_phase_duration(0.4) # return 0.7
91 footsteps.get_phase_remaining(0.4) # return 0.6
92 footsteps.is_double_from_left_to_right(0) # return False
93 footsteps.is_double_from_left_to_right(1) # return True
96 ## 6.2) computing reference ZMP
98 Implement a python class called `ZmpRef` that takes as input a sequence
99 of times and a sequence of steps. Objects of this class behave as a
100 function of time that returns a 2 dimensional vector:
103 zmp = ZmpRef(footsteps)
108 The function should be a piecewise affine function
110 - starting in the middle of the ankles of the two first steps,
111 - finishing in the middle of the two ankles of the two last steps,
112 - constant under the foot support during single support phases.
114 You can use the template below.
117 class ZmpRef(object):
118 def __init__(self, footsteps):
119 self.footsteps = footsteps
121 def __call__(self, t):
122 return np.array(self.footsteps[0])
125 For the inputs provided above, the graph of `zmp` is given below.
128 ## 6.3) reference trajectory of the center of mass
130 Using the reference zmp trajectory implemented above,
131 implement a class `ComRef` that computes the reference trajectory of the
132 center of mass by optimal control.
134 To write the underlying optimization problem, you can use a factor
135 graph. A simple implementation is available in
136 [this file](factor_8py_source.html). An example of use is
137 the following. Try to guess the solution before executing it.
140 f = FactorGraph(1, 5) # Define a factor of 5 variables of dimension 1
142 M = eye(1) # M is simply 1 written as a 1x1 matrix.
144 f.add_factor_constraint([Factor(i, M), Factor(i + 1, -M)], zero(1))
146 f.addFactor([Factor(0, M)], M * 10)
147 f.addFactor([Factor(4, M)], M * 20)
152 ## 6.4) reference trajectories of the feet
154 Using the same method as in 6.2, implement two classes
155 `RightAnkleRef` and `LeftAnkleRef` that return reference positions of
156 the ankles as homogeneous matrices. Unlike zmp reference, trajectories
157 of the feet should be continuously differentiable.
159 ## 6.5) generate walk motion
161 Use the classes defined in the previous sections to generate a walk
162 motion using the inverse kinematics solver of Lab 2.