treeiksolverpos_online.hpp
Go to the documentation of this file.
00001 // Copyright  (C)  2011  PAL Robotics S.L.  All rights reserved.
00002 // Copyright  (C)  2007-2008  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00003 // Copyright  (C)  2008  Mikael Mayer
00004 // Copyright  (C)  2008  Julia Jesse
00005 
00006 // Version: 1.0
00007 // Author: Marcus Liebhardt
00008 // This class has been derived from the KDL::TreeIkSolverPos_NR_JL class
00009 // by Julia Jesse, Mikael Mayer and Ruben Smits
00010 
00011 // This library is free software; you can redistribute it and/or
00012 // modify it under the terms of the GNU Lesser General Public
00013 // License as published by the Free Software Foundation; either
00014 // version 2.1 of the License, or (at your option) any later version.
00015 
00016 // This library is distributed in the hope that it will be useful,
00017 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00018 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00019 // Lesser General Public License for more details.
00020 
00021 // You should have received a copy of the GNU Lesser General Public
00022 // License along with this library; if not, write to the Free Software
00023 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00024 
00025 #ifndef KDLTREEIKSOLVERPOS_ONLINE_HPP
00026 #define KDLTREEIKSOLVERPOS_ONLINE_HPP
00027 
00028 #include <vector>
00029 #include <string>
00030 #include "kdl/treeiksolver.hpp"
00031 #include "kdl/treefksolver.hpp"
00032 #include "kdl/jntarrayvel.hpp"
00033 
00034 namespace KDL {
00035 
00046 class TreeIkSolverPos_Online: public TreeIkSolverPos {
00047 public:
00060     TreeIkSolverPos_Online(const double& nr_of_jnts,
00061                            const std::vector<std::string>& endpoints,
00062                            TreeFkSolverPos& fksolver,
00063                            TreeIkSolverVel& iksolver,
00064                            const JntArray& q_min,
00065                            const JntArray& q_max,
00066                            const JntArray& q_dot_min,
00067                            const JntArray& q_dot_max,
00068                            const double x_dot_trans_max,
00069                            const double x_dot_rot_max,
00070                            const double x_dot_trans_min = 0.0,
00071                            const double x_dot_rot_min = 0.0,
00072                            const double low_pass_factor = 0.0,
00073                            const unsigned int maxiter = 100,
00074                            const double eps = 1e-6);
00075 
00076     ~TreeIkSolverPos_Online();
00077 
00078     virtual double CartToJnt(const JntArray& q_in, const Frames& p_in, JntArray& q_out);
00079 
00092     double CartToJnt_it(const JntArray& q_in, const Frames& p_in, JntArray& q_out);
00093 
00094 private:
00102   bool enforceCartVelLimits();
00103 
00104   void enforceCartVelLimits_it(Twist& old_twist, Twist& current_twist);
00105 
00112   bool enforceJointVelLimits();
00113 
00114   void enforceJointVelLimits_it(JntArray& q_dot_old, JntArray& q_dot_current);
00115 
00116   void filter(JntArray& q_dot, JntArray& q_out, JntArray& q_out_old);
00117 
00118   TreeFkSolverPos& fksolver_;
00119   TreeIkSolverVel& iksolver_;
00120   JntArray q_min_;
00121   JntArray q_max_;
00122   JntArray q_dot_min_;
00123   JntArray q_dot_max_;
00124   double x_dot_trans_max_;
00125   double x_dot_rot_max_;
00126   double x_dot_trans_min_;
00127   double x_dot_rot_min_;
00128   double low_pass_factor_;
00129   unsigned int maxiter_;
00130   double eps_;
00131 
00132   JntArray q_dot_;
00133   Twist twist_;
00134   Frames frames_;
00135   Twists delta_twists_;
00136   Twists old_twists_;
00137   Frames frames_pos_lim_;
00138   Frames frames_vel_lim_;
00139   JntArray q_dot_old_;
00140   JntArray q_dot_new_;
00141   Frames p_in_old_;
00142   JntArray q_out_old_;
00143   double low_pass_adj_factor_;
00144   unsigned int nr_of_still_endeffectors_;
00145   bool small_task_space_movement_;
00146 };
00147 
00148 } // namespace
00149 
00150 #endif /* KDLTREEIKSOLVERPOS_ONLINE_HPP */
00151 


tree_kinematics
Author(s): Marcus Liebhardt
autogenerated on Mon Jan 6 2014 11:40:04