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, 0.0, 1.0]))
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  iMd = data.oMi[JOINT_ID].actInv(oMdes)
24  err = pinocchio.log(iMd).vector # in joint frame
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) # in joint frame
32  J = -np.dot(pinocchio.Jlog6(iMd.inverse()), J)
33  v = - J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))
34  q = pinocchio.integrate(model,q,v*DT)
35  if not i % 10:
36  print('%d: error = %s' % (i, err.T))
37  i += 1
38 
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)
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6.
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 Fri Jun 23 2023 02:38:30