KDL  1.4.0
chainiksolvervel_pinv_givens.hpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2 
3 #ifndef KDL_CHAIN_IKSOLVERVEL_PINV_GIVENS_HPP
4 #define KDL_CHAIN_IKSOLVERVEL_PINV_GIVENS_HPP
5 
6 #include "chainiksolver.hpp"
8 
9 #include <Eigen/Core>
10 
11 using namespace Eigen;
12 
13 namespace KDL
14 {
25  {
26  public:
27 
39  explicit ChainIkSolverVel_pinv_givens(const Chain& chain);
41 
42  virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
47  virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return (error = E_NOT_IMPLEMENTED);};
48 
50  virtual void updateInternalDataStructures();
51 
52  private:
53  const Chain& chain;
54  unsigned int nj;
57  bool transpose,toggle;
58  unsigned int m,n;
59  MatrixXd jac_eigen,U,V,B;
60  VectorXd S,tempi,tempj,UY,SUY,qdot_eigen,v_in_eigen;
61  };
62 }
63 #endif
KDL::ChainIkSolverVel_pinv_givens::SUY
VectorXd SUY
Definition: chainiksolvervel_pinv_givens.hpp:60
KDL::JntArray
Definition: jntarray.hpp:70
chainiksolvervel_pinv_givens.hpp
KDL::ChainIkSolverVel_pinv_givens::qdot_eigen
VectorXd qdot_eigen
Definition: chainiksolvervel_pinv_givens.hpp:60
KDL::SolverI::E_NOT_UP_TO_DATE
@ E_NOT_UP_TO_DATE
Chain size changed.
Definition: solveri.hpp:97
KDL::ChainIkSolverVel_pinv_givens::tempj
VectorXd tempj
Definition: chainiksolvervel_pinv_givens.hpp:60
KDL::ChainIkSolverVel_pinv_givens::transpose
bool transpose
Definition: chainiksolvervel_pinv_givens.hpp:57
KDL::ChainIkSolverVel_pinv_givens::CartToJnt
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
Calculate inverse velocity kinematics, from joint positions and cartesian velocity to joint velocitie...
Definition: chainiksolvervel_pinv_givens.cpp:72
KDL::ChainIkSolverVel_pinv_givens::toggle
bool toggle
Definition: chainiksolvervel_pinv_givens.hpp:57
KDL::ChainIkSolverVel_pinv_givens::jac_eigen
MatrixXd jac_eigen
Definition: chainiksolvervel_pinv_givens.hpp:59
KDL
Definition: articulatedbodyinertia.cpp:28
KDL::ChainIkSolverVel_pinv_givens::v_in_eigen
VectorXd v_in_eigen
Definition: chainiksolvervel_pinv_givens.hpp:60
KDL::ChainIkSolverVel_pinv_givens
Implementation of a inverse velocity kinematics algorithm based on the generalize pseudo inverse to c...
Definition: chainiksolvervel_pinv_givens.hpp:25
KDL::ChainIkSolverVel_pinv_givens::n
unsigned int n
Definition: chainiksolvervel_pinv_givens.hpp:58
KDL::Jacobian::resize
void resize(unsigned int newNrOfColumns)
Allocates memory for new size (can break realtime behavior)
Definition: jacobian.cpp:55
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:720
KDL::JntArray::rows
unsigned int rows() const
Returns the number of rows (size) of the array.
Definition: jntarray.cpp:72
KDL::ChainJntToJacSolver::JntToJac
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int segmentNR=-1)
Calculate the jacobian expressed in the base frame of the chain, with reference point at the end effe...
Definition: chainjnttojacsolver.cpp:48
chainiksolver.hpp
KDL::ChainIkSolverVel_pinv_givens::B
MatrixXd B
Definition: chainiksolvervel_pinv_givens.hpp:59
KDL::SolverI::E_SIZE_MISMATCH
@ E_SIZE_MISMATCH
Input size does not match internal state.
Definition: solveri.hpp:99
KDL::ChainIkSolverVel_pinv_givens::m
unsigned int m
Definition: chainiksolvervel_pinv_givens.hpp:58
KDL::ChainIkSolverVel_pinv_givens::U
MatrixXd U
Definition: chainiksolvervel_pinv_givens.hpp:59
KDL::ChainIkSolverVel_pinv_givens::jac
Jacobian jac
Definition: chainiksolvervel_pinv_givens.hpp:56
KDL::ChainJntToJacSolver::updateInternalDataStructures
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition: chainjnttojacsolver.cpp:31
KDL::JntArrayVel
Definition: jntarrayvel.hpp:46
KDL::ChainIkSolverVel_pinv_givens::CartToJnt
virtual int CartToJnt(const JntArray &q_init, const FrameVel &v_in, JntArrayVel &q_out)
not (yet) implemented.
Definition: chainiksolvervel_pinv_givens.hpp:47
KDL::ChainIkSolverVel_pinv_givens::V
MatrixXd V
Definition: chainiksolvervel_pinv_givens.hpp:59
KDL::ChainIkSolverVel_pinv_givens::chain
const Chain & chain
Definition: chainiksolvervel_pinv_givens.hpp:53
KDL::ChainIkSolverVel_pinv_givens::tempi
VectorXd tempi
Definition: chainiksolvervel_pinv_givens.hpp:60
KDL::svd_eigen_Macie
int svd_eigen_Macie(const MatrixXd &A, MatrixXd &U, VectorXd &S, MatrixXd &V, MatrixXd &B, VectorXd &tempi, double threshold, bool toggle)
svd_eigen_Macie provides Maciejewski implementation for SVD.
Definition: svd_eigen_Macie.hpp:59
KDL::ChainIkSolverVel_pinv_givens::S
VectorXd S
Definition: chainiksolvervel_pinv_givens.hpp:60
KDL::FrameVel
Definition: framevel.hpp:204
KDL::ChainIkSolverVel
Definition: chainiksolver.hpp:66
chainjnttojacsolver.hpp
svd_eigen_Macie.hpp
KDL::ChainIkSolverVel_pinv_givens::nj
unsigned int nj
Definition: chainiksolvervel_pinv_givens.hpp:54
KDL::ChainIkSolverVel_pinv_givens::updateInternalDataStructures
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition: chainiksolvervel_pinv_givens.cpp:49
KDL::ChainIkSolverVel_pinv_givens::jnt2jac
ChainJntToJacSolver jnt2jac
Definition: chainiksolvervel_pinv_givens.hpp:55
KDL::ChainJntToJacSolver
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers.
Definition: chainjnttojacsolver.hpp:42
KDL::SolverI::E_NOERROR
@ E_NOERROR
No error.
Definition: solveri.hpp:91
KDL::ChainIkSolverVel_pinv_givens::UY
VectorXd UY
Definition: chainiksolvervel_pinv_givens.hpp:60
KDL::Chain
Definition: chain.hpp:35
KDL::Chain::getNrOfJoints
unsigned int getNrOfJoints() const
Request the total number of joints in the chain.
Definition: chain.hpp:71
KDL::ChainIkSolverVel_pinv_givens::ChainIkSolverVel_pinv_givens
ChainIkSolverVel_pinv_givens(const Chain &chain)
Constructor of the solver.
Definition: chainiksolvervel_pinv_givens.cpp:27
KDL::SolverI::error
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
KDL::Jacobian
Definition: jacobian.hpp:37
KDL::ChainIkSolverVel_pinv_givens::~ChainIkSolverVel_pinv_givens
~ChainIkSolverVel_pinv_givens()
Definition: chainiksolvervel_pinv_givens.cpp:67