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 KNIKATANAKINEMATICS6M180_H 00021 #define KNIKATANAKINEMATICS6M180_H 00022 00023 #include "common/dllexport.h" 00024 00025 #include "KNI_InvKin/KatanaKinematics.h" 00026 #include "KNI_InvKin/KatanaKinematicsDecisionAlgorithms.h" 00027 00028 00029 #include <vector> 00030 00031 00032 namespace KNI { 00033 00040 class DLLDIR_IK KatanaKinematics6M180 : public KatanaKinematics { 00041 00042 public: 00043 00044 void init(metrics const& length, parameter_container const& parameters); 00045 00046 // strong guarantee provided here: 00047 void DK(coordinates& solution, encoders const& current_encoders) const; 00048 void IK(encoders::iterator solution, coordinates const& pose, encoders const& cur_angles) const; 00049 00050 00051 private: 00052 00053 struct position { 00054 double x; 00055 double y; 00056 double z; 00057 }; 00058 00059 struct angles_calc { 00060 double theta1; 00061 double theta2; 00062 double theta3; 00063 double theta4; 00064 double theta5; 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_b1b2costh3_6M180(angles_calc &a, const position &p) const; 00083 00084 void thetacomp(angles_calc &a, const position &p_m) const; 00085 00086 bool angledef(angles_calc &a) const; 00087 00088 bool AnglePositionTest(const angles_calc &a) const; 00089 bool PositionTest6M180(const angles_calc &a, const position &p) const; 00090 00091 }; 00092 00093 00094 00095 00096 00097 } 00098 00099 #endif