Go to the documentation of this file.
3 from numpy.linalg
import norm, solve
5 model = pinocchio.buildSampleModelManipulator()
6 data = model.createData()
20 iMd = data.oMi[JOINT_ID].actInv(oMdes)
21 err = pinocchio.log(iMd).vector
30 v = -J.T.dot(
solve(J.dot(J.T) + damp * np.eye(6), err))
33 print(
"%d: error = %s" % (i, err.T))
37 print(
"Convergence achieved!")
41 "Warning: the iterative algorithm has not reached convergence "
42 "to the desired precision"
45 print(f
"\nresult: {q.flatten().tolist()}")
46 print(f
"\nfinal error: {err.T}")
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 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.
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.
Eigen::Matrix< Scalar, 6, 6, Options > Jlog6(const SE3Tpl< Scalar, Options > &M)
Derivative of log6.
void computeJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const JointIndex joint_id, 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 Sun Dec 22 2024 03:41:09