pinocchio  2.3.1-dirty
4-dyn.md
1 # 4) Snap your fingers (aka direct and inverse dynamics)
2 
3 ## Objectives
4 
5 The main objective of the tutorial is to implement a simple torque
6 control inside a home-made contact simulator.
7 
8 ## 4.0) Technical prerequisites
9 
10 ### Robots
11 
12 We are going to use a 4-finger hand, whose model is defined in Python
13 (no urdf model) using capsule volumes. The code of the robot is
14 [available here](robot__hand_8py_source.html). It needs a
15 `Display` class wrapping the Gepetto-viewer client [available here](display_8py_source.html),
16 and contains a `Robot` class implementing the robot hand and a simple example si [available
17 here](hand__example__8py_source.html).
18 
19 ```py
20 from robot_hand import Robot
21 
22 robot = Robot()
23 robot.display(robot.q0)
24 ```
25 
26 Take care that the hand is small: zoom in to see it in the window (or
27 press the space bar).
28 
29 ### Solver
30 
31 We will need a proper QP solver with inequality. QuadProg is a Python
32 wrap of a nice Golub-based solver. Install it with PIP
33 
34 ```
35 pip install --user quadprog
36 ```
37 
38 QuadProg main function is `solve_qp`. You have a bit of documentation
39 using the Python help command `help(solve_qp)`. A simple example
40 follows:
41 
42 ```py
43 from quadprog import solve_qp
44 
45 # Solve min_x .5 xHx - gx s.t. Cx <= d
46 x, _, _, _, _, _ = solve_qp(H, g, C, d)
47 ```
48 
49 ## 4.1. Direct dynamics
50 
51 Choosing an arbitrary joint torque \f$\tau_q\f$, we now compute the robot
52 acceleration and integrate it.
53 
54 The dynamic equation of the robot is \f$M a_q + b = \tau_q\f$, with \f$M\f$ the mass,
55 \f$a_q\f$ the joint acceleration and \f$b\f$ the drift. The mass matrix can be
56 computed using `CRB` algorithm (function of \f$q\f$). The drift is computed
57 using `RNE` algorithm (function of \f$q\f$, \f$v_q\f$ and \f$a_q\f$ with \f$a_q=0\f$).
58 
59 ```py
60 import pinocchio as se3
61 
62 q = rand(robot.model.nq)
63 vq = rand(robot.model.nv)
64 aq0 = zero(robot.model.nv)
65 # compute dynamic drift -- Coriolis, centrifugal, gravity
66 b = se3.rnea(robot.model, robot.data, q, vq, aq0)
67 # compute mass matrix M
68 M = se3.crba(robot.model, robot.data, q)
69 ```
70 
71 These terms correspond to the inverse dynamics. They can be numerically
72 inverted to compute the direct dynamics.
73 
74 #### Question 1
75 Using \f$M\f$ and \f$b\f$ computed by the above algorithms, and knowing
76 a given set of joint torques \f$\tau_q\f$, compute \f$a_q\f$ so that \f$M*a_q+b = \tau_q\f$.
77 
78 Once \f$a_q\f$ as been computed, it is straight forward to integrate it to
79 velocity using \f$v_q += a_q * dt\f$. Integration to joint position is more
80 complex in general. It is implemented in pinocchio:
81 
82 ```py
83 q = se3.integrate(robot.model, q, vq * dt)
84 ```
85 
86 In the particular case of only simple joints (like the robot hand), the same integration
87 \f$q += v_q * dt\f$ also holds.
88 
89 #### Question 2
90 Implement the simulation of the robot hand moving freely
91 with constant (possibly 0) torques. Implement a variation where the
92 torques are only joint friction (\f$\tau_q = -K_f v_q\f$ at each iteration).
93 
94 ## 4.2) PD and computed torques
95 
96 Now choose a reference joint position (possibly time varying, like in
97 the hand example). The joint torques can then be computed to track the
98 desired position, with \f$\tau_q = -K_p (q-q_{des}) - K_v v_q\f$.
99 Both gains \f$K_p\f$ and \f$K_v\f$
100 should be properly chosen. Optimal tracking is obtained with
101 \f$K_v = 2 \sqrt{K_p}\f$. In general, a desired velocity is also tracked to avoid
102 tracking errors.
103 
104 #### Question 3
105 Implement then simulate a PD, by compute the torques from a
106 PD law, then integrate it using the simulator of question 2.
107 
108 Here, there is a strong coupling between joints, due to the mass matrix
109 that is not compensated in the simple PD law. In theory, the computed
110 torques is to compute the joint torque by inverse dynamics from a
111 reference joint acceleration. With boils down to canceling the
112 simulation equation by choosing the proper terms in the control law. It
113 is now very interesting to implement in case of perfect dynamics
114 knowledge. It might be more interesting to study in case the simulation
115 is done with the perfect M, while the control is computed with
116 approximate M (for example, using only the diagonal terms of the mass
117 matrix). Let's rather simulate contact.
118 
119 ## 4.3) Collision checking
120 
121 The robot hand is composed of capsules, i.e. level-set of constant
122 distance to a segment. Collision checking and distances are then easy to
123 implement. The source code of collision checking is available in the
124 [robot_hand.py](robot__hand_8py_source.html) file.
125 Pinocchio also implement a complete and efficient
126 collision checking based on FCL, also not used in the tutorial.
127 
128 Collision checking are done for a set of collision pairs that must be
129 specified to the robot. The collision checking method indeed compute the
130 distance between the two objects, along with the so-called witness
131 points. A method can also be used to display them.
132 
133 ```py
134 from robot_hand import Robot
135 robot = Robot()
136 robot.display(robot.q0)
137 
138 # Create 10 witness points in the rendering window
139 
140 for i in range(10):
141  robot.viewer.viewer.gui.addCylinder('world/wa%i' % i, .01, .003, [1, 0, 0, 1])
142  robot.viewer.viewer.gui.addCylinder('world/wb%i' % i, .01, .003, [1, 0, 0, 1])
143  robot.viewer.viewer.gui.setVisibility('world/wa%i' % i, 'OFF')
144  robot.viewer.viewer.gui.setVisibility('world/wb%i' % i, 'OFF')
145 
146 # Add 4 pairs between finger tips and palm
147 
148 robot.collisionPairs.append([2, 8])
149 robot.collisionPairs.append([2, 11])
150 robot.collisionPairs.append([2, 14])
151 robot.collisionPairs.append([2, 16])
152 
153 # Compute distance between object 2 and 8, i.e the first collision pair
154 
155 idx = 0
156 dist = robot.checkCollision(idx)
157 
158 # Display the collision pair by adding two disks at the witness points.
159 
160 robot.displayCollision(idx, 0)
161 ```
162 
163 The Jacobian of the corresponding pair can be computed using the
164 `collisionJacobian` method
165 
166 ```py
167 J = robot.collisionJacobian(idx, q)
168 ```
169 
170 The jacobian is a 1xN matrix (row
171 matrix) corresponding to the contact normal. Take care that some
172 information are stored in the visual objects when calling
173 checkCollision, that are later used by collisionJacobian. You have to
174 call collisionJacobian right after checkCollision, or the resulting
175 jacobian might not be coherent.
176 
177 For all collision pairs in contact (distance below 1e-3), the Jacobian
178 must be collected and stacked in a single J matrix (which has as many
179 rows as active constraints). Similarly, distances must be stacked in a
180 vector (same number of rows as the jacobian).
181 
182 Now, the joint acceleration is constrained by the contact constraint. It
183 can be written as a minimization problem using Gauss principle
184 
185 \f$min \quad \frac{1}{2} (\ddot q - \ddot q_0 )^T M (\ddot q - \ddot q_0 )\f$
186 
187 \f$s.t. \quad J \ddot q > 0 \f$
188 
189 where \f$\ddot q_0\f$ is the free acceleration, i.e. the acceleration obtained
190 in Question 2 where no constraint is active.
191 
192 In theory, the acceleration should be above the "centrifugal"
193 acceleration (i.e. the acceleration caused by joint velocity only, often
194 written \f$\dot J \dot q\f$) but we neglect it here.
195 
196 In case of penetration or negative velocity, having only position
197 acceleration is not enough. A "trick" is often to require the contact
198 acceleration to be above a proportional depending of the penetration
199 distance: \f$J \ddot q >= -dist\f$, with \f$dist\f$ the vector of stacked
200 distances.
201 
202 #### Question 4
203 Implement a contact simulator using QuadProg, the results
204 of Question 2 and the jacobian matrix of constraints whose distance is
205 below 1e-3.
206 
207 A better solution to avoid penetration is to implement an impact model.
208 The simplest one is the inelastic impact, where normal velocity is
209 simply canceled at impact. For that, remember inactive contact (i.e.
210 those that were not in collision at previous simulation step). When a
211 collision pair is detected that was not previously active, project the
212 current velocity on the null space of all contacts:
213 
214 \f$\dot q = \dot q - J^+ J \dot q\f$
215 
216 #### Question 5
217 
218 The complete loop should be as follows: \f$\tau_q\f$ is computed
219 from a PD tracking a time-varying joint position (question 3). After
220 computing \f$\tau_q\f$, all collision pairs must be checked to find those with
221 distances below 1e-3. Corresponding Jacobians must be computed and
222 stacked. If a new collision as appeared, the joint velocity must be
223 projected to nullify it. If not collision is active, the joint
224 acceleration is computed from inverting the mass matrix (question 2).
225 Otherwise, it is computed using QuadProg (question 4). The resulting
226 acceleration is integrated twice (question 1) before displaying the
227 robot starting a new simulation iteration.