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 {
00046 class ChainIkSolverPos_NR_JL_Mimic : public ChainIkSolverPos
00047 {
00048 public:
00066   ChainIkSolverPos_NR_JL_Mimic(const Chain& chain, const JntArray& q_min, const JntArray& q_max,
00067                                ChainFkSolverPos& fksolver, ChainIkSolverVel& iksolver, unsigned int maxiter = 100,
00068                                double eps = 1e-6, bool position_ik = false);
00069 
00070   ~ChainIkSolverPos_NR_JL_Mimic();
00071 
00072   virtual int CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out);
00073 
00074   virtual int CartToJntAdvanced(const JntArray& q_init, const Frame& p_in, JntArray& q_out, bool lock_redundant_joints);
00075 
00076   bool setMimicJoints(const std::vector<kdl_kinematics_plugin::JointMimic>& mimic_joints);
00077 
00078 private:
00079   const Chain chain;
00080   JntArray q_min;        // These are the limits for the "reduced" state consisting of only active DOFs
00081   JntArray q_min_mimic;  // These are the limits for the full state
00082   JntArray q_max;        // These are the limits for the "reduced" state consisting of only active DOFs
00083   JntArray q_max_mimic;  // These are the limits for the full state
00084   JntArray q_temp;
00085   ChainFkSolverPos& fksolver;
00086   ChainIkSolverVel& iksolver;
00087   JntArray delta_q;
00088   Frame f;
00089   Twist delta_twist;
00090   unsigned int maxiter;
00091   double eps;
00092   std::vector<kdl_kinematics_plugin::JointMimic> mimic_joints;
00093   void qToqMimic(const JntArray& q,
00094                  JntArray& q_result);  // Convert from the "reduced" state (only active DOFs) to the "full" state
00095   void qMimicToq(const JntArray& q, JntArray& q_result);  // Convert from the "full" state to the "reduced" state
00096   bool position_ik;
00097 };
00098 }
00099 
00100 #endif


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Mon Jul 24 2017 02:21:29