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 KNIKATANAKINEMATICS5M180_H 00021 #define KNIKATANAKINEMATICS5M180_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 KatanaKinematics5M180 : 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 theta234; 00065 double b1; 00066 double b2; 00067 double costh3; 00068 }; 00069 00070 typedef std::vector<angles_calc> angles_container; 00071 00072 metrics _length; 00073 parameter_container _parameters; 00074 00075 static const double _tolerance; // initialized in .cpp 00076 static const int _nrOfPossibleSolutions; 00077 00078 void _setLength(metrics const& length) { _length = length; } 00079 void _setParameters(parameter_container const& parameters) { _parameters = parameters; } 00080 00081 00082 }; 00083 00084 00085 00086 00087 00088 } 00089 00090 #endif