ikBase.cpp
Go to the documentation of this file.
00001 /*
00002  *   Katana Native Interface - A C++ interface to the robot arm Katana.
00003  *   Copyright (C) 2005 Neuronics AG
00004  *   Check out the AUTHORS file for detailed contact information.
00005  *
00006  *   This program is free software; you can redistribute it and/or modify
00007  *   it under the terms of the GNU General Public License as published by
00008  *   the Free Software Foundation; either version 2 of the License, or
00009  *   (at your option) any later version.
00010  *
00011  *   This program is distributed in the hope that it will be useful,
00012  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *   GNU General Public License for more details.
00015  *
00016  *   You should have received a copy of the GNU General Public License
00017  *   along with this program; if not, write to the Free Software
00018  *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  */
00020 
00021 
00022 #include "KNI_InvKin/ikBase.h"
00023 #include "libKinematics.h"
00024 
00025 CikBase::~CikBase() {
00026         if(mKinematics != 0) {
00027                 // mKinematics == 1 and default
00028                 // RobAnaGuess Kinematics
00029                 if(_kinematicsIsInitialized) {
00030                         kin_clean();
00031                 }
00032         }
00033 }
00034 
00035 void CikBase::_initKinematics() {
00036 
00037         if(mKinematics == 0) {
00038                 // Analytical Kinematics
00039                 if( std::string(base->GetGNL()->modelName) == "Katana6M90A_G") {
00040                         _kinematicsImpl.reset(new KNI::KatanaKinematics6M90G);
00041                 } else if(std::string(base->GetGNL()->modelName) == "Katana6M90A_F") {
00042                         _kinematicsImpl.reset(new KNI::KatanaKinematics6M90T);
00043                 } else if( std::string(base->GetGNL()->modelName) == "Katana6M90B_G") {
00044                         _kinematicsImpl.reset(new KNI::KatanaKinematics6M90G);
00045                 } else if(std::string(base->GetGNL()->modelName) == "Katana6M90B_F") {
00046                         _kinematicsImpl.reset(new KNI::KatanaKinematics6M90T);
00047                 } else if( std::string(base->GetGNL()->modelName) == "Katana6M90G") {
00048                         _kinematicsImpl.reset(new KNI::KatanaKinematics6M90G);
00049                 } else if(std::string(base->GetGNL()->modelName) == "Katana6M90T") {
00050                         _kinematicsImpl.reset(new KNI::KatanaKinematics6M90T);
00051                 } else if(std::string(base->GetGNL()->modelName) == "Katana6M180") {
00052                         _kinematicsImpl.reset(new KNI::KatanaKinematics6M180);
00053                 } else if(std::string(base->GetGNL()->modelName) == "Katana5M180") {
00054                         _kinematicsImpl.reset(new KNI::KatanaKinematics5M180);
00055                 } else {
00056                         return;
00057                 }
00058 
00059                 const TKatEFF* eff = base->GetEFF();
00060                 const TKatMOT* mot = base->GetMOT();
00061 
00062                 KNI::KatanaKinematics::metrics length;
00063                 for(int i = 0; i < getNumberOfMotors()-2; ++i) {
00064                         length.push_back( eff->arr_segment[i] );
00065                 }
00066 
00067                 KNI::KinematicParameters joint;
00068                 KNI::KatanaKinematics::parameter_container parameters;
00069                 for(int i = 0; i < getNumberOfMotors(); ++i) {
00070                         joint.epc         = mot->arr[i].GetInitialParameters()->encodersPerCycle;
00071                         joint.encOffset   = mot->arr[i].GetInitialParameters()->encoderOffset;
00072                         joint.angleOffset = mot->arr[i].GetInitialParameters()->angleOffset;
00073                         joint.angleStop   = mot->arr[i].GetInitialParameters()->angleStop;
00074                         joint.rotDir      = mot->arr[i].GetInitialParameters()->rotationDirection;
00075                         parameters.push_back(joint);
00076                 }
00077 
00078                 _kinematicsImpl->init(length, parameters);
00079         } else {
00080                 // mKinematics == 1 and default
00081                 // RobAnaGuess Kinematics
00082                 int type = -1;
00083                 if( std::string(base->GetGNL()->modelName) == "Katana6M90A_G") {
00084                         type = 1;
00085                 } else if(std::string(base->GetGNL()->modelName) == "Katana6M90A_F") {
00086                         type = 0;
00087                 } else if(std::string(base->GetGNL()->modelName) == "Katana6M90B_G") {
00088                         type = 4;
00089                 } else if(std::string(base->GetGNL()->modelName) == "Katana6M90B_F") {
00090                         type = 3;
00091                 } else if(std::string(base->GetGNL()->modelName) == "Katana6M90G") {
00092                         type = 1;
00093                 } else if(std::string(base->GetGNL()->modelName) == "Katana6M90T") {
00094                         type = 0;
00095                 } else if(std::string(base->GetGNL()->modelName) == "Katana6M180") {
00096                         type = 2;
00097                 } else {
00098                         return;
00099                 }
00100 
00101                 // set type
00102                 kin_setType(type);
00103                 int dom = kin_getDOM();
00104                 // set link length
00105                 FloatVector links;
00106                 links.data[0] = (float) (base->GetEFF()->arr_segment[0] / 1000.0);
00107                 links.data[1] = (float) (base->GetEFF()->arr_segment[1] / 1000.0);
00108                 links.data[2] = (float) (base->GetEFF()->arr_segment[2] / 1000.0);
00109                 links.data[3] = (float) (base->GetEFF()->arr_segment[3] / 1000.0);
00110                 links.length = 4;
00111                 kin_setLinkLen(&links);
00112                 // set encoder per cycle
00113                 IntVector epc;
00114                 for (int i = 0; i < dom; ++i) {
00115                         epc.data[i] = base->GetMOT()->arr[i].GetInitialParameters()->encodersPerCycle;
00116                 }
00117                 epc.length = dom;
00118                 kin_setEPC(&epc);
00119                 // set encoder offset
00120                 IntVector encOffset;
00121                 for (int i = 0; i < dom; ++i) {
00122                         encOffset.data[i] = base->GetMOT()->arr[i].GetInitialParameters()->encoderOffset;
00123                 }
00124                 encOffset.length = dom;
00125                 kin_setEncOff(&encOffset);
00126                 // set rotation direction
00127                 IntVector rotDir;
00128                 for (int i = 0; i < dom; ++i) {
00129                         if (i < 3) {
00130                                 // invert first three rotation directions (different angle definitions)
00131                                 rotDir.data[i] = -base->GetMOT()->arr[i].GetInitialParameters()->rotationDirection;
00132                         } else {
00133                                 rotDir.data[i] = base->GetMOT()->arr[i].GetInitialParameters()->rotationDirection;
00134                         }
00135                 }
00136                 rotDir.length = dom;
00137                 kin_setRotDir(&rotDir);
00138                 // set angle offset
00139                 FloatVector angleOffset;
00140                 for (int i = 0; i < dom; ++i) {
00141                         angleOffset.data[i] = (float) (base->GetMOT()->arr[i].GetInitialParameters()->angleOffset);
00142                 }
00143                 angleOffset.length = dom;
00144                 FloatVector angleOffset2;
00145                 kin_K4D2mDHAng(&angleOffset, &angleOffset2);
00146                 kin_setAngOff(&angleOffset2);
00147                 // set angle range
00148                 FloatVector angleRange;
00149                 for (int i = 0; i < dom; ++i) {
00150                         angleRange.data[i] = (float) fabs(base->GetMOT()->arr[i].GetInitialParameters()->angleRange);
00151                 }
00152                 angleRange.length = dom;
00153                 kin_setAngRan(&angleRange);
00154                 // initialize kinematics
00155                 kin_init();
00156         }
00157 
00158         _kinematicsIsInitialized = true;
00159 }
00160 
00161 void CikBase::getKinematicsVersion(std::vector<int>& version) {
00162         if(mKinematics == 0) {
00163                 // Analytical Kinematics
00164                 version.clear();
00165                 version.push_back(0);
00166                 version.push_back(1);
00167                 version.push_back(0);
00168         } else {
00169                 // mKinematics == 1 and default
00170                 // RobAnaGuess Kinematics
00171                 IntVector v;
00172                 kin_getVersion(&v);
00173                 version.clear();
00174                 for (int i = 0; i < v.length; ++i) {
00175                         version.push_back(v.data[i]);
00176                 }
00177         }
00178 }
00179 
00180 void CikBase::setTcpOffset(double xoff, double yoff, double zoff, double psioff) {
00181 
00182         if(mKinematics != 0) {
00183                 // mKinematics == 1 and default
00184                 // RobAnaGuess Kinematics
00185                 FloatVector tcpOff;
00186                 tcpOff.data[0] = (float) xoff;
00187                 tcpOff.data[1] = (float) yoff;
00188                 tcpOff.data[2] = (float) zoff;
00189                 tcpOff.data[3] = (float) psioff;
00190                 tcpOff.length = 4;
00191                 kin_setTcpOff(&tcpOff);
00192         }
00193 
00194 }
00195 
00196 void CikBase::DKApos(double* position) {
00197         getCoordinates(position[0], position[1], position[2], position[3], position[4], position[5]);
00198 }
00199 
00200 void CikBase::IKCalculate(double X, double Y, double Z, double phi, double theta, double psi, std::vector<int>::iterator solution_iter) {
00201 
00202         if(!_kinematicsIsInitialized)
00203                 _initKinematics();
00204 
00205         if(mKinematics == 0) {
00206                 // Analytical Kinematics
00207                 std::vector<double> pose(6);
00208                 pose[0] = X;
00209                 pose[1] = Y;
00210                 pose[2] = Z;
00211                 pose[3] = phi;
00212                 pose[4] = theta;
00213                 pose[5] = psi;
00214 
00215                 std::vector<int> actualPosition;
00216                 base->recvMPS();
00217                 for(int c = 0; c < getNumberOfMotors(); ++c) {
00218                         actualPosition.push_back(getMotorEncoders(c, false));
00219                 }
00220 
00221                 _kinematicsImpl->IK(solution_iter, pose, actualPosition);
00222         } else {
00223                 // mKinematics == 1 and default
00224                 // RobAnaGuess Kinematics
00225                 int maxBisection = 3;
00226                 int nOfMot = getNumberOfMotors();
00227 
00228                 FloatVector pose;
00229                 pose.data[0] = (float) (X / 1000);
00230                 pose.data[1] = (float) (Y / 1000);
00231                 pose.data[2] = (float) (Z / 1000);
00232                 pose.data[3] = (float) phi;
00233                 pose.data[4] = (float) theta;
00234                 pose.data[5] = (float) psi;
00235                 pose.length = 6;
00236 
00237                 IntVector actualPosition;
00238                 base->recvMPS();
00239                 for(int i = 0; i < nOfMot; ++i) {
00240                         actualPosition.data[i] = getMotorEncoders(i, false);
00241                 }
00242                 actualPosition.length = nOfMot;
00243                 FloatVector prev;
00244                 kin_enc2rad(&actualPosition, &prev);
00245 
00246                 FloatVector ikangle;
00247                 kin_IK(&pose, &prev, &ikangle, maxBisection);
00248 
00249                 IntVector ikenc;
00250                 kin_rad2enc(&ikangle, &ikenc);
00251 
00252                 // copy motor 6 encoder if KNI uses 6 and kinematics uses 5 motors (w/o gripper)
00253                 if (ikenc.length == actualPosition.length - 1) {
00254                         ikenc.data[ikenc.length] = actualPosition.data[ikenc.length];
00255                         ikenc.length = actualPosition.length;
00256                 }
00257 
00258                 for(int i = 0; i < nOfMot; ++i) {
00259                         *solution_iter = ikenc.data[i];
00260                         solution_iter++;
00261                 }
00262         }
00263 
00264 }
00265 
00266 void CikBase::IKCalculate(double X, double Y, double Z, double phi, double theta, double psi, std::vector<int>::iterator solution_iter, const std::vector<int>& actualPosition) {
00267 
00268         if(!_kinematicsIsInitialized)
00269                 _initKinematics();
00270 
00271         if(mKinematics == 0) {
00272                 // Analytical Kinematics
00273                 std::vector<double> pose(6);
00274                 pose[0] = X;
00275                 pose[1] = Y;
00276                 pose[2] = Z;
00277                 pose[3] = phi;
00278                 pose[4] = theta;
00279                 pose[5] = psi;
00280 
00281                 _kinematicsImpl->IK(solution_iter, pose, actualPosition);
00282         } else {
00283                 // mKinematics == 1 and default
00284                 // RobAnaGuess Kinematics
00285                 int maxBisection = 3;
00286                 int nOfMot = getNumberOfMotors();
00287 
00288                 FloatVector pose;
00289                 pose.data[0] = (float) (X / 1000);
00290                 pose.data[1] = (float) (Y / 1000);
00291                 pose.data[2] = (float) (Z / 1000);
00292                 pose.data[3] = (float) phi;
00293                 pose.data[4] = (float) theta;
00294                 pose.data[5] = (float) psi;
00295                 pose.length = 6;
00296 
00297                 IntVector actPos;
00298                 for(int i = 0; i < nOfMot; ++i) {
00299                         actPos.data[i] = actualPosition.at(i);
00300                 }
00301                 actPos.length = nOfMot;
00302                 FloatVector prev;
00303                 kin_enc2rad(&actPos, &prev);
00304 
00305                 FloatVector ikangle;
00306                 int error = kin_IK(&pose, &prev, &ikangle, maxBisection);
00307                 if (error)
00308                   throw KNI::NoSolutionException();
00309 
00310                 IntVector ikenc;
00311                 kin_rad2enc(&ikangle, &ikenc);
00312 
00313                 // copy motor 6 encoder if KNI uses 6 and kinematics uses 5 motors (w/o gripper)
00314                 if (ikenc.length == actPos.length - 1) {
00315                         ikenc.data[ikenc.length] = actPos.data[ikenc.length];
00316                         ikenc.length = actPos.length;
00317                 }
00318 
00319                 for(int i = 0; i < nOfMot; ++i) {
00320                         *solution_iter = ikenc.data[i];
00321                         solution_iter++;
00322                 }
00323         }
00324 
00325 }
00326 
00327 void CikBase::IKGoto(double X, double Y, double Z, double Al, double Be, double Ga,  bool wait, int tolerance, long timeout) {
00328 
00329         if(!_kinematicsIsInitialized)
00330                 _initKinematics();
00331 
00332         const TKatMOT* mot = base->GetMOT();
00333 
00334         std::vector<int> solution(getNumberOfMotors());
00335         // fills act_pos[] with the current position in degree units
00336         std::vector<int> act_pos(getNumberOfMotors());
00337         std::vector<int> distance(getNumberOfMotors());
00338 
00339         base->recvMPS();
00340         for (int idx=0; idx<getNumberOfMotors(); ++idx) {
00341                 act_pos[idx] = mot->arr[idx].GetPVP()->pos;
00342         }
00343 
00344         IKCalculate( X, Y, Z, Al, Be, Ga, solution.begin(), act_pos );
00345         moveRobotToEnc( solution.begin(), solution.end(), wait, tolerance, timeout);
00346 
00347 }
00348 
00349 void CikBase::getCoordinates(double& x, double& y, double& z, double& phi, double& theta, double& psi, bool refreshEncoders) {
00350 
00351         if(!_kinematicsIsInitialized)
00352                 _initKinematics();
00353 
00354         if(refreshEncoders)
00355                 base->recvMPS();
00356 
00357         if(mKinematics == 0) {
00358                 // Analytical Kinematics
00359                 std::vector<int> current_encoders(getNumberOfMotors());
00360                 for (int i=0; i<getNumberOfMotors(); i++) {
00361                         current_encoders[i] = base->GetMOT()->arr[i].GetPVP()->pos;
00362                 }
00363 
00364                 std::vector<double> pose(6);
00365 
00366                 _kinematicsImpl->DK(pose, current_encoders);
00367 
00368                 x = pose[0];
00369                 y = pose[1];
00370                 z = pose[2];
00371                 phi = pose[3];
00372                 theta = pose[4];
00373                 psi = pose[5];
00374         } else {
00375                 // mKinematics == 1 and default
00376                 // RobAnaGuess Kinematics
00377                 int nOfMot = getNumberOfMotors();
00378 
00379                 IntVector actPos;
00380                 for(int i = 0; i < nOfMot; ++i) {
00381                         actPos.data[i] = base->GetMOT()->arr[i].GetPVP()->pos;
00382                 }
00383                 actPos.length = nOfMot;
00384                 FloatVector angle;
00385                 kin_enc2rad(&actPos, &angle);
00386 
00387                 FloatVector pose;
00388                 kin_DK(&angle, &pose);
00389 
00390                 x = pose.data[0] * 1000;
00391                 y = pose.data[1] * 1000;
00392                 z = pose.data[2] * 1000;
00393                 phi = pose.data[3];
00394                 theta = pose.data[4];
00395                 psi = pose.data[5];
00396         }
00397 }
00398 
00399 void CikBase::getCoordinatesFromEncoders(std::vector<double>& pos, const std::vector<int>& encs){
00400 
00401         if(!_kinematicsIsInitialized)
00402                 _initKinematics();
00403 
00404         if(mKinematics == 0) {
00405                 // Analytical Kinematics
00406                 _kinematicsImpl->DK(pos, encs);
00407         } else {
00408                 // mKinematics == 1 and default
00409                 int nOfMot = getNumberOfMotors();
00410                 IntVector actPos;
00411                 for(int i = 0; i < nOfMot; ++i) {
00412                         actPos.data[i] = encs.at(i);
00413                 }
00414                 actPos.length = nOfMot;
00415                 FloatVector angle;
00416                 kin_enc2rad(&actPos, &angle);
00417                 FloatVector pose;
00418                 kin_DK(&angle, &pose);
00419                 pos.clear();
00420                 pos.push_back(pose.data[0] * 1000);
00421                 pos.push_back(pose.data[1] * 1000);
00422                 pos.push_back(pose.data[2] * 1000);
00423                 pos.push_back(pose.data[3]);
00424                 pos.push_back(pose.data[4]);
00425                 pos.push_back(pose.data[5]);
00426         }
00427 }
00428 
00429 void CikBase::moveRobotTo(double x, double y, double z, double phi, double theta, double psi, const bool waitUntilReached, const int waitTimeout) {
00430         IKGoto(x, y, z, phi, theta, psi, waitUntilReached, 100, waitTimeout);
00431 }
00432 
00433 void CikBase::moveRobotTo(std::vector<double> coordinates, const bool waitUntilReached, const int waitTimeout) {
00434         IKGoto(coordinates.at(0), coordinates.at(1), coordinates.at(2), coordinates.at(3), coordinates.at(4), coordinates.at(5), waitUntilReached, 100, waitTimeout);
00435 }
 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