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, JntArray opt_pos, 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
00098 private:
00099 const Chain chain;
00100 ChainJntToJacSolver jnt2jac;
00101 unsigned int nj;
00102 Jacobian jac;
00103 Eigen::MatrixXd U;
00104 Eigen::VectorXd S;
00105 Eigen::VectorXd Sinv;
00106 Eigen::MatrixXd V;
00107 Eigen::VectorXd tmp;
00108 Eigen::VectorXd tmp2;
00109 double eps;
00110 int maxiter;
00111
00112 double alpha;
00113 JntArray weights;
00114 JntArray opt_pos;
00115 };
00116 }
00117 #endif
00118