Inverse kinematics worked example

doc/b-examples/i-inverse-kinematics.md

@@ -1,7 +1,84 @@
 # Inverse kinematics (clik)
 
+This example shows how to position the end effector of a manipulator robot to a given position.
+The example employs a simple Jacobian-based iterative algorithm, which is called closed-loop inverse kinematics (CLIK).
+
 ## Python
-\include i-inverse-kinematics.py
+\includelineno i-inverse-kinematics.py
+
+### Explanation of the code
+First of all, we import the necessary libraries and we create the `Model` and `Data` objects:
+\until data
+
+The end effector corresponds to the 6th joint
+\skipline JOINT
+
+and its desired position is given as
+\skipline xdes
+
+Next, we define an initial configuration
+\skipline q
+
+This is the starting point of the algorithm. *A priori*, any valid configuration would work.
+We decided to use the value returned by `neutralConfiguration`.
+For a simple manipulator such as the one in this case, it simply corresponds to an all-zero vector,
+but using this method generalizes well to more complex kinds of robots, ensuring validity.
+
+Next, we set some computation-related values
+\until DT
+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.
+
+Then, we begin the iterative process.
+At each iteration, we begin by computing the end-effector pose for the current configuration value:
+\skip forwardKinematics
+\until R
+
+Next, we compute the error between the desired position and the current one. Notice we chose to express it in the local joint frame:
+\skipline err
+
+If the error norm is below the previously-defined threshold, we have found the solution and we break out of the loop
+\until break
+
+Otherwise, we search for another configuration trying to reduce the error.
+
+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
+\skipline J
+
+Next, we can compute the evolution of the configuration by taking the pseudo-inverse of the Jacobian:
+\skipline v
+
+Finally, we can add the obtained tangent vector to the current configuration
+\skipline q
+
+where `integrate` in our case amounts to a simple sum.
+
+If the loop terminates without ever breaking, it means a solution has not been found.
+We just warn the user
+\skip else
+\until print
+
+Finally, we display the result:
+\skip print
+\until final
 
 ## C++
-\include i-inverse-kinematics.cpp
+The equivalent C++ implemetation is given below
+
+\includelineno i-inverse-kinematics.cpp
+
+### Explanation of the code
+The code follows exactly the same steps as Python.
+We can identify two major differences. 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
+\skipline J(6,model.nv)
+
+\skipline jointJacobian
+
+This allows to always use the same memory space, avoiding re-allocation and achieving greater efficiency.
+
+The second difference consists in the way the velocity is computed
+
+\skipline bdcSvd
+
+This is equivalent to using the pseudo-inverse, but way more efficient.
+

examples/i-inverse-kinematics.cpp

@@ -10,29 +10,37 @@ int main(int argc, char ** argv)
   pinocchio::Data data(model);
 
   const int    JOINT_ID = 6;
-  const double DT       = 1e-1;
+  Eigen::Vector3d xdes;        xdes << 0.5, -0.5, 0.5;  
+
   Eigen::VectorXd q     = pinocchio::neutral(model);
-  Eigen::Vector3d xdes;        xdes << 0.5, -0.5, 0.5;
+  const double eps      = 1e-4;
+  const int IT_MAX      = 1000
+  const double DT       = 1e-1;
 
   pinocchio::Data::Matrix6x J(6,model.nv); J.setZero();
   unsigned int svdOptions = Eigen::ComputeThinU | Eigen::ComputeThinV;
 
-  for (int i=0 ; i<100 ; ++i)
+  for (int i=0 ; i<IT_MAX ; ++i)
     {
-      pinocchio::computeJointJacobians(model,data,q);
-      pinocchio::getJointJacobian(model,data,JOINT_ID,pinocchio::LOCAL,J);
+      pinocchio::forwardKinematics(model,data,q);
       const Eigen::Vector3d & x   = data.oMi[JOINT_ID].translation();
       const Eigen::Matrix3d & R   = data.oMi[JOINT_ID].rotation();
       const Eigen::Vector3d & err = R.transpose()*(x-xdes);
+      if(err.norm() < eps)
+        {
+          std::cout << "Convergence achieved!" << std::endl;
+          break;
+        }
+      pinocchio::jointJacobian(model,data,JOINT_ID,pinocchio::LOCAL,J);
       const Eigen::VectorXd v     = -J.topRows<3>().bdcSvd(svdOptions).solve(err);
       q = pinocchio::integrate(model,q,v*DT);
       if(!(i%10)) std::cout << "error = " << err.transpose() << std::endl;
     }
 
-  // Computing error for final q
-  pinocchio::forwardKinematics(model,data,q);
-  const Eigen::Vector3d & x   = data.oMi[JOINT_ID].translation();
-  const Eigen::Vector3d & err = x-xdes;
+  if(i==IT_MAX)
+    std::cout << "\nWarning: the iterative algorithm has not reached convergence to the desired precision" << std::endl;
+
+  std::cout << "\nresult: " << q.transpose() << std::endl;
   std::cout << "\nfinal error: " << err.transpose() << std::endl;
 
 }

examples/i-inverse-kinematics.py

@@ -1,27 +1,33 @@
-import numpy
+from __future__ import print_function
+
+import numpy as np
 import pinocchio
-from sys import argv
 
 model = pinocchio.buildSampleModelManipulator()
 data  = model.createData()
 
 JOINT_ID = 6
-DT       = 1e-1
+xdes     = np.matrix([ 0.5,-0.5,0.5]).T
+
 q        = model.neutralConfiguration
-xdes     = numpy.matrix([ 0.5,-0.5,0.5]).T
+eps      = 1e-4
+IT_MAX   = 1000
+DT       = 1e-1
 
-for i in range(100):
-    pinocchio.computeJointJacobians(model,data,q)
-    J   = pinocchio.jointJacobian(model,data,q,JOINT_ID,pinocchio.ReferenceFrame.LOCAL,True)[:3,:]
+for i in range(IT_MAX):
+    pinocchio.forwardKinematics(model,data,q)
     x   = data.oMi[JOINT_ID].translation
     R   = data.oMi[JOINT_ID].rotation
     err = R.T*(x-xdes)
-    v   = - numpy.linalg.pinv(J)*err
+    if np.linalg.norm(err) < eps:
+        print("Convergence achieved!")
+        break
+    J   = pinocchio.jointJacobian(model,data,q,JOINT_ID,pinocchio.ReferenceFrame.LOCAL,True)[:3,:]
+    v   = - np.linalg.pinv(J)*err
     q   = pinocchio.integrate(model,q,v*DT)
-    if not i % 10:        print 'error = ', (x-xdes).T
+    if not i % 10:        print('error = %s' % (x-xdes).T)
+else:
+    print("\nWarning: the iterative algorithm has not reached convergence to the desired precision")
 
-# Computing error for final q
-pinocchio.forwardKinematics(model,data,q)
-x   = data.oMi[JOINT_ID].translation
-err = x-xdes
-print '\nfinal error: ', err.T
+print('\nresult: %s' % q.flatten().tolist())
+print('\nfinal error: %s' % (x-xdes).T)