pinocchio  2.6.17
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
Inverse kinematics (clik)

This example shows how to position the end effector of a manipulator robot to a given pose (position and orientation). 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 from numpy.linalg import norm, solve
5 
6 import pinocchio
7 
8 model = pinocchio.buildSampleModelManipulator()
9 data = model.createData()
10 
11 JOINT_ID = 6
12 oMdes = pinocchio.SE3(np.eye(3), np.array([1., 0., 1.]))
13 
14 q = pinocchio.neutral(model)
15 eps = 1e-4
16 IT_MAX = 1000
17 DT = 1e-1
18 damp = 1e-12
19 
20 i=0
21 while True:
22  pinocchio.forwardKinematics(model,data,q)
23  dMi = oMdes.actInv(data.oMi[JOINT_ID])
24  err = pinocchio.log(dMi).vector
25  if norm(err) < eps:
26  success = True
27  break
28  if i >= IT_MAX:
29  success = False
30  break
31  J = pinocchio.computeJointJacobian(model,data,q,JOINT_ID)
32  v = - J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))
33  q = pinocchio.integrate(model,q,v*DT)
34  if not i % 10:
35  print('%d: error = %s' % (i, err.T))
36  i += 1
37 
38 if success:
39  print("Convergence achieved!")
40 else:
41  print("\nWarning: the iterative algorithm has not reached convergence to the desired precision")
42 
43 print('\nresult: %s' % q.flatten().tolist())
44 print('\nfinal error: %s' % err.T)

Explanation of the code

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

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

The end effector corresponds to the 6th joint

9 JOINT_ID = 6

and its desired pose is given as

11 oMdes = pinocchio.SE3(np.eye(3), np.array([1., 0., 1.]))

Next, we define an initial configuration

12 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

14 eps = 1e-4
15 IT_MAX = 1000
16 DT = 1e-1
17 damp = 1e-12

corresponding to the desired position precision (we will see later what it exactly means), a maximum number of iterations (to avoid infinite looping in case the position is not reachable), a positive "time step" defining the convergence rate, and a fixed damping factor for the pseudoinversion (see below).

Then, we begin the iterative process. At each iteration, we begin by computing the forward kinematics:

18  pinocchio.forwardKinematics(model,data,q)

Next, we compute the error between the desired pose and the current one.

23  dMi = oMdes.actInv(data.oMi[JOINT_ID])
24  err = pinocchio.log(dMi).vector

Here, data.oMi[JOINT_ID] corresponds to the placement of the sixth joint (previously computed by forwardKinematics), dMi corresponds to the transformation between the desired pose and the current one, and err is the error. In order to compute the error, we employed log: this is a Motion object, and it is one way of computing an error in \(SO(3)\) as a six-dimensional vector.

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

25  if norm(err) < eps:
26  success = True
27  break

Notice that, strictly speaking, the norm of a spatial velocity does not make physical sense, since it mixes linear and angular quantities. A more rigorous implementation should treat the linar part and the angular part separately. In this example, however, we choose to slightly abuse the notation in order to keep it simple.

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

28  if i >= IT_MAX:
29  success = False
30  break

Otherwise, we search for another configuration trying to reduce the error.

We start by computing the Jacobian.

31  J = pinocchio.computeJointJacobian(model,data,q,JOINT_ID)

Next, we can compute the evolution of the configuration by solving the inverse kinematics. In order to avoid problems at singularities, we employ the damped pseudo-inverse: \(v = - J^T (J J^T + \lambda I)^{-1} e\) implementing the equation as

32  v = - J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))

Notice that this way to compute the damped pseudo-inverse was chosen mostly because of its simplicity of implementation. It is not necessarily the best nor the fastest way, and using a fixed damping factor \(\lambda\) is not necessarily the best course of action.

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

33  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:

39 if success:
40  print("Convergence achieved!")
41 else:
42  print("\nWarning: the iterative algorithm has not reached convergence to the desired precision")
43 
44 print('\nresult: %s' % q.flatten().tolist())
45 print('\nfinal error: %s' % err.T)

