chainjnttojacdotsolver.hpp
Go to the documentation of this file.
00001 /*
00002     Computes the Jacobian time derivative
00003     Copyright (C) 2015  Antoine Hoarau <hoarau [at] isir.upmc.fr>
00004 
00005     This library is free software; you can redistribute it and/or
00006     modify it under the terms of the GNU Lesser General Public
00007     License as published by the Free Software Foundation; either
00008     version 2.1 of the License, or (at your option) any later version.
00009 
00010     This library is distributed in the hope that it will be useful,
00011     but WITHOUT ANY WARRANTY; without even the implied warranty of
00012     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00013     Lesser General Public License for more details.
00014 
00015     You should have received a copy of the GNU Lesser General Public
00016     License along with this library; if not, write to the Free Software
00017     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
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     // Hybrid representation ref Frame: base, ref Point: end-effector
00056     static const int HYBRID = 0;
00057     // Body-fixed representation ref Frame: end-effector, ref Point: end-effector
00058     static const int BODYFIXED = 1;
00059     // Inertial representation ref Frame: base, ref Point: base
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


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:22