inverse-kinematics.py
Go to the documentation of this file.
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 
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)
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...
Mat & solve(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &y)
Return the solution of using the Cholesky decomposition stored in data given the entry ...
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
Author(s):
autogenerated on Tue Jun 1 2021 02:45:03