KatanaKinematics5M180.cpp
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 
00021 #include "KNI_InvKin/KatanaKinematics5M180.h"
00022 #include "common/MathHelperFunctions.h"
00023 
00024 namespace KNI {
00025 
00026 const double KatanaKinematics5M180::_tolerance = 0.001;
00027 const int KatanaKinematics5M180::_nrOfPossibleSolutions = 1;
00028 
00029 void
00030 KatanaKinematics5M180::DK(coordinates& solution, encoders const& current_encoders) const {
00031         using namespace KNI_MHF;
00032         // numbering the angles by 0-4
00033         double a, b, c, alpha1, alpha2;
00034         coordinates pose(6, 0);
00035 
00036         // size has to be 5
00037         angles current_angles(5);
00038         for(unsigned int z = 0; z < current_encoders.size(); ++z) {
00039                 current_angles[z] = enc2rad(current_encoders[z], _parameters[z].angleOffset, _parameters[z].epc, _parameters[z].encOffset, _parameters[z].rotDir);
00040         }
00041 
00042         a = _length[1]+_length[2];
00043         b = _length[0];
00044         c = sqrt( pow2(a)+pow2(b)-2*a*b*cos(current_angles[2]));// law of cosinef
00045         alpha1 = asin(a*sin(current_angles[2])/c);                  // law of sine
00046         alpha2 = current_angles[1]-alpha1;
00047 
00048         pose[0] = c*cos(alpha2)*cos(current_angles[0]);
00049         pose[1] = c*cos(alpha2)*sin(current_angles[0]);
00050         pose[2] = c*sin(alpha2);
00051 
00052         std::swap(solution, pose);
00053 }
00054 
00055 void
00056 KatanaKinematics5M180::init( metrics const& length, parameter_container const& parameters ) {
00057         assert( (length.size() == 3) && "You have to provide the metrics for exactly 3 links" ); // we have 3 links
00058         assert( (parameters.size() == 5) && "You have to provide exactly 5 motor parameters" ); // 5 motors are used for IK calculations
00059         _setLength( length );
00060         _setParameters ( parameters );
00061 }
00062 
00063 
00064 void
00065 KatanaKinematics5M180::IK(encoders::iterator solution, coordinates const& pose, encoders const& current_encoders) const {
00066         using namespace KNI_MHF;
00067 
00068         // pose: Winkel Deg->Rad
00069         // Nur eine Lösung
00070 
00071         // Declarations
00072         position p_m;
00073         angles_container angle(_nrOfPossibleSolutions);
00074 
00075         double dist, a, b;
00076         double alpha1Px, alpha2Px;
00077 
00078         // Initialization
00079         p_m.x = pose[0];
00080         p_m.y = pose[1];
00081         p_m.z = pose[2];
00082 
00083         dist = sqrt(pow2(p_m.x)+pow2(p_m.y)+pow2(p_m.z));
00084         alpha2Px = asin(p_m.z/dist);
00085 
00086         angle[0].theta1 = atan1(p_m.x,p_m.y);
00087         if (angle[0].theta1 > (2*M_PI+_parameters[0].angleOffset))
00088                 angle[0].theta1 -= 2*M_PI;
00089 
00090         a = _length[1]+_length[2];
00091         b = _length[0];
00092         angle[0].theta3 = acos((pow2(a)+pow2(b)-pow2(dist))/(2*a*b)); // law of cosinef
00093         if (angle[0].theta3 > (2*M_PI+_parameters[2].angleOffset))
00094                 angle[0].theta3 -= 2*M_PI;
00095 
00096         alpha1Px = asin(a*sin(angle[0].theta3)/dist);
00097         angle[0].theta2 = alpha1Px+alpha2Px;
00098         if (angle[0].theta2 > (2*M_PI+_parameters[1].angleOffset))
00099                 angle[0].theta2 -= 2*M_PI;
00100 
00101         std::vector<int> temp_solution(5);
00102 
00103         temp_solution[0] = rad2enc(angle[0].theta1, _parameters[0].angleOffset, _parameters[0].epc, _parameters[0].encOffset, _parameters[0].rotDir);
00104         temp_solution[1] = rad2enc(angle[0].theta2, _parameters[1].angleOffset, _parameters[1].epc, _parameters[1].encOffset, _parameters[1].rotDir);
00105 
00106         temp_solution[2] = rad2enc(angle[0].theta3, _parameters[2].angleOffset, _parameters[2].epc, _parameters[2].encOffset, _parameters[2].rotDir);
00107         temp_solution[3] = current_encoders[3];  // copy encoders
00108         temp_solution[4] = current_encoders[4];  // copy encoders
00109 
00110         std::copy(temp_solution.begin(), temp_solution.end(), solution);
00111 
00112 }
00113 
00114 
00115 
00116 } // NAMESPACE: KNI
00117 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Tue May 28 2013 14:52:53