chainiksolver_pos_nr_jl_mimic.hpp
Go to the documentation of this file.
00001 // Copyright  (C)  2007-2008  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00002 // Copyright  (C)  2008  Mikael Mayer
00003 // Copyright  (C)  2008  Julia Jesse
00004 
00005 // Version: 1.0
00006 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00007 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00008 // URL: http://www.orocos.org/kdl
00009 
00010 // This library is free software; you can redistribute it and/or
00011 // modify it under the terms of the GNU Lesser General Public
00012 // License as published by the Free Software Foundation; either
00013 // version 2.1 of the License, or (at your option) any later version.
00014 
00015 // This library is distributed in the hope that it will be useful,
00016 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00017 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018 // Lesser General Public License for more details.
00019 
00020 // You should have received a copy of the GNU Lesser General Public
00021 // License along with this library; if not, write to the Free Software
00022 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00023 
00024 // Modified to account for "mimic" joints, i.e. joints whose motion has a
00025 // linear relationship to that of another joint.
00026 // Copyright  (C)  2013  Sachin Chitta, Willow Garage
00027 
00028 #ifndef KDLCHAINIKSOLVERPOS_NR_JL_Mimic_HPP
00029 #define KDLCHAINIKSOLVERPOS_NR_JL_Mimic_HPP
00030 
00031 #include "kdl/chainiksolver.hpp"
00032 #include "kdl/chainfksolver.hpp"
00033 
00034 #include <moveit/kdl_kinematics_plugin/joint_mimic.hpp>
00035 
00036 namespace KDL
00037 {
00038 
00047 class ChainIkSolverPos_NR_JL_Mimic : public ChainIkSolverPos
00048 {
00049 public:
00067   ChainIkSolverPos_NR_JL_Mimic(const Chain& chain,const JntArray& q_min, const JntArray& q_max, ChainFkSolverPos& fksolver,ChainIkSolverVel& iksolver,unsigned int maxiter=100,double eps=1e-6, bool position_ik = false);
00068 
00069   ~ChainIkSolverPos_NR_JL_Mimic();
00070 
00071   virtual int CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out);
00072 
00073   virtual int CartToJntAdvanced(const JntArray& q_init, const Frame& p_in, JntArray& q_out, bool lock_redundant_joints);
00074 
00075   bool setMimicJoints(const std::vector<kdl_kinematics_plugin::JointMimic>& mimic_joints);
00076 
00077 private:
00078   const Chain chain;
00079   JntArray q_min;//These are the limits for the "reduced" state consisting of only active DOFs
00080   JntArray q_min_mimic;//These are the limits for the full state
00081   JntArray q_max;//These are the limits for the "reduced" state consisting of only active DOFs
00082   JntArray q_max_mimic;//These are the limits for the full state
00083   JntArray q_temp;
00084   ChainFkSolverPos& fksolver;
00085   ChainIkSolverVel& iksolver;
00086   JntArray delta_q;
00087   Frame f;
00088   Twist delta_twist;
00089   unsigned int maxiter;
00090   double eps;
00091   std::vector<kdl_kinematics_plugin::JointMimic> mimic_joints;
00092   void qToqMimic(const JntArray& q, JntArray& q_result); //Convert from the "reduced" state (only active DOFs) to the "full" state
00093   void qMimicToq(const JntArray& q, JntArray& q_result); //Convert from the "full" state to the "reduced" state
00094   bool position_ik;
00095 
00096 };
00097 
00098 }
00099 
00100 #endif


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Oct 6 2014 02:31:39