pinocchio  2.4.5
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
doc/d-practical-exercises/6-wpg.md
1 # 6) Take a walk (aka optimal control)
2 
3 ## Objective
4 
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.
9 
10 ## 6.0) prerequesites
11 
12 ### Prerequesite 1
13 A humanoid robot models, with at least two legs.
14 
15 ### Prerequesite 2
16 An inverse geometry solver based on BFGS.
17 
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.
21 
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:
28 
29 ```py
30 cost.Mdes = se3.SE3(eye(3), np.matrix([0.2, 0, 0.1 + t / 100.])) # Reference target at time 0.
31 q = np.copy(robot.q0)
32 for t in range(100):
33  cost.Mdes.translation = np.matrix([0.2, 0, 0.1 + t / 100.])
34  q = fmin_bfgs(cost, q, maxiter=10, disp=False)
35  robot.display(q)
36 ```
37 
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.
41 
42 ## 6.1) defining input
43 
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.
48 
49 ```py
50 # Define 6 steps forward, starting with the left foot and stoping at the same forward position.
51 
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')
66 ```
67 
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.
71 
72 Each interval corresponds to the following constant support phases:
73 
74 interval |   |  
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]
82 
83 ```py
84 # Example of use
85 
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
94 ```
95 
96 ## 6.2) computing reference ZMP
97 
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:
101 
102 ```py
103 zmp = ZmpRef(footsteps)
104 zmp(2.5)
105 array([0.41, 0.096])
106 ```
107 
108 The function should be a piecewise affine function
109 
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.
113 
114 You can use the template below.
115 
116 ```py
117 class ZmpRef(object):
118  def __init__(self, footsteps):
119  self.footsteps = footsteps
120 
121  def __call__(self, t):
122  return np.array(self.footsteps[0])
123 ```
124 
125 For the inputs provided above, the graph of `zmp` is given below.
126 
127 
128 ## 6.3) reference trajectory of the center of mass
129 
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.
133 
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.
138 
139 ```py
140 f = FactorGraph(1, 5) # Define a factor of 5 variables of dimension 1
141 
142 M = eye(1) # M is simply 1 written as a 1x1 matrix.
143 for i in range(4):
144  f.add_factor_constraint([Factor(i, M), Factor(i + 1, -M)], zero(1))
145 
146 f.addFactor([Factor(0, M)], M * 10)
147 f.addFactor([Factor(4, M)], M * 20)
148 
149 x = f.solve()
150 ```
151 
152 ## 6.4) reference trajectories of the feet
153 
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.
158 
159 ## 6.5) generate walk motion
160 
161 Use the classes defined in the previous sections to generate a walk
162 motion using the inverse kinematics solver of Lab 2.