pinocchio  2.3.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 pose (position and orientation).
4 The example employs a simple Jacobian-based iterative algorithm, which is called closed-loop inverse kinematics (CLIK).
5 
6 ## Python
7 \includelineno 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 pose is given as
17 \skipline oMdes
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 damp
29 corresponding to the desired position precision (we will see later what it exactly means),
30 a maximum number of iterations (to avoid infinite looping in case the position is not reachable),
31 a positive "time step" defining the convergence rate,
32 and a fixed damping factor for the pseudoinversion (see below).
33 
34 Then, we begin the iterative process.
35 At each iteration, we begin by computing the forward kinematics:
36 \skipline forwardKinematics
37 
38 Next, we compute the error between the desired pose and the current one.
39 \skip dMi
40 \until err
41 Here, `data.oMi[JOINT_ID]` corresponds to the placement of the sixth joint (previously computed by `forwardKinematics`),
42 `dMi` corresponds to the transformation between the desired pose and the current one, and `err` is the error.
43 In order to compute the error, we employed `log`: this is a `Motion` object, and it is one way of computing an error in \f$SO(3)\f$ as a six-dimensional vector.
44 
45 If the error norm is below the previously-defined threshold, we have found the solution and we break out of the loop
46 \until break
47 Notice that, strictly speaking, the norm of a spatial velocity does not make physical sense, since it mixes linear and angular quantities.
48 A more rigorous implementation should treat the linar part and the angular part separately.
49 In this example, however, we choose to slightly abuse the notation in order to keep it simple.
50 
51 If we have reached the maximum number of iterations, it means a solution has not been found. We take notice of the failure and we also break
52 \until break
53 
54 Otherwise, we search for another configuration trying to reduce the error.
55 
56 We start by computing the Jacobian.
57 \skipline J
58 
59 Next, we can compute the evolution of the configuration by solving the inverse kinematics.
60 In order to avoid problems at singularities, we employ the damped pseudo-inverse:
61 \f$v = - J^T (J J^T + \lambda I)^{-1} e\f$
62 implementing the equation as
63 \skipline v
64 Notice that this way to compute the damped pseudo-inverse was chosen mostly because of its simplicity of implementation.
65 It is not necessarily the best nor the fastest way,
66 and using a fixed damping factor \f$\lambda\f$ is not necessarily the best course of action.
67 
68 Finally, we can add the obtained tangent vector to the current configuration
69 \skipline q
70 
71 where `integrate` in our case amounts to a simple sum. The resulting error will be verified in the next iteration.
72 
73 At the end of the loop, we display the result:
74 \skip success
75 \until final
76 
77 ## C++
78 The equivalent C++ implemetation is given below
79 
80 \includelineno inverse-kinematics.cpp
81 
82 ### Explanation of the code
83 The code follows exactly the same steps as Python.
84 Apart from the usual syntactic discrepancies between Python and C++, we can identify two major differences.
85 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
86 \skip J(6,model.nv)
87 \until setZero
88 
89 \skip computeJointJacobian
90 \line computeJointJacobian
91 
92 This allows to always use the same memory space, avoiding re-allocation and achieving greater efficiency.
93 
94 The second difference consists in the way the velocity is computed
95 
96 \skip JJt
97 \until solve
98 
99 This code is longer than the Python version, but equivalent to it. Notice we explicitly employ the `ldlt` decomposition.