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 static const int E_JACSOLVER_FAILED = -101;
00053 static const int E_FKSOLVERPOS_FAILED = -102;
00054
00055
00056 static const int HYBRID = 0;
00057
00058 static const int BODYFIXED = 1;
00059
00060 static const int INERTIAL = 2;
00061
00062 explicit ChainJntToJacDotSolver(const Chain& chain);
00063 virtual ~ChainJntToJacDotSolver();
00072 virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Twist& jac_dot_q_dot, int seg_nr = -1);
00082 virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Jacobian& jdot, int seg_nr = -1);
00083 int setLockedJoints(const std::vector<bool>& locked_joints);
00084
00091 void setHybridRepresentation(){setRepresentation(HYBRID);}
00097 void setBodyFixedRepresentation(){setRepresentation(BODYFIXED);}
00103 void setInertialRepresentation(){setRepresentation(INERTIAL);}
00110 void setRepresentation(const int& representation);
00111
00113 virtual void updateInternalDataStructures();
00114
00116 virtual const char* strError(const int error) const;
00117
00118
00119 protected:
00128 const Twist& getPartialDerivativeHybrid(const Jacobian& bs_J_ee,
00129 const unsigned int& joint_idx,
00130 const unsigned int& column_idx);
00139 const Twist& getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee,
00140 const unsigned int& joint_idx,
00141 const unsigned int& column_idx);
00150 const Twist& getPartialDerivativeInertial(const Jacobian& bs_J_bs,
00151 const unsigned int& joint_idx,
00152 const unsigned int& column_idx);
00162 const Twist& getPartialDerivative(const Jacobian& J,
00163 const unsigned int& joint_idx,
00164 const unsigned int& column_idx,
00165 const int& representation);
00166 private:
00167
00168 const Chain& chain;
00169 std::vector<bool> locked_joints_;
00170 unsigned int nr_of_unlocked_joints_;
00171 ChainJntToJacSolver jac_solver_;
00172 Jacobian jac_;
00173 Jacobian jac_dot_;
00174 int representation_;
00175 ChainFkSolverPos_recursive fk_solver_;
00176 Frame F_bs_ee_;
00177 Twist jac_dot_k_;
00178 Twist jac_j_, jac_i_;
00179 Twist t_djdq_;
00180 };
00181
00182 }
00183 #endif