C++

The equivalent C++ implemetation is given below

1 #include "pinocchio/parsers/sample-models.hpp"
2 #include "pinocchio/spatial/explog.hpp"
3 #include "pinocchio/algorithm/kinematics.hpp"
4 #include "pinocchio/algorithm/jacobian.hpp"
5 #include "pinocchio/algorithm/joint-configuration.hpp"
6 
7 int main(int /* argc */, char ** /* argv */)
8 {
9  pinocchio::Model model;
11  pinocchio::Data data(model);
12 
13  const int JOINT_ID = 6;
14  const pinocchio::SE3 oMdes(Eigen::Matrix3d::Identity(), Eigen::Vector3d(1., 0., 1.));
15 
16  Eigen::VectorXd q = pinocchio::neutral(model);
17  const double eps = 1e-4;
18  const int IT_MAX = 1000;
19  const double DT = 1e-1;
20  const double damp = 1e-6;
21 
22  pinocchio::Data::Matrix6x J(6,model.nv);
23  J.setZero();
24 
25  bool success = false;
26  typedef Eigen::Matrix<double, 6, 1> Vector6d;
27  Vector6d err;
28  Eigen::VectorXd v(model.nv);
29  for (int i=0;;i++)
30  {
31  pinocchio::forwardKinematics(model,data,q);
32  const pinocchio::SE3 dMi = oMdes.actInv(data.oMi[JOINT_ID]);
33  err = pinocchio::log6(dMi).toVector();
34  if(err.norm() < eps)
35  {
36  success = true;
37  break;
38  }
39  if (i >= IT_MAX)
40  {
41  success = false;
42  break;
43  }
44  pinocchio::computeJointJacobian(model,data,q,JOINT_ID,J);
45  pinocchio::Data::Matrix6 JJt;
46  JJt.noalias() = J * J.transpose();
47  JJt.diagonal().array() += damp;
48  v.noalias() = - J.transpose() * JJt.ldlt().solve(err);
49  q = pinocchio::integrate(model,q,v*DT);
50  if(!(i%10))
51  std::cout << i << ": error = " << err.transpose() << std::endl;
52  }
53 
54  if(success)
55  {
56  std::cout << "Convergence achieved!" << std::endl;
57  }
58  else
59  {
60  std::cout << "\nWarning: the iterative algorithm has not reached convergence to the desired precision" << std::endl;
61  }
62 
63  std::cout << "\nresult: " << q.transpose() << std::endl;
64  std::cout << "\nfinal error: " << err.transpose() << std::endl;
65 }

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

22  pinocchio::Data::Matrix6x J(6,model.nv);
23  J.setZero();
45  pinocchio::computeJointJacobian(model,data,q,JOINT_ID,J);

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

47  pinocchio::Data::Matrix6 JJt;
48  JJt.noalias() = J * J.transpose();
49  JJt.diagonal().array() += damp;
50  v.noalias() = - J.transpose() * JJt.ldlt().solve(err);

This code is longer than the Python version, but equivalent to it. Notice we explicitly employ the ldlt decomposition.

pinocchio::DataTpl
Definition: data.hpp:29
pinocchio::forwardKinematics
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.
pinocchio::log6
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Definition: explog.hpp:403
pinocchio::SE3Tpl< double, 0 >
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: data.hpp:73
pinocchio::forwardKinematics
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...
pinocchio::buildModels::manipulator
void manipulator(ModelTpl< Scalar, Options, JointCollectionTpl > &model)
Create a 6-DOF kinematic chain shoulder-elbow-wrist.
pinocchio::neutral
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.
pinocchio::integrate
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.
Definition: joint-configuration.hpp:54
pinocchio::integrate
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.
pinocchio::computeJointJacobian
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...
pinocchio::neutral
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
Definition: joint-configuration.hpp:257
pinocchio::ModelTpl
Definition: fwd.hpp:23