pinocchio  2.2.1-dirty
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

1 from __future__ import print_function
2 
3 import numpy as np
4 import pinocchio
5 pinocchio.switchToNumpyMatrix()
6 
7 model = pinocchio.buildSampleModelManipulator()
8 data = model.createData()
9 
10 JOINT_ID = 6
11 xdes = np.matrix([ 0.5,-0.5,0.5]).T
12 
13 q = pinocchio.neutral(model)
14 eps = 1e-4
15 IT_MAX = 1000
16 DT = 1e-1
17 
18 i=0
19 while True:
20  pinocchio.forwardKinematics(model,data,q)
21  x = data.oMi[JOINT_ID].translation
22  R = data.oMi[JOINT_ID].rotation
23  err = R.T*(x-xdes)
24  if np.linalg.norm(err) < eps:
25  print("Convergence achieved!")
26  break
27  if i >= IT_MAX:
28  print("\nWarning: the iterative algorithm has not reached convergence to the desired precision")
29  break
30  J = pinocchio.computeJointJacobian(model,data,q,JOINT_ID)[:3,:]
31  v = - np.linalg.pinv(J)*err
32  q = pinocchio.integrate(model,q,v*DT)
33  if not i % 10: print('error = %s' % err.T)
34  i += 1
35 
36 print('\nresult: %s' % q.flatten().tolist())
37 print('\nfinal error: %s' % err.T)
Eigen::Matrix< typename LieGroupCollection::Scalar, Eigen::Dynamic, 1, LieGroupCollection::Options > neutral(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the neutral element of it.
void integrate(const LieGroupGenericTpl< LieGroupCollection > &lg, const Eigen::MatrixBase< ConfigIn_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< ConfigOut_t > &qout)
Visit a LieGroupVariant to call its integrate method.
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a)
Update the joint placements, spatial velocities and spatial accelerations according to the current jo...
void computeJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const JointIndex jointId, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed in the local frame of the joint and store t...

Explanation of the code

First of all, we import the necessary libraries and we create the Model and Data objects:

1 from __future__ import print_function
2 
3 import numpy as np
4 import pinocchio
5 pinocchio.switchToNumpyMatrix()
6 
7 model = pinocchio.buildSampleModelManipulator()
8 data = model.createData()

The end effector corresponds to the 6th joint

1 JOINT_ID = 6

and its desired position is given as

1 xdes = np.matrix([ 0.5,-0.5,0.5]).T

Next, we define an initial configuration

1 q = pinocchio.neutral(model)

This is the starting point of the algorithm. A priori, any valid configuration would work. We decided to use the robot's neutral configuration, returned by algorithm neutral. 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

1 eps = 1e-4
2 IT_MAX = 1000
3 DT = 1e-1
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:

1  pinocchio.forwardKinematics(model,data,q)
2  x = data.oMi[JOINT_ID].translation
3  R = data.oMi[JOINT_ID].rotation

Next, we compute the error between the desired position and the current one. Notice we chose to express it in the local joint frame:

1  err = R.T*(x-xdes)

If the error norm is below the previously-defined threshold, we have found the solution and we break out of the loop

1  if np.linalg.norm(err) < eps:
2  print("Convergence achieved!")
3  break

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

1  if i >= IT_MAX:
2  print("\nWarning: the iterative algorithm has not reached convergence to the desired precision")
3  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

1  J = pinocchio.computeJointJacobian(model,data,q,JOINT_ID)[:3,:]

Next, we can compute the evolution of the configuration by taking the pseudo-inverse of the Jacobian:

1  v = - np.linalg.pinv(J)*err

Finally, we can add the obtained tangent vector to the current configuration

1  q = pinocchio.integrate(model,q,v*DT)

where integrate in our case amounts to a simple sum. The resulting error will be verified in the next iteration.

At the end of the loop, we display the result:

1 print('\nresult: %s' % q.flatten().tolist())
2 print('\nfinal error: %s' % err.T)

C++

The equivalent C++ implemetation is given below

1 #include "pinocchio/parsers/sample-models.hpp"
2 #include "pinocchio/algorithm/kinematics.hpp"
3 #include "pinocchio/algorithm/jacobian.hpp"
4 #include "pinocchio/algorithm/joint-configuration.hpp"
5 
6 int main(int /* argc */, char ** /* argv */)
7 {
8  pinocchio::Model model;
10  pinocchio::Data data(model);
11 
12  const int JOINT_ID = 6;
13  Eigen::Vector3d xdes; xdes << 0.5, -0.5, 0.5;
14 
15  Eigen::VectorXd q = pinocchio::neutral(model);
16  const double eps = 1e-4;
17  const int IT_MAX = 1000;
18  const double DT = 1e-1;
19 
20  pinocchio::Data::Matrix6x J(6,model.nv); J.setZero();
21  unsigned int svdOptions = Eigen::ComputeThinU | Eigen::ComputeThinV;
22  Eigen::BDCSVD<pinocchio::Data::Matrix3x> svd(3,model.nv,svdOptions);
23 
24  Eigen::Vector3d err;
25  for (int i=0;;i++)
26  {
27  pinocchio::forwardKinematics(model,data,q);
28  const Eigen::Vector3d & x = data.oMi[JOINT_ID].translation();
29  const Eigen::Matrix3d & R = data.oMi[JOINT_ID].rotation();
30  err = R.transpose()*(x-xdes);
31  if(err.norm() < eps)
32  {
33  std::cout << "Convergence achieved!" << std::endl;
34  break;
35  }
36  if (i >= IT_MAX)
37  {
38  std::cout << "\nWarning: the iterative algorithm has not reached convergence to the desired precision" << std::endl;
39  break;
40  }
41  pinocchio::computeJointJacobian(model,data,q,JOINT_ID,J);
42  const Eigen::VectorXd v = - svd.compute(J.topRows<3>()).solve(err);
43  q = pinocchio::integrate(model,q,v*DT);
44  if(!(i%10)) std::cout << "error = " << err.transpose() << std::endl;
45  }
46 
47  std::cout << "\nresult: " << q.transpose() << std::endl;
48  std::cout << "\nfinal error: " << err.transpose() << std::endl;
49 }
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time...
int nv
Dimension of the velocity vector space.
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: data.hpp:68
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
void computeJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const JointIndex jointId, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed in the local frame of the joint and store t...
void manipulator(ModelTpl< Scalar, Options, JointCollectionTpl > &model)
Create a 6-DOF kinematic chain shoulder-elbow-wrist.

Explanation of the code

The code follows exactly the same steps as Python. Apart from the usual syntactic discrepancies between Python and C++, 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

1  pinocchio::Data::Matrix6x J(6,model.nv); J.setZero();

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

1  unsigned int svdOptions = Eigen::ComputeThinU | Eigen::ComputeThinV;
2  Eigen::BDCSVD<pinocchio::Data::Matrix3x> svd(3,model.nv,svdOptions);
1  const Eigen::VectorXd v = - svd.compute(J.topRows<3>()).solve(err);

This is equivalent to using the pseudo-inverse, but way more efficient. Also notice we have chosen to pre-allocate the space for the SVD decomposition instead of using method bdcSvd(svdOptions).