kni_test.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-2009 Neuronics AG
00004  *   Check out the AUTHORS file for detailed contact information.
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  *   This program is distributed in the hope that it will be useful,
00010  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  *   GNU General Public License for more details.
00013  *   You should have received a copy of the GNU General Public License
00014  *   along with this program; if not, write to the Free Software
00015  *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00016  **********************************************************************************/
00017 
00019 // test.cpp
00020 // test program for KNI libraries
00022 
00023 #include "kniBase.h"
00024 #include <iostream>
00025 #include <cstdio>
00026 #include <memory>
00027 #include <vector>
00028 #include <fstream>
00030 #define LEFT false
00031 #define RIGHT true
00032 
00033 
00034 const double PI = 3.14159265358979323846;
00035 
00036 struct TPoint {
00037         double X, Y, Z;
00038         double phi, theta, psi;
00039 };
00040 
00041 struct TCurrentMot {
00042         int idx;
00043         bool running;
00044         bool dir;
00045 };
00046 
00047 struct Tpos{
00048         static std::vector<int> x,y,z,u,v,w;
00049         static const int xArr[], yArr[], zArr[], uArr[], vArr[], wArr[];
00050 };
00051 
00052 //Katana obj.
00053 std::auto_ptr<CLMBase> katana;
00055 
00056 int main(int argc, char *argv[]) {
00057 
00058         if (argc != 3) {
00059                 std::cout << "---------------------------------\n";
00060                 std::cout << "usage for socket connection: kni_test CONFIGFILE IP_ADDR\n";
00061                 std::cout << "---------------------------------\n";
00062                 return 0;
00063         }
00064 
00065         std::cout << "-----------------\n";
00066         std::cout << "KNI_TEST DEMO STARTED\n";
00067         std::cout << "-----------------\n";
00068 
00069         //----------------------------------------------------------------//
00070         //open device:
00071         //----------------------------------------------------------------//
00072 
00073                 
00074         std::auto_ptr<CCdlSocket> device;
00075         std::auto_ptr<CCplSerialCRC> protocol;
00076 
00077         try {
00078 
00079                 int port = 5566;
00080                 device.reset(new CCdlSocket(argv[2], port));
00081 
00082                 std::cout << "-------------------------------------------\n";
00083                 std::cout << "success:  port " << port << " open\n";
00084                 std::cout << "-------------------------------------------\n";
00085 
00086                 //--------------------------------------------------------//
00087                 //init protocol:
00088                 //--------------------------------------------------------//
00089 
00090                 protocol.reset(new CCplSerialCRC());
00091                 std::cout << "-------------------------------------------\n";
00092                 std::cout << "success: protocol class instantiated\n";
00093                 std::cout << "-------------------------------------------\n";
00094                 protocol->init(device.get()); //fails if no response from Katana
00095                 std::cout << "-------------------------------------------\n";
00096                 std::cout << "success: communication with Katana initialized\n";
00097                 std::cout << "-------------------------------------------\n";
00098 
00099 
00100                 //--------------------------------------------------------//
00101                 //init robot:
00102                 //--------------------------------------------------------//
00103                 katana.reset(new CLMBase());
00104                 katana->create(argv[1], protocol.get());
00105 
00106 
00107         } catch(Exception &e) {
00108                 std::cout << "ERROR: " << e.message() << std::endl;
00109                 return -1;
00110         }
00111         std::cout << "-------------------------------------------\n";
00112         std::cout << "success: katana initialized\n";
00113         std::cout << "-------------------------------------------\n";
00114 
00115         //set linear velocity to 60
00116         katana->setMaximumLinearVelocity(60);
00117         
00118         // declare information variables
00119         int nOfMot = katana->getNumberOfMotors(); // number of motors
00120         int controller; // controller type: 0 for position, 1 for current
00121         const char* model; // katana model name
00122         std::vector<TPoint> points(0); // list of points used for motion
00123         
00124         // declare temporary variables
00125         std::vector<int> encoders(nOfMot, 0);
00126         std::vector<double> pose(6, 0.0);
00127         TCurrentMot mot[6];
00128         for (int i = 0; i< 6; i++){
00129                 mot[i].running = false;
00130                 mot[i].idx = i;
00131                 mot[i].dir = RIGHT;
00132         }
00133         int tmp_int;
00134         
00135         // calibration
00136         std::cout << "- Calibrating Katana, please wait for termination..." << std::endl;
00137         katana->calibrate();
00138         std::cout << "   ...done." << std::endl;
00139         
00140         // get controller information
00141         std::cout << "- Check if Katana has position or current controllers..." << std::endl;
00142         controller = katana->getCurrentControllerType(1);
00143         for(short motorNumber = 1; motorNumber < nOfMot; ++motorNumber) {
00144                 tmp_int = katana->getCurrentControllerType(motorNumber+1);
00145                 if (tmp_int != controller) {
00146                         std::cout << "*** ERROR: Katana has mixed controller types on its axes! ***" << std::endl;
00147                         return 1;
00148                 }
00149         }
00150         std::cout << "   Katana has all ";
00151         controller == 0 ? std::cout << "position" : std::cout << "current";
00152         std::cout << " controllers." << std::endl;
00153         std::cout << "   ...done." << std::endl;
00154         
00155         // read current force if current controller installed
00156         if (controller == 1) {
00157                 std::cout << "- Read forces..." << std::endl;
00158                 for (short motorNumber = 0; motorNumber < nOfMot; ++motorNumber) {
00159                         std::cout << "   Motor " << (motorNumber+1) << ": " << katana->getForce(motorNumber+1) << std::endl;
00160                 }
00161                 std::cout << "   ...done." << std::endl;
00162         }
00163         
00164         // get Katana model name
00165         std::cout << "- Get Katana model name..." << std ::endl;
00166         model = katana->GetBase()->GetGNL()->modelName;
00167         std::cout << "   " << model << std::endl;
00168         std::cout << "   ...done." << std::endl;
00169         
00170         // switch motors off
00171         std::cout << "- Switch robot off..." << std ::endl;
00172         katana->switchRobotOff();
00173         std::cout << "   Robot off" << std::endl;
00174         std::cout << "   ...done." << std::endl;
00175         
00176         // get robot encoders
00177         std::cout << "- Read robot encoders..." << std ::endl;
00178         std::cout << "   Encoder values:";
00179         katana->getRobotEncoders(encoders.begin(), encoders.end());
00180         for (std::vector<int>::iterator i= encoders.begin(); i != encoders.end(); ++i) {
00181                 std::cout << " " << *i;
00182         }
00183         std::cout << std::endl;
00184         std::cout << "   ...done." << std::endl;
00185         
00186         // switch motors on
00187         std::cout << "- Switch motors on..." << std ::endl;
00188         for (short motorNumber = 0; motorNumber < nOfMot; ++motorNumber) {
00189                 katana->switchMotorOn((short)motorNumber);
00190                 std::cout << "   Motor " << (motorNumber+1) << " on" << std::endl;
00191         }
00192         std::cout << "   ...done." << std::endl;
00193 
00194         // move single axes (inc, dec, mov, incDegrees, decDegrees, movDegrees, moveMotorByEnc, moveMotorBy, moveMotorToEnc, moveMotorTo)
00195         std::cout << "- Move single axes..." << std ::endl;
00196         katana->dec(0, 10000, true);
00197         std::cout << "   Motor 1 decreased by 10000 encoders" << std::endl;
00198         katana->inc(1, 10000, true);
00199         std::cout << "   Motor 2 increased by 10000 encoders" << std::endl;
00200         katana->decDegrees(2, 70, true);
00201         std::cout << "   Motor 3 decreased by 70 degrees" << std::endl;
00202         katana->mov(3, 20000, true);
00203         std::cout << "   " << "Motor 4 moved to encoder position 20000" << std::endl;
00204         katana->movDegrees(4, 90, true);
00205         std::cout << "   Motor 5 moved to position 90 degrees" << std::endl;
00206         katana->incDegrees(5, -35, true);
00207         std::cout << "   Motor 6 increased by -35 degrees" << std::endl;
00208         katana->moveMotorBy(0, 0.2, true);
00209         std::cout << "   Motor 1 moved by 0.2 rad" << std::endl;
00210         katana->moveMotorByEnc(1, -5000, true);
00211         std::cout << "   Motor 2 moved by -5000 encoders" << std::endl;
00212         katana->moveMotorTo(2, 3.1, true);
00213         std::cout << "   Motor 3 moved to 3.1 rad" << std::endl;
00214         katana->moveMotorToEnc(3, 10000, true);
00215         std::cout << "   Motor 4 moved to 10000 encoders" << std::endl;
00216         std::cout << "   ...done." << std::endl;
00217         
00218         // move all axes (moveRobotToEnc)
00219         std::cout << "- Move all axes..." << std ::endl;
00220         for (short motorNumber = 0; motorNumber < nOfMot; ++motorNumber) {
00221                 encoders[motorNumber] = 30500;
00222                 if (motorNumber == 1 || motorNumber == 2)
00223                         encoders[motorNumber] = -30500;
00224         }
00225         katana->moveRobotToEnc(encoders, true);
00226         std::cout << "   Robot moved to encoder position 30500 -30500 -30500 30500 30500 30500" << std::endl;
00227         std::cout << "   ...done." << std::endl;
00228         
00229         // get coordinates
00230         std::cout << "- Get coordinates..." << std ::endl;
00231         {
00232                 double x, y, z, phi, theta, psi;
00233                 katana->getCoordinates(x, y, z, phi, theta, psi);
00234                 std::cout << "   Current coordinates: " << x << " " << y << " " << z << " " << phi << " " << theta << " " << psi << std::endl;
00235         }
00236         std::cout << "   ...done." << std::endl;
00237         
00238         // get coordinates from given encoders
00239         std::cout << "- Get coordinates from given encoders..." << std ::endl;
00240         {
00241                 encoders[0] = encoders[3] = encoders[4] = encoders[5] = 25000;
00242                 encoders[1] = encoders[2] = -25000;
00243                 katana->getCoordinatesFromEncoders(pose, encoders);
00244                 std::cout << "   Encoders: 25000 -25000 -25000 25000 25000 25000" << std::endl;
00245                 std::cout << "   Coordinates: " << pose[0] << " " << pose[1] << " " << pose[2] << " " << pose[3] << " " << pose[4] << " " << pose[5] << std::endl;
00246         }
00247         std::cout << "   ...done." << std::endl;
00248         
00249         // calculate inverse kinematics
00250         std::cout << "- Calculate inverse kinematics..." << std ::endl;
00251         {
00252                 katana->IKCalculate(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], encoders.begin());
00253                 std::cout << "   encoders.size(): " << encoders.size() << std::endl;
00254                 std::cout << "   Possible encoders: " << encoders[0] << " " << encoders[1] << " " << encoders[2] << " " << encoders[3] << " " << encoders[4] << " " << encoders[5] << std::endl;
00255         }
00256         std::cout << "   ...done." << std::endl;
00257         
00258         // move robot to pose
00259         std::cout << "- Move robot to pose..." << std ::endl;
00260         katana->moveRobotTo(pose, true);
00261         std::cout << "   Coordinates: " << pose[0] << " " << pose[1] << " " << pose[2] << " " << pose[3] << " " << pose[4] << " " << pose[5] << std::endl;
00262         std::cout << "   ...done." << std::endl;
00263         
00264         // load points file
00265         {
00266                 using namespace std;
00267                 cout << "- Load points file..." << endl;
00268                 ifstream listfile(model);
00269                 if(!listfile) {
00270                         cout << "*** ERROR: File '" << model << "' not found or access denied! ***" << endl;
00271                         return 1;
00272                 }
00273                 string line;
00274                 vector<string> tokens;
00275                 const string delimiter = ",";
00276                 int lines = 0;
00277                 while(!listfile.eof()) {
00278                         listfile >> line;
00279                         string::size_type lastPos = line.find_first_not_of(delimiter, 0);
00280                         string::size_type pos     = line.find_first_of(delimiter, lastPos);
00281                         while (string::npos != pos || string::npos != lastPos) {
00282                                 // Found a token, add it to the vector.
00283                                 tokens.push_back(line.substr(lastPos, pos - lastPos));
00284                                 // Skip delimiters.  Note the "not_of"
00285                                 lastPos = line.find_first_not_of(delimiter, pos);
00286                                 // Find next "non-delimiter"
00287                                 pos = line.find_first_of(delimiter, lastPos);
00288                         }
00289                         TPoint point;
00290                         point.X = atof((tokens.at(0)).data());
00291                         point.Y = atof((tokens.at(1)).data());
00292                         point.Z = atof((tokens.at(2)).data());
00293                         point.phi = atof((tokens.at(3)).data());
00294                         point.theta = atof((tokens.at(4)).data());
00295                         point.psi = atof((tokens.at(5)).data());
00296                         points.push_back( point );
00297                         ++lines;
00298                         tokens.clear();
00299                 }
00300                 cout << "   " << lines << " points loaded." << endl;
00301                 cout << "   ...done." << endl;
00302         }
00303         
00304         // move to point in list
00305         std::cout << "- Move in point list..." << std ::endl;
00306         {
00307                 int i, j;
00308                 bool wait;
00309                 std::cout << "   Single p2p movements..." << std::flush;
00310                 for (i = 0; i < (int)points.size(); ++i) {
00311                         katana->moveRobotTo(points[i].X, points[i].Y, points[i].Z, points[i].phi, points[i].theta, points[i].psi);
00312                 }
00313                 std::cout << " ...done." << std::endl;
00314                 std::cout << "   Single linear movements..." << std::flush;
00315                 for (i = 0; i < (int)points.size(); ++i) {
00316                         katana->moveRobotLinearTo(points[i].X, points[i].Y, points[i].Z, points[i].phi, points[i].theta, points[i].psi);
00317                 }
00318                 std::cout << " ...done." << std::endl;
00319                 std::cout << "   Concatenated p2p movements..." << std::flush;
00320                 for (i = 0; i < (int)points.size(); ++i) {
00321                         j = (i + points.size() - 1) % points.size();
00322                         wait = false;
00323                         if (i == ((int)points.size()-1)) wait = true;
00324                         katana->movP2P(points[j].X, points[j].Y, points[j].Z, points[j].phi, points[j].theta, points[j].psi, points[i].X, points[i].Y, points[i].Z, points[i].phi, points[i].theta, points[i].psi, true, 70.0, wait);
00325                 }
00326                 std::cout << " ...done." << std::endl;
00327                 std::cout << "   Concatenated linear movements..." << std::flush;
00328                 for (i = 0; i < (int)points.size(); ++i) {
00329                         j = (i + points.size() - 1) % points.size();
00330                         wait = false;
00331                         if (i == ((int)points.size()-1)) wait = true;
00332                         katana->movLM2P(points[j].X, points[j].Y, points[j].Z, points[j].phi, points[j].theta, points[j].psi, points[i].X, points[i].Y, points[i].Z, points[i].phi, points[i].theta, points[i].psi, true, 100.0, wait);
00333                 }
00334                 std::cout << " ...done." << std::endl;
00335         }
00336         std::cout << "   ...done." << std::endl;
00337         
00338         // move all axes (moveRobotToEnc)
00339         std::cout << "- Move all axes..." << std ::endl;
00340         for (short motorNumber = 0; motorNumber < nOfMot; ++motorNumber) {
00341                 encoders[motorNumber] = 30500;
00342                 if (motorNumber == 1 || motorNumber == 2)
00343                         encoders[motorNumber] = -30500;
00344         }
00345         katana->moveRobotToEnc(encoders, true);
00346         std::cout << "   Robot moved to encoder position 30500 -30500 -30500 30500 30500 30500" << std::endl;
00347         std::cout << "   ...done." << std::endl;
00348         
00349         return 0;
00350 }
00352 


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