pinocchio  2.2.1-dirty
i-inverse-kinematics.md
1 # Inverse kinematics (clik)
2 
3 This example shows how to position the end effector of a manipulator robot to a given position.
4 The example employs a simple Jacobian-based iterative algorithm, which is called closed-loop inverse kinematics (CLIK).
5 
6 ## Python
7 \includelineno i-inverse-kinematics.py
8 
9 ### Explanation of the code
10 First of all, we import the necessary libraries and we create the `Model` and `Data` objects:
11 \until data
12 
13 The end effector corresponds to the 6th joint
14 \skipline JOINT
15 
16 and its desired position is given as
17 \skipline xdes
18 
19 Next, we define an initial configuration
20 \skipline q
21 
22 This is the starting point of the algorithm. *A priori*, any valid configuration would work.
23 We decided to use the robot's neutral configuration, returned by algorithm `neutral`.
24 For a simple manipulator such as the one in this case, it simply corresponds to an all-zero vector,
25 but using this method generalizes well to more complex kinds of robots, ensuring validity.
26 
27 Next, we set some computation-related values
28 \until DT
29 corresponding to the desired position precision (a tenth of a millimeter will do), a maximum number of iterations (to avoid infinite looping in case the position is not reachable) and a positive "time step" defining the convergence rate.
30 
31 Then, we begin the iterative process.
32 At each iteration, we begin by computing the end-effector pose for the current configuration value:
33 \skip forwardKinematics
34 \until R
35 
36 Next, we compute the error between the desired position and the current one. Notice we chose to express it in the local joint frame:
37 \skipline err
38 
39 If the error norm is below the previously-defined threshold, we have found the solution and we break out of the loop
40 \until break
41 
42 If we have reached the maximum number of iterations, it means a solution has not been found. We print an error message and we also break
43 \until break
44 
45 Otherwise, we search for another configuration trying to reduce the error.
46 
47 We start by computing the Jacobian, also in the local joint frame. Since we are only interested in the position, and not in the orientation, we select the first three lines, corresponding to the translation part
48 \skipline J
49 
50 Next, we can compute the evolution of the configuration by taking the pseudo-inverse of the Jacobian:
51 \skipline v
52 
53 Finally, we can add the obtained tangent vector to the current configuration
54 \skipline q
55 
56 where `integrate` in our case amounts to a simple sum. The resulting error will be verified in the next iteration.
57 
58 At the end of the loop, we display the result:
59 \skip result
60 \until final
61 
62 ## C++
63 The equivalent C++ implemetation is given below
64 
65 \includelineno i-inverse-kinematics.cpp
66 
67 ### Explanation of the code
68 The code follows exactly the same steps as Python.
69 Apart from the usual syntactic discrepancies between Python and C++, we can identify two major differences.
70 The first one concerns the Jacobian computation. In C++, you need to pre-allocate its memory space, set it to zero, and pass it as an input
71 \skipline J(6,model.nv)
72 \skipline jointJacobian
73 
74 This allows to always use the same memory space, avoiding re-allocation and achieving greater efficiency.
75 
76 The second difference consists in the way the velocity is computed
77 
78 \dontinclude i-inverse-kinematics.cpp
79 \skip svdOptions
80 \until BDCSVD
81 \skipline svd.compute
82 
83 This is equivalent to using the pseudo-inverse, but way more efficient.
84 Also notice we have chosen to pre-allocate the space for the SVD decomposition instead of using method `bdcSvd(svdOptions)`.
85