Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #ifndef KDL_CHAIN_IKSOLVERVEL_PINV_NSO_HPP
00023 #define KDL_CHAIN_IKSOLVERVEL_PINV_NSO_HPP
00024
00025 #include "chainiksolver.hpp"
00026 #include "chainjnttojacsolver.hpp"
00027 #include <Eigen/Core>
00028
00029 namespace KDL
00030 {
00046 class ChainIkSolverVel_pinv_nso : public ChainIkSolverVel
00047 {
00048 public:
00063 ChainIkSolverVel_pinv_nso(const Chain& chain, const JntArray& opt_pos, const JntArray& weights, double eps=0.00001,int maxiter=150, double alpha = 0.25);
00064 explicit ChainIkSolverVel_pinv_nso(const Chain& chain, double eps=0.00001,int maxiter=150, double alpha = 0.25);
00065 ~ChainIkSolverVel_pinv_nso();
00066
00067 virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
00072 virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;};
00073
00080 virtual int setWeights(const JntArray &weights);
00081
00088 virtual int setOptPos(const JntArray &opt_pos);
00089
00096 virtual int setAlpha(const double alpha);
00097
00102 int getSVDResult()const {return svdResult;};
00103
00104 private:
00105 const Chain chain;
00106 ChainJntToJacSolver jnt2jac;
00107 unsigned int nj;
00108 Jacobian jac;
00109 Eigen::MatrixXd U;
00110 Eigen::VectorXd S;
00111 Eigen::VectorXd Sinv;
00112 Eigen::MatrixXd V;
00113 Eigen::VectorXd tmp;
00114 Eigen::VectorXd tmp2;
00115 double eps;
00116 int maxiter;
00117 int svdResult;
00118 double alpha;
00119 JntArray weights;
00120 JntArray opt_pos;
00121 };
00122 }
00123 #endif
00124