1 # 4) Snap your fingers (aka direct and inverse dynamics)
5 The main objective of the tutorial is to implement a simple torque
6 control inside a home-made contact simulator.
8 ## 4.0) Technical prerequisites
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).
20 from robot_hand import Robot
23 robot.display(robot.q0)
26 Take care that the hand is small: zoom in to see it in the window (or
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
35 pip install --user quadprog
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
43 from quadprog import solve_qp
45 # Solve min_x .5 xHx - gx s.t. Cx <= d
46 x, _, _, _, _, _ = solve_qp(H, g, C, d)
49 ## 4.1. Direct dynamics
51 Choosing an arbitrary joint torque \f$\tau_q\f$, we now compute the robot
52 acceleration and integrate it.
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$).
60 import pinocchio as se3
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)
71 These terms correspond to the inverse dynamics. They can be numerically
72 inverted to compute the direct dynamics.
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$.
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:
83 q = se3.integrate(robot.model, q, vq * dt)
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.
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).
94 ## 4.2) PD and computed torques
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
105 Implement then simulate a PD, by compute the torques from a
106 PD law, then integrate it using the simulator of question 2.
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.
119 ## 4.3) Collision checking
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.
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.
134 from robot_hand import Robot
136 robot.display(robot.q0)
138 # Create 10 witness points in the rendering window
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')
146 # Add 4 pairs between finger tips and palm
148 robot.collisionPairs.append([2, 8])
149 robot.collisionPairs.append([2, 11])
150 robot.collisionPairs.append([2, 14])
151 robot.collisionPairs.append([2, 16])
153 # Compute distance between object 2 and 8, i.e the first collision pair
156 dist = robot.checkCollision(idx)
158 # Display the collision pair by adding two disks at the witness points.
160 robot.displayCollision(idx, 0)
163 The Jacobian of the corresponding pair can be computed using the
164 `collisionJacobian` method
167 J = robot.collisionJacobian(idx, q)
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.
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).
182 Now, the joint acceleration is constrained by the contact constraint. It
183 can be written as a minimization problem using Gauss principle
185 \f$min \quad \frac{1}{2} (\ddot q - \ddot q_0 )^T M (\ddot q - \ddot q_0 )\f$
187 \f$s.t. \quad J \ddot q > 0 \f$
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.
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.
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
203 Implement a contact simulator using QuadProg, the results
204 of Question 2 and the jacobian matrix of constraints whose distance is
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:
214 \f$\dot q = \dot q - J^+ J \dot q\f$
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.