KatanaKinematics6M90T.h
Go to the documentation of this file.
00001 /***************************************************************************
00002  *   Copyright (C) 2006 by Tiziano Mueller   *
00003  *   tiziano.mueller@neuronics.ch   *
00004  *                                                                         *
00005  *   This program is free software; you can redistribute it and/or modify  *
00006  *   it under the terms of the GNU General Public License as published by  *
00007  *   the Free Software Foundation; either version 2 of the License, or     *
00008  *   (at your option) any later version.                                   *
00009  *                                                                         *
00010  *   This program 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         *
00013  *   GNU General Public License for more details.                          *
00014  *                                                                         *
00015  *   You should have received a copy of the GNU General Public License     *
00016  *   along with this program; if not, write to the                         *
00017  *   Free Software Foundation, Inc.,                                       *
00018  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
00019  ***************************************************************************/
00020 #ifndef KNIKatanaKinematics6M90T_H
00021 #define KNIKatanaKinematics6M90T_H
00022 
00023 #include "common/dllexport.h"
00024 
00025 #include "KNI_InvKin/KatanaKinematics.h"
00026 #include "KNI_InvKin/KatanaKinematicsDecisionAlgorithms.h"
00027 
00028 #include <vector>
00029 
00030 
00031 namespace KNI {
00032 
00039 class DLLDIR_IK KatanaKinematics6M90T : public KatanaKinematics {
00040 
00041 public:
00042 
00043     void init(metrics const& length, parameter_container const& parameters);
00044 
00045     // strong guarantee provided here:
00046     void DK(coordinates& solution, encoders const& current_encoders) const;
00047     void IK(encoders::iterator solution, coordinates const& pose, encoders const& cur_angles) const;
00048 
00049 
00050 private:
00051 
00052     struct position {
00053                 double x;
00054                 double y;
00055                 double z;
00056     };
00057 
00058     struct angles_calc {
00059                 double theta1;
00060                 double theta2;
00061                 double theta3;
00062                 double theta4;
00063                 double theta5;
00064                 double theta6;
00065                 double theta234;
00066                 double b1;
00067                 double b2;
00068                 double costh3;
00069     };
00070 
00071     typedef std::vector<angles_calc> angles_container;
00072 
00073     metrics _length;
00074     parameter_container _parameters;
00075 
00076     static const double _tolerance; // initialized in .cpp
00077     static const int    _nrOfPossibleSolutions;
00078 
00079     void _setLength(metrics const& length) { _length = length; }
00080     void _setParameters(parameter_container const& parameters) { _parameters = parameters; }
00081 
00082     void IK_theta234theta5(angles_calc& angle, const position &p_gr) const;
00083     void IK_b1b2costh3_6MS(angles_calc &a, const position &p) const;
00084 
00085     void thetacomp(angles_calc &a, const position &p_m, const coordinates& pose) const;
00086     
00087     bool angledef(angles_calc &a) const;
00088 
00089     bool GripperTest(const position &p_gr, const angles_calc &angle) const;
00090     bool AnglePositionTest(const angles_calc &a) const;
00091     bool PositionTest6MS(const double& theta1, const double& theta2, const double& theta3, const double& theta234, const position &p) const;
00092 
00093     double findFirstEqualAngle(const angles& v1, const angles& v2) const;
00094 
00095 };
00096 
00097 
00098 
00099 
00100 
00101 }
00102 
00103 #endif


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Mon Oct 6 2014 10:45:32