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 #ifndef KDL_CHAINJNTTOJACDOTSOLVER_HPP
00022 #define KDL_CHAINJNTTOJACDOTSOLVER_HPP
00023
00024 #include "solveri.hpp"
00025 #include "frames.hpp"
00026 #include "jntarrayvel.hpp"
00027 #include "jacobian.hpp"
00028 #include "chain.hpp"
00029 #include "framevel.hpp"
00030 #include "chainjnttojacsolver.hpp"
00031 #include "chainfksolverpos_recursive.hpp"
00032
00033 namespace KDL
00034 {
00035
00048 class ChainJntToJacDotSolver : public SolverI
00049 {
00050 public:
00051 static const int E_JAC_DOT_FAILED = -100;
00052
00053
00054 static const int HYBRID = 0;
00055
00056 static const int BODYFIXED = 1;
00057
00058 static const int INTERTIAL = 2;
00059
00060 explicit ChainJntToJacDotSolver(const Chain& chain);
00061 virtual ~ChainJntToJacDotSolver();
00070 virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Twist& jac_dot_q_dot, int seg_nr = -1);
00080 virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Jacobian& jdot, int seg_nr = -1);
00081 int setLockedJoints(const std::vector<bool> locked_joints);
00082
00089 void setHybridRepresentation(){setRepresentation(HYBRID);}
00095 void setBodyFixedRepresentation(){setRepresentation(BODYFIXED);}
00101 void setInternialRepresentation(){setRepresentation(INTERTIAL);}
00108 void setRepresentation(const int& representation);
00109
00111 virtual const char* strError(const int error) const;
00112 protected:
00121 const Twist& getPartialDerivativeHybrid(const Jacobian& bs_J_ee,
00122 const unsigned int& joint_idx,
00123 const unsigned int& column_idx);
00132 const Twist& getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee,
00133 const unsigned int& joint_idx,
00134 const unsigned int& column_idx);
00143 const Twist& getPartialDerivativeInertial(const Jacobian& bs_J_bs,
00144 const unsigned int& joint_idx,
00145 const unsigned int& column_idx);
00155 const Twist& getPartialDerivative(const Jacobian& J,
00156 const unsigned int& joint_idx,
00157 const unsigned int& column_idx,
00158 const int& representation);
00159 private:
00160
00161 const Chain chain;
00162 std::vector<bool> locked_joints_;
00163 unsigned int nr_of_unlocked_joints_;
00164 ChainJntToJacSolver jac_solver_;
00165 Jacobian jac_;
00166 Jacobian jac_dot_;
00167 int representation_;
00168 ChainFkSolverPos_recursive fk_solver_;
00169 Frame F_bs_ee_;
00170 Twist jac_dot_k_;
00171 Twist jac_j_, jac_i_;
00172 Twist t_djdq_;
00173 };
00174
00175 }
00176 #endif