KDL  1.5.1
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 namespace KDL
12 {
23  {
24  public:
25 
33  explicit ChainIkSolverVel_pinv_givens(const Chain& chain);
35 
36  virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
41  virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return (error = E_NOT_IMPLEMENTED);};
42 
44  virtual void updateInternalDataStructures();
45 
46  private:
47  const Chain& chain;
48  unsigned int nj;
52  unsigned int m,n;
53  Eigen::MatrixXd jac_eigen,U,V,B;
54  Eigen::VectorXd S,tempi,UY,SUY,qdot_eigen,v_in_eigen;
55  };
56 }
57 #endif
Jacobian jac
Definition: chainiksolvervel_pinv_givens.hpp:50
Implementation of a inverse velocity kinematics algorithm based on the generalize pseudo inverse to c...
Definition: chainiksolvervel_pinv_givens.hpp:22
Definition: jacobian.hpp:36
unsigned int nj
Definition: chainiksolvervel_pinv_givens.hpp:48
Eigen::VectorXd v_in_eigen
Definition: chainiksolvervel_pinv_givens.hpp:54
This class encapsulates a serial kinematic interconnection structure.
Definition: chain.hpp:35
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
~ChainIkSolverVel_pinv_givens()
Definition: chainiksolvervel_pinv_givens.cpp:65
ChainIkSolverVel_pinv_givens(const Chain &chain)
Constructor of the solver.
Definition: chainiksolvervel_pinv_givens.cpp:27
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers.
Definition: chainjnttojacsolver.hpp:38
bool toggle
Definition: chainiksolvervel_pinv_givens.hpp:51
represents both translational and rotational velocities.
Definition: frames.hpp:720
Not yet implemented.
Definition: solveri.hpp:105
Definition: articulatedbodyinertia.cpp:26
Eigen::VectorXd tempi
Definition: chainiksolvervel_pinv_givens.hpp:54
unsigned int n
Definition: chainiksolvervel_pinv_givens.hpp:52
Definition: jntarrayvel.hpp:45
Eigen::VectorXd SUY
Definition: chainiksolvervel_pinv_givens.hpp:54
const Chain & chain
Definition: chainiksolvervel_pinv_givens.hpp:47
Eigen::VectorXd S
Definition: chainiksolvervel_pinv_givens.hpp:54
Eigen::MatrixXd V
Definition: chainiksolvervel_pinv_givens.hpp:53
ChainJntToJacSolver jnt2jac
Definition: chainiksolvervel_pinv_givens.hpp:49
unsigned int m
Definition: chainiksolvervel_pinv_givens.hpp:52
Eigen::VectorXd qdot_eigen
Definition: chainiksolvervel_pinv_givens.hpp:54
Eigen::VectorXd UY
Definition: chainiksolvervel_pinv_givens.hpp:54
Eigen::MatrixXd U
Definition: chainiksolvervel_pinv_givens.hpp:53
bool transpose
Definition: chainiksolvervel_pinv_givens.hpp:51
Eigen::MatrixXd B
Definition: chainiksolvervel_pinv_givens.hpp:53
virtual int CartToJnt(const JntArray &, const FrameVel &, JntArrayVel &)
not (yet) implemented.
Definition: chainiksolvervel_pinv_givens.hpp:41
Definition: framevel.hpp:212
Eigen::MatrixXd jac_eigen
Definition: chainiksolvervel_pinv_givens.hpp:53
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition: chainiksolvervel_pinv_givens.cpp:48
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:70
This abstract class encapsulates the inverse velocity solver for a KDL::Chain.
Definition: chainiksolver.hpp:66