control.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-2008 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  **********************************************************************************/
00018 // control.cpp
00019 // demo program for KNI libraries
00021 #include "kniBase.h"
00022 #include <iostream>
00023 #include <cstdio>
00024 #include <memory>
00025 #include <vector>
00026 #include <fstream>
00027 #include <pthread.h>
00029 #ifdef WIN32
00030 #       include <conio.h>
00031 #else //LINUX
00032 #       include "keyboard.h"
00033 #endif
00034 #define LEFT false
00035 #define RIGHT true
00036 
00037 //Thread structs:
00038 pthread_mutex_t mutex;
00039 
00040 struct TPoint {
00041         double X, Y, Z;
00042         double phi, theta, psi;
00043 };
00044 
00045 struct TCurrentMot {
00046         int idx;
00047         bool running;
00048         bool dir;
00049 };
00050 
00051 struct Tpos{
00052         static std::vector<int> x,y,z,u,v,w;
00053         static const int xArr[], yArr[], zArr[], uArr[], vArr[], wArr[];
00054 };
00055 //Katana obj.
00056 std::auto_ptr<CLMBase> katana;
00057 //std::vector<int> Foo::vec(array, array + sizeof(array)/sizeof(*array));
00058 //positionen, hard-coded. Use values from file instead
00059 const int Tpos::xArr[] = {30206, -23393, -3066, 14454, 30000, 30000};
00060 const int Tpos::yArr[] = {24327, -7837, -16796, 5803, 30290, 31000};
00061 const int Tpos::zArr[] = {24327, -7837, -16796, 5802, 30290, 10924};
00062 const int Tpos::uArr[] = {5333, -13791, -9985, 11449, 30996, 12063};
00063 const int Tpos::vArr[] = {-3799, -5703, -11676, 8210, 30995, 12063};
00064 const int Tpos::wArr[] = {-3799, -5703, -11676, 8210, 30995, 30992};
00065 std::vector<int> Tpos::x(xArr, xArr + sizeof(xArr)/sizeof(*xArr));
00066 std::vector<int> Tpos::y(yArr, yArr + sizeof(yArr)/sizeof(*yArr));
00067 std::vector<int> Tpos::z(zArr, zArr + sizeof(zArr)/sizeof(*zArr));
00068 std::vector<int> Tpos::u(uArr, uArr + sizeof(uArr)/sizeof(*uArr));
00069 std::vector<int> Tpos::v(vArr, vArr + sizeof(vArr)/sizeof(*vArr));
00070 std::vector<int> Tpos::w(wArr, wArr + sizeof(wArr)/sizeof(*wArr));
00071 std::vector<TPoint> points(0);
00072 void StartPointlistMovement();
00073 void StartProgram(int index);
00074 pthread_t tid;
00075 void* RunProgram(void*);
00076 pid_t threadPid;
00077 int retVal = 0;
00078 bool progRunning = false;
00079 const double PI = 3.14159265358979323846;
00081 void DisplayHelp() {
00082         std::cout << "-------------------------------------------       \n";
00083         std::cout << "?: Display this help\n";
00084         std::cout << "c: Calibrate the Katana\n";
00085         std::cout << "b: Read current controller types\n";
00086         std::cout << "e: Read the current encoder values\n";
00087         std::cout << "E: Read the current force values\n";
00088         std::cout << "o: Switch motors off/on (Default: On)\n";
00089         std::cout << "r: Switch angle format: Radian/Degree (Default: Rad)\n";
00090         std::cout << "x: Read the current position\n";
00091         std::cout << "v: Set the velocity limits for all motors seperately\n";
00092         std::cout << "V: Set the velocity limits for all motors (or for the TCP if in linear movement mode)\n";
00093         std::cout << "a: Set the acceleration limits for all motors seperately\n";
00094         std::cout << "A: Set the acceleration limits for all motors\n";
00095         std::cout << ",: Set the force limits for all motors\n";
00096         std::cout << "w: Read the velocity limits of all motors \n";
00097         std::cout << "W: Read the acceleration limits of all motors     \n";
00098         std::cout << "q: Read the Sensors\n";
00099         std::cout << "y: Set a new position using IK\n";
00100         std::cout << "l: Switch on/off linear movements\n";
00101         std::cout << "<: Add a point to the point list\n";
00102         std::cout << ">: Move to a specific point\n";
00103         std::cout << " : (space) Move to the next point in the point list\n";
00104         std::cout << "=: write pointlist to file\n";
00105         std::cout << "(: calculate DK from any encoders\n";
00106         std::cout << "f: read pointlist from file\n";
00107         std::cout << "g: Open Gripper\n";
00108         std::cout << "h: Close Gripper\n";
00109         std::cout << "n: Set the speed collision limit for all motors seperately\n";
00110         std::cout << "N: Set the speed collision limit for all motors\n";
00111         std::cout << "s: Set the position collision limit for all motors seperately\n";
00112         std::cout << "S: Set the position collision limit for all motors\n";
00113         std::cout << "t: Switch collision limit on\n";
00114         std::cout << "T: Switch collision limit off\n";
00115         std::cout << "u: Unblock motors after crash\n";
00116         std::cout << "d: Move motor to degrees\n";
00117         std::cout << "z: Set TCP offset\n\n\n";
00118         std::cout << "Keyboard of Katana, use the following keys:\n\n";
00119         std::cout << "1: Move motor1 left\n";
00120         std::cout << "2: Move motor1 right\n";
00121         std::cout << "3: Move motor2 left\n";
00122         std::cout << "4: Move motor2 right\n";
00123         std::cout << "5: Move motor3 left\n";
00124         std::cout << "6: Move motor3 right\n";
00125         std::cout << "7: Move motor4 left\n";
00126         std::cout << "8: Move motor4 right\n";
00127         std::cout << "9: Move motor5 left\n";
00128         std::cout << "0: Move motor5 right\n";
00129         std::cout << "/: Move motor6 left\n";
00130         std::cout << "*: Move motor6 right\n";
00131         std::cout << ".: Toggle Step mode\n";
00132         std::cout << "+: Increase step size\n";
00133         std::cout << "-: Decrease step size\n\n";
00134         std::cout << "$: Start/Stop Program\n";
00135         std::cout << "p: Start/Stop movement through points list\n\n";
00136 }
00138 int main(int argc, char *argv[]) {
00139 
00140         if (argc != 3) {
00141                 std::cout << "---------------------------------\n";
00142                 std::cout << "usage: control CONFIGFILE IP_ADDR\n";
00143                 std::cout << "---------------------------------\n";
00144                 return 0;
00145         }
00146 
00147         std::cout << "--------------------\n";
00148         std::cout << "CONTROL DEMO STARTED\n";
00149         std::cout << "--------------------\n";
00150 
00151         //----------------------------------------------------------------//
00152         //open device: a serial port is opened in this case
00153         //----------------------------------------------------------------//
00154 
00155                 
00156         std::auto_ptr<CCdlSocket> device;
00157         std::auto_ptr<CCplSerialCRC> protocol;
00158 
00159         try {
00160 
00161                 int port = 5566;
00162                 device.reset(new CCdlSocket(argv[2], port));
00163 
00164                 std::cout << "-------------------------------------------\n";
00165                 std::cout << "success:  port " << port << " open\n";
00166                 std::cout << "-------------------------------------------\n";
00167 
00168                 //--------------------------------------------------------//
00169                 //init protocol:
00170                 //--------------------------------------------------------//
00171 
00172                 protocol.reset(new CCplSerialCRC());
00173                 std::cout << "-------------------------------------------\n";
00174                 std::cout << "success: protocol class instantiated\n";
00175                 std::cout << "-------------------------------------------\n";
00176                 protocol->init(device.get()); //fails if no response from Katana
00177                 std::cout << "-------------------------------------------\n";
00178                 std::cout << "success: communication with Katana initialized\n";
00179                 std::cout << "-------------------------------------------\n";
00180 
00181 
00182                 //--------------------------------------------------------//
00183                 //init robot:
00184                 //--------------------------------------------------------//
00185                 katana.reset(new CLMBase());
00186                 katana->create(argv[1], protocol.get());
00187 
00188 
00189         } catch(Exception &e) {
00190                 std::cout << "ERROR: " << e.message() << std::endl;
00191                 return -1;
00192         }
00193         std::cout << "-------------------------------------------\n";
00194         std::cout << "success: katana initialized\n";
00195         std::cout << "-------------------------------------------\n";
00196 
00197         DisplayHelp();
00198 
00199         short counter = 0;
00200         bool loop = true;
00201         int pos = 0;
00202         bool DispInRad = true;
00203         bool IsOff = false;
00204         bool useLinearMode = false;
00205         double RadToDeg = 1.0;
00206         std::vector<int> encoders(katana->getNumberOfMotors(), 0);
00207         const TKatMOT* motors;
00208         TCurrentMot mot[6];
00209         for (int i = 0; i< 6; i++){
00210                 mot[i].running = false;
00211                 mot[i].idx = i;
00212                 mot[i].dir = true;
00213                 mot[i].dir = RIGHT;
00214         }
00215         bool stepMode = false;
00216         int stepSize = 100;
00217         bool runProgram = false;
00218         //pthread_mutex_init(&mutex, 0);
00219         //pthread_mutex_lock(&mutex);
00220         //pthread_mutex_unlock(&mutex);
00221         //set sensor controller values
00222         /*TSctDesc  sctdesc[1] = {{15, 8, 16}};         //sctID,resol,count
00223         CSctBase* sctarr     = new CSctBase[1];         //create sensor-ctrl class
00224         TKatSCT   katsct     = {1, sctarr, sctdesc};    //fill TKatSCT structure
00225         */
00226         CSctBase* sensctrl      = &katana->GetBase()->GetSCT()->arr[0];
00227         int limit;
00228         
00229         //set linear velocity to 60
00230         katana->setMaximumLinearVelocity(60);
00231         
00232         while (loop) {
00233                 double arr_pos[6];
00234 
00235                 int input = _getch();
00236                 if(progRunning){
00237                         //Thread killen:
00238                         progRunning = false;
00239                         continue;
00240                 }
00241 
00242                 try {
00243                         
00244                         switch (input) {
00245                                 
00246 /*                      case 'i': //VK_I: Test routine, connected P2P movement
00247                                 {
00248                                 double posX = points[counter].X;
00249                                 double posY = points[counter].Y;
00250                                 double posZ = points[counter].Z;
00251                                 double posPhi = points[counter].phi;
00252                                 double posTheta = points[counter].theta;
00253                                 double posPsi = points[counter].psi;
00254                                 katana->moveRobotTo(posX, posY, posZ, posPhi, posTheta, posPsi, false);
00255                                 counter++;
00256                                 counter = counter % ((short) points.size());
00257                                 katana->movP2P(posX, posY, posZ, posPhi, posTheta, posPsi, points[counter].X, points[counter].Y, points[counter].Z, points[counter].phi, points[counter].theta, points[counter].psi, true, 20, true);
00258                                 counter++;
00259                                 counter = counter % ((short) points.size());
00260                                 break;
00261                                 }
00262 */
00263                         case '(':
00264                         {
00265                                 std::vector<double> pose_result(6, 0);
00266                                 std::vector<int> etc;
00267                                 etc.push_back(20000);
00268                                 etc.push_back(-20000);
00269                                 etc.push_back(-20000);
00270                                 etc.push_back(20000);
00271                                 etc.push_back(20000);
00272                                 etc.push_back(20000);
00273                                 katana->getCoordinatesFromEncoders(pose_result, etc);
00274                                 std::cout.precision(6);
00275                                 std::cout << "\n------------------------------------\n";
00276                                 std::cout << "X: "     << pose_result.at(0) << "\n";
00277                                 std::cout << "Y: "     << pose_result.at(1) << "\n";
00278                                 std::cout << "Z: "     << pose_result.at(2) << "\n";
00279                                 std::cout << "phi: "  << RadToDeg*pose_result.at(3) << "\n";
00280                                 std::cout << "theta: "  << RadToDeg*pose_result.at(4) << "\n";
00281                                 std::cout << "psi: " << RadToDeg*pose_result.at(5) << "\n";
00282                                 std::cout << "------------------------------------\n";
00283                         }
00284                                 break;
00285 
00286                         case '.':
00287                                 if(stepMode == false){
00288                                         stepMode = true;
00289                                         std::cout << "Step Mode ON \n" << std::endl;
00290                                 }
00291                                 else{
00292                                         stepMode = false;
00293                                         std::cout << "Step Mode OFF \n" << std::endl;
00294                                 }
00295                                 break;
00296                         case '+':
00297                                 if(stepSize <= 3000){
00298                                         stepSize += 300;
00299                                         std::cout << "Step Size =  " << stepSize << std::endl;
00300                                         
00301                                 }
00302                                 break;
00303                         case '-':
00304                                 if(stepSize >= 330){
00305                                         stepSize -= 300;
00306                                         std::cout << "Step Size =  " << stepSize << std::endl;
00307                                 }
00308                                 break;
00309                         case '1':
00310                                 if((stepMode == true) ||(mot[0].running == false || (mot[0].running == true && mot[0].dir == LEFT))){
00311                                         mot[0].idx = 0;
00312                                         motors = katana->GetBase()->GetMOT();
00313                                         pos = motors->arr[mot[0].idx].GetEncoderMinPos();
00314                                         if(motors->arr[mot[0].idx].checkEncoderInRange(mot[0].idx)){
00315                                                 if(stepMode){
00316                                                         pos = katana->getMotorEncoders(0) + stepSize;
00317                                                 }
00318                                                 std::cout << "\nMoving to max encoder position " << pos << std::endl;
00319                                                 katana->moveMotorToEnc(mot[0].idx, pos);
00320                                                 mot[0].running = true;
00321                                                 mot[0].dir = RIGHT;
00322                                         }
00323                                         else{
00324                                                 std::cout << "\nEncoder target value out of range! \n" << std::endl;
00325                                         }
00326                                 }
00327                                 else{
00328                                                 katana->freezeMotor(0);
00329                                                 mot[0].running = false;
00330                                 }
00331                                 break;
00332                         case '2':
00333                                 if((stepMode == true) ||(mot[0].running == false || (mot[0].running == true && mot[0].dir == RIGHT))){
00334                                         mot[0].idx = 0;
00335                                         //motors = katana->GetBase()->GetMOT();
00336                                         motors = katana->GetBase()->GetMOT();
00337                                         pos = motors->arr[mot[0].idx].GetEncoderMaxPos();
00338                                         if(motors->arr[mot[0].idx].checkEncoderInRange(mot[0].idx)){
00339                                                 if(stepMode){
00340                                                         pos = katana->getMotorEncoders(0) - stepSize;
00341                                                 }
00342                                                 std::cout << "\nMoving to max encoder position " << pos <<  std::endl;
00343                                                 katana->moveMotorToEnc(mot[0].idx, pos);
00344                                                 mot[0].running = true;
00345                                                 mot[0].dir = LEFT;
00346                                         }
00347                                         else{
00348                                                 std::cout << "\nEncoder target value out of range! \n" << std::endl;
00349                                         }
00350                                 }
00351                                 else{
00352                                         katana->freezeMotor(0);
00353                                         mot[0].running = false;
00354                                 }
00355                                 break;
00356                         case '3':
00357                                 if((stepMode == true) ||(mot[1].running == false || (mot[1].running == true && mot[1].dir == LEFT))){
00358                                         mot[1].idx = 1;
00359                                         motors = katana->GetBase()->GetMOT();
00360                                         pos = motors->arr[mot[1].idx].GetEncoderMinPos();
00361                                         if(motors->arr[mot[1].idx].checkEncoderInRange(mot[1].idx)){
00362                                                 if(stepMode){
00363                                                         pos = katana->getMotorEncoders(1) + stepSize;
00364                                                 }
00365                                                 std::cout << "\nMoving to max encoder position " << pos << std::endl;
00366                                                 katana->moveMotorToEnc(mot[1].idx, pos);
00367                                                 mot[1].running = true;
00368                                                 mot[1].dir = RIGHT;
00369                                         }
00370                                         else{
00371                                                 std::cout << "\nEncoder target value out of range! \n" << std::endl;
00372                                         }
00373                                 }
00374                                 else{
00375                                         katana->freezeMotor(1);
00376                                         mot[1].running = false;
00377                                 }
00378                                 break;
00379                         case '4':
00380                                 if((stepMode == true) ||(mot[1].running == false || (mot[1].running == true && mot[1].dir == RIGHT))){
00381                                         mot[1].idx = 1;
00382                                         //motors = katana->GetBase()->GetMOT();
00383                                         motors = katana->GetBase()->GetMOT();
00384                                         pos = motors->arr[mot[1].idx].GetEncoderMaxPos();
00385                                         if(motors->arr[mot[1].idx].checkEncoderInRange(mot[1].idx)){
00386                                                 if(stepMode){
00387                                                         pos = katana->getMotorEncoders(1) - stepSize;
00388                                                 }
00389                                                 std::cout << "\nMoving to max encoder position " << pos <<  std::endl;
00390                                                 katana->moveMotorToEnc(mot[1].idx, pos);
00391                                                 mot[1].running = true;
00392                                                 mot[1].dir = LEFT;
00393                                         }
00394                                         else{
00395                                                 std::cout << "\nEncoder target value out of range! \n" << std::endl;
00396                                         }
00397                                 }
00398                                 else{
00399                                         katana->freezeMotor(1);
00400                                         mot[1].running = false;
00401                                 }
00402                                 break;
00403                         case '5':
00404                                 if((stepMode == true) ||(mot[2].running == false || (mot[2].running == true && mot[2].dir == LEFT))){
00405                                         mot[2].idx = 2;
00406                                         motors = katana->GetBase()->GetMOT();
00407                                         pos = motors->arr[mot[2].idx].GetEncoderMinPos();
00408                                         if(motors->arr[mot[2].idx].checkEncoderInRange(mot[2].idx)){
00409                                                 if(stepMode){
00410                                                         pos = katana->getMotorEncoders(2) + stepSize;
00411                                                 }
00412                                                 std::cout << "\nMoving to max encoder position " << pos << std::endl;
00413                                                 katana->moveMotorToEnc(mot[2].idx, pos);
00414                                                 mot[2].running = true;
00415                                                 mot[2].dir = RIGHT;
00416                                         }
00417                                         else{
00418                                                 std::cout << "\nErunProgram =ncoder target value out of range! \n" << std::endl;
00419                                         }
00420                                 }
00421                                 else{
00422                                         katana->freezeMotor(2);
00423                                         mot[2].running = false;
00424                                 }
00425                                 break;
00426                         case '6':
00427                                 if((stepMode == true) ||(mot[2].running == false || (mot[2].running == true && mot[2].dir == RIGHT))){
00428                                         mot[2].idx = 2;
00429                                         //motors = katana->GetBase()->GetMOT();
00430                                         motors = katana->GetBase()->GetMOT();
00431                                         pos = motors->arr[mot[2].idx].GetEncoderMaxPos();
00432                                         if(motors->arr[mot[2].idx].checkEncoderInRange(mot[2].idx)){
00433                                                 if(stepMode){
00434                                                         pos = katana->getMotorEncoders(2) - stepSize;
00435                                                 }
00436                                                 std::cout << "\nMoving to max encoder position " << pos <<  std::endl;
00437                                                 katana->moveMotorToEnc(mot[2].idx, pos);
00438                                                 mot[2].running = true;
00439                                                 mot[2].dir = LEFT;
00440                                         }
00441                                         else{
00442                                                 std::cout << "\nEncoder target value out of range! \n" << std::endl;
00443                                         }
00444                                 }
00445                                 else{
00446                                         katana->freezeMotor(2);
00447                                         mot[2].running = false;
00448                                 }
00449                                 break;
00450                         case '7':
00451                                 if((stepMode == true) ||(mot[3].running == false || (mot[3].running == true && mot[3].dir == LEFT))){
00452                                         mot[3].idx = 3;
00453                                         motors = katana->GetBase()->GetMOT();
00454                                         pos = motors->arr[mot[3].idx].GetEncoderMinPos();
00455                                         if(motors->arr[mot[3].idx].checkEncoderInRange(mot[3].idx)){
00456                                                 if(stepMode){
00457                                                         pos = katana->getMotorEncoders(3) + stepSize;
00458                                                 }
00459                                                 std::cout << "\nMoving to max encoder position " << pos << std::endl;
00460                                                 katana->moveMotorToEnc(mot[3].idx, pos);
00461                                                 mot[3].running = true;
00462                                                 mot[3].dir = RIGHT;
00463                                         }
00464                                         else{
00465                                                 std::cout << "\nEncoder target value out of range! \n" << std::endl;
00466                                         }
00467                                 }
00468                                 else{
00469                                         katana->freezeMotor(3);runProgram =
00470                                         mot[3].running = false;
00471                                 }
00472                                 break;
00473                         case '8':
00474                                 if((stepMode == true) ||(mot[3].running == false || (mot[3].running == true && mot[3].dir == RIGHT))){
00475                                         mot[3].idx = 3;
00476                                         //motors = katana->GetBase()->GetMOT();
00477                                         motors = katana->GetBase()->GetMOT();
00478                                         pos = motors->arr[mot[3].idx].GetEncoderMaxPos();
00479                                         if(motors->arr[mot[3].idx].checkEncoderInRange(mot[3].idx)){
00480                                                 if(stepMode){
00481                                                         pos = katana->getMotorEncoders(3) - stepSize;
00482                                                 }
00483                                                 std::cout << "\nMoving to max encoder position " << pos <<  std::endl;
00484                                                 katana->moveMotorToEnc(mot[3].idx, pos);
00485                                                 mot[3].running = true;
00486                                                 mot[3].dir = LEFT;
00487                                         }
00488                                         else{
00489                                                 std::cout << "\nEncoder target value out of range! \n" << std::endl;
00490                                         }
00491                                 }
00492                                 else{
00493                                         katana->freezeMotor(3);
00494                                         mot[3].running = false;
00495                                 }
00496                                 break;
00497                         case '9':
00498                                 if((stepMode == true) ||(mot[4].running == false || (mot[4].running == true && mot[4].dir == LEFT))){
00499                                         mot[4].idx = 4;
00500                                         motors = katana->GetBase()->GetMOT();
00501                                         pos = motors->arr[mot[4].idx].GetEncoderMinPos();
00502                                         if(motors->arr[mot[4].idx].checkEncoderInRange(mot[4].idx)){
00503                                                 if(stepMode){
00504                                                         pos = katana->getMotorEncoders(4) + stepSize;
00505                                                 }
00506                                                 std::cout << "\nMoving to max encoder position " << pos << std::endl;
00507                                                 katana->moveMotorToEnc(mot[4].idx, pos);
00508                                                 mot[4].running = true;
00509                                                 mot[4].dir = RIGHT;
00510                                         }
00511                                         else{
00512                                                 std::cout << "\nEncoder target value out of range! \n" << std::endl;
00513                                         }
00514                                 }
00515                                 else{
00516                                         katana->freezeMotor(4);
00517                                         mot[4].running = false;
00518                                 }
00519                                 break;
00520                         case '0':
00521                                 if((stepMode == true) ||(mot[4].running == false || (mot[4].running == true && mot[4].dir == RIGHT))){
00522                                         mot[4].idx = 4;
00523                                         //motors = katana->GetBase()->GetMOT();
00524                                         motors = katana->GetBase()->GetMOT();
00525                                         pos = motors->arr[mot[4].idx].GetEncoderMaxPos();
00526                                         if(motors->arr[mot[4].idx].checkEncoderInRange(mot[4].idx)){
00527                                                 if(stepMode){
00528                                                         pos = katana->getMotorEncoders(4) - stepSize;
00529                                                 }
00530                                                 std::cout << "\nMoving to max encoder position " << pos <<  std::endl;
00531                                                 katana->moveMotorToEnc(mot[4].idx, pos);
00532                                                 mot[4].running = true;
00533                                                 mot[4].dir = LEFT;
00534                                         }
00535                                         else{
00536                                                 std::cout << "\nEncoder target value out of range! \n" << std::endl;
00537                                         }
00538                                 }
00539                                 else{
00540                                         katana->freezeMotor(4);
00541                                         mot[4].running = false;
00542                                 }
00543                                 break;
00544                         case '/':
00545                                 if((stepMode == true) ||(mot[5].running == false || (mot[5].running == true && mot[5].dir == LEFT))){
00546                                         mot[5].idx = 5;
00547                                         motors = katana->GetBase()->GetMOT();
00548                                         pos = motors->arr[mot[0].idx].GetEncoderMinPos();
00549                                         if(motors->arr[mot[0].idx].checkEncoderInRange(mot[5].idx)){
00550                                                 if(stepMode){
00551                                                         pos = katana->getMotorEncoders(5) + stepSize;
00552                                                         std::cout << "\nMoving to max encoder position " << pos + 1000 << std::endl;
00553                                                         katana->moveMotorToEnc(mot[5].idx, pos);
00554                                                 }
00555                                                 else{
00556                                                         katana->openGripper();
00557                                                 }
00558                                                 mot[5].running = true;
00559                                                 mot[5].dir = RIGHT;
00560                                         }
00561                                         else{
00562                                                 std::cout << "\nEncoder target value out of range! \n" << std::endl;
00563                                         }
00564                                 }
00565                                 else{
00566                                         katana->freezeMotor(5);
00567                                         mot[5].running = false;
00568                                 }
00569                                 break;
00570                         case '*':
00571                                 if((stepMode == true) ||(mot[5].running == false || (mot[5].running == true && mot[5].dir == RIGHT))){
00572                                         mot[5].idx = 5;
00573                                         //motors = katana->GetBase()->GetMOT();
00574                                         motors = katana->GetBase()->GetMOT();
00575                                         pos = motors->arr[mot[5].idx].GetEncoderMaxPos();
00576                                         if(motors->arr[mot[5].idx].checkEncoderInRange(mot[5].idx)){
00577                                                 if(stepMode){
00578                                                         pos = katana->getMotorEncoders(5) - stepSize;
00579                                                         std::cout << "\nMoving to max encoder position " << pos <<  std::endl;
00580                                                         katana->moveMotorToEnc(mot[5].idx, pos);
00581                                                 }
00582                                                 else{
00583                                                         katana->closeGripper();
00584                                                 }
00585                                                 mot[5].running = true;
00586                                                 mot[5].dir = LEFT;
00587                                         }
00588                                         else{
00589                                                 std::cout << "\nEncoder target value out of range! \n" << std::endl;
00590                                         }
00591                                 }
00592                                 else{
00593                                         katana->freezeMotor(5);
00594                                         mot[5].running = false;
00595                                 }
00596                                 break;
00597                         case '$':
00598 //                              loop = false;
00599 //                              runProgram = true;
00600 //                              StartProgram(0);
00601                                 std::cout << "\n Adjust hardcoded pointlist and program for your katana before running the program!!!\n" << std::endl;
00602                                 break;
00603                         case '?':
00604                                 DisplayHelp();
00605                                 break;
00606 
00607                         case 'r':
00608                                 if(DispInRad) {
00609                                         DispInRad = false;
00610                                         RadToDeg = 180.0/PI;
00611                                         std::cout << "\nAngles in DEGREE !! \n" << std::endl;
00612                                 } else {
00613                                         DispInRad = true;
00614                                         RadToDeg = 1.0;
00615                                         std::cout << "\nAngles in RADIAN !! \n" << std::endl;
00616                                 }
00617                                 break;
00618 
00619 
00620                         case 'c': //VK_C (Calibration)
00621                                 std::cout << "\nCalibrating Katana, please wait for termination... \n" << std::flush;
00622                                 katana->calibrate();
00623                                 break;
00624 
00625                         case 'b': //VK_B (get current controller type)
00626                                 std::cout << "\nController types: " << std::endl;
00627                                 for (int i = 0; i < katana->getNumberOfMotors(); ++i) {
00628                                         std::cout << " " << (katana->getCurrentControllerType(i+1) == 0 ? "position" : "current");
00629                                 }
00630                                 std::cout << std::endl;
00631                                 break;
00632 
00633                         case 'e': //VK_E (read encoder values)
00634                                 std::cout << "\nEncoder values: " << std::endl;
00635                                 katana->getRobotEncoders(encoders.begin(), encoders.end());
00636                                 for (std::vector<int>::iterator i= encoders.begin(); i != encoders.end(); ++i) {
00637                                         std::cout << *i << " ";
00638                                 }
00639                                 std::cout << std::endl;
00640                                 break;
00641 
00642                         case 'E': //VK_E (read force values)
00643                                 std::cout << "\nForce values:" << std::endl;
00644                                 for (int i = 0; i < katana->getNumberOfMotors(); ++i) {
00645                                         std::cout << " " << (int)katana->getForce(i+1);
00646                                 }
00647                                 std::cout << std::endl;
00648                                 break;
00649 
00650                         case 'o': //VK_O (motors off/on)
00651                                 if(IsOff) {
00652                                         katana->switchRobotOn();
00653                                         IsOff = false;
00654                                         std::cout << "\nMotors on\n";
00655                                 } else {
00656                                         katana->switchRobotOff();
00657                                         IsOff = true;
00658                                         std::cout << "\nMotors off\n";
00659                                 }
00660                                 break;
00661 
00662                         case 'x': //VK_O (DK to screen)
00663                                 {
00664                                         TPoint p;
00665                                         katana->getCoordinates(p.X, p.Y, p.Z, p.phi, p.theta, p.psi);
00666 
00667                                         std::cout.precision(6);
00668                                         std::cout << "\n------------------------------------\n";
00669                                         std::cout << "X: "     << p.X << "\n";
00670                                         std::cout << "Y: "     << p.Y << "\n";
00671                                         std::cout << "Z: "     << p.Z << "\n";
00672                                         std::cout << "phi: "  << RadToDeg*p.phi << "\n";
00673                                         std::cout << "theta: "  << RadToDeg*p.theta << "\n";
00674                                         std::cout << "psi: " << RadToDeg*p.psi << "\n";
00675                                         std::cout << "------------------------------------\n";
00676 
00677                                         break;
00678                                 }
00679                         case 'q': //read sensors:
00680                                 {
00681                                         std::cout << "\nCurrent Sensor values:" << std::endl;
00682                                         const TSctDAT* data     = sensctrl->GetDAT();
00683                                         bool* change = new bool[data->cnt];
00684                                         byte* lastarr = new byte[data->cnt];
00685                                         sensctrl->recvDAT();
00686                                         for (int k=0; k<data->cnt; k++) {                                       //init stuff
00687                                                 change[k] = false;
00688                                                 lastarr[k] = (byte) data->arr[k];
00689                                         }
00690                                         //clock_t t = clock();                                                  //init timer
00691                                         sensctrl->recvDAT();                                                    //update sensor data
00692                                         for (int i=0; i<data->cnt; i++) {
00693                                                 std::cout.width(5);
00694                                                 std::cout << data->arr[i] << " ";                               //printout data 
00695                                         } std::cout << "\n";
00696                                         /*
00697                                         while (clock() - t < 25) {}                                             //wait 25 millisec
00698 
00699                                         for (int j=0; j<data->cnt; j++) {
00700                                                 change[j] |= (data->arr[j] != lastarr[j]);
00701                                                 lastarr[j] = data->arr[j];
00702                                         }
00703                                         */
00704                                         break;
00705                                 }
00706                         case 'v': //VK_V (Set max vel)
00707                                 {
00708                                         short velocity;
00709                                         std::cout << "\n\nSet maximum velocity for motors to: \n";
00710                                         for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) {
00711                                                 std::cout << motorNumber+1 << ": ";
00712                                                 std::cin >> velocity;
00713                                                 katana->setMotorVelocityLimit(motorNumber, velocity);
00714                                         }
00715                                 }
00716                                 break;
00717 
00718                         case 'V': //VK_V (Set max vel)
00719                                 {
00720                                         if(useLinearMode) {
00721                                                 double velocity;
00722                                                 std::cout << "\n\nSet the TCP velocity to: ";
00723                                                 std::cin >> velocity;
00724                                                 katana->setMaximumLinearVelocity(velocity);
00725                                                 katana->setRobotVelocityLimit(static_cast<short>(velocity));
00726                                         } else {
00727                                                 short velocity;
00728                                                 std::cout << "\n\nSet maximum velocity for all motors to: ";
00729                                                 std::cin >> velocity;
00730                                                 katana->setRobotVelocityLimit(velocity);
00731                                                 katana->setMaximumLinearVelocity(static_cast<double>(velocity));
00732                                         }
00733                                         std::cout << std::endl;
00734                                 }
00735                                 break;
00736 
00737                         case 'a': //VK_A (Set max acc)
00738                                 {
00739                                         short acceleration;
00740                                         std::cout << "\n\nSet maximum velocity for motors to: \n";
00741                                         for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) {
00742                                                 std::cout << motorNumber+1 << ": ";
00743                                                 std::cin >> acceleration;
00744                                                 katana->setMotorAccelerationLimit(motorNumber, acceleration);
00745                                         }
00746                                 }
00747                                 break;
00748                         case 'A': //VK_A (Set max acc)
00749                                 {
00750                                         short acceleration;
00751                                         std::cout << "\n\nSet maximum acceleration for all motors to: ";
00752                                         std::cin >> acceleration;
00753                                         std::cout << std::endl;
00754                                         katana->setRobotAccelerationLimit(acceleration);
00755                                 }
00756                                 break;
00757                         case ',': //set force limits for all motors
00758                                 {
00759                                         short limit;
00760                                         std::cout << "\nSet force limit for all motors to (%): ";
00761                                         std::cin >> limit;
00762                                         std::cout << std::endl;
00763                                         katana->setForceLimit(0, limit);
00764                                 }
00765                                 break;
00766 
00767                         case 'w': //VK_W (Read current max vel)
00768                                 {
00769                                         std::cout << "\nCurrent velocity limits:" << std::endl;
00770                                         for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber)
00771                                                 std::cout << motorNumber+1 << ": " << katana->getMotorVelocityLimit(motorNumber) << std::endl;
00772                                         std::cout << "linear: " << katana->getMaximumLinearVelocity() << std::endl;
00773                                         break;
00774                                 }
00775                         case 'W': //VK_W (Read current max acc)
00776                                 {
00777                                         std::cout << "\nCurrent acceleration limits:" << std::endl;
00778                                         for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber)
00779                                                 std::cout << motorNumber+1 << ": " << katana->getMotorAccelerationLimit(motorNumber) << std::endl;
00780                                         break;
00781                                 }
00782 
00783                         case 'l': //VK_L (switch linear mode)
00784                                 if(useLinearMode) {
00785                                         std::cout << "Switching to inverse kinematics movement mode\n";
00786                                 } else {
00787                                         std::cout << "Switching to linear movement mode\n";
00788                                 }
00789                                 useLinearMode = !useLinearMode;
00790                                 break;
00791                         case 'y':  //VK_Y (IKGoto)
00792                                 std::cout << "\n\nInsert cartesian parameters: \n";
00793                                 std::cout << "X: ";
00794                                 std::cin >> arr_pos[0];
00795                                 std::cout << "Y: ";
00796                                 std::cin >> arr_pos[1];
00797                                 std::cout << "Z: ";
00798                                 std::cin >> arr_pos[2];
00799                                 std::cout << "phi: ";
00800                                 std::cin >> arr_pos[3];
00801                                 std::cout << "theta: ";
00802                                 std::cin >> arr_pos[4];
00803                                 std::cout << "psi: ";
00804                                 std::cin >> arr_pos[5];
00805 
00806                                 arr_pos[3] = 1/RadToDeg*arr_pos[3];
00807                                 arr_pos[4] = 1/RadToDeg*arr_pos[4];
00808                                 arr_pos[5] = 1/RadToDeg*arr_pos[5];
00809 
00810                                 if(useLinearMode) {
00811                                         katana->moveRobotLinearTo(arr_pos[0], arr_pos[1], arr_pos[2], arr_pos[3], arr_pos[4], arr_pos[5]);
00812                                 } else {
00813                                         katana->moveRobotTo(arr_pos[0], arr_pos[1], arr_pos[2], arr_pos[3], arr_pos[4], arr_pos[5]);
00814                                 }
00815                                 break;
00816 
00817                         case '<':
00818                                 std::cout.precision(6);
00819                                 TPoint point;
00820                                 katana->getCoordinates(point.X, point.Y, point.Z, point.phi, point.theta, point.psi);
00821                                 std::cout << "Point: ";
00822                                 std::cout << "  X="    << point.X;
00823                                 std::cout << ", Y="    << point.Y;
00824                                 std::cout << ", Z="    << point.Z;
00825                                 std::cout << ", phi="  << RadToDeg*point.phi;
00826                                 std::cout << ", theta="<< RadToDeg*point.theta;
00827                                 std::cout << ", psi="  << RadToDeg*point.psi;
00828                                 std::cout << " ... added to point list as number ";
00829                                 std::cout << points.size() << std::endl;
00830                                 points.push_back(point);
00831                                 break;
00832 
00833                         case '>':
00834                                 std::cout.width(10);
00835                                 std::cout.precision(3);
00836                                 std::cout << "\nMoving to point? ";
00837                                 unsigned int pointNumber;
00838                                 std::cin >> pointNumber;
00839                                 if(pointNumber >= points.size()) {
00840                                         std::cout << "Invalid point number. You have only " << points.size() << " points in your list" << std::endl;
00841                                         break;
00842                                 }
00843                                 std::cout.width(6);
00844                                 std::cout << " x=" << points[pointNumber].X;
00845                                 std::cout.width(6);
00846                                 std::cout << " y=" << points[pointNumber].Y;
00847                                 std::cout.width(6);
00848                                 std::cout << " z=" << points[pointNumber].Z;
00849                                 std::cout.width(6);
00850                                 std::cout << " phi=" << RadToDeg*points[pointNumber].phi;
00851                                 std::cout.width(6);
00852                                 std::cout << " theta=" <<RadToDeg*points[pointNumber].theta;
00853                                 std::cout.width(6);
00854                                 std::cout << " psi=" << RadToDeg*points[pointNumber].psi;
00855                                 std::cout << std::endl;
00856                                 if(useLinearMode) {
00857                                         katana->moveRobotLinearTo(points[pointNumber].X, points[pointNumber].Y, points[pointNumber].Z, points[pointNumber].phi, points[pointNumber].theta, points[pointNumber].psi);
00858                                 } else {
00859                                         katana->moveRobotTo(points[pointNumber].X, points[pointNumber].Y, points[pointNumber].Z, points[pointNumber].phi, points[pointNumber].theta, points[pointNumber].psi);
00860                                 }
00861                                 break;
00862 
00863                         case 3:
00864                         case 4:
00865                         case 27: //VK_ESCAPE 
00866                                 loop = false;
00867                                 continue;
00868 
00869                         case ' ': //VK_SPACE
00870                                 //Move to the next point
00871                                 std::cout.width(10);
00872                                 std::cout.precision(3);
00873                                 std::cout << "Moving to point " << counter << ": ";
00874                                 std::cout.width(6);
00875                                 std::cout << " x=" << points[counter].X;
00876                                 std::cout.width(6);
00877                                 std::cout << " y=" << points[counter].Y;
00878                                 std::cout.width(6);
00879                                 std::cout << " z=" << points[counter].Z;
00880                                 std::cout.width(6);
00881                                 std::cout << " phi=" << RadToDeg*points[counter].phi;
00882                                 std::cout.width(6);
00883                                 std::cout << " theta=" <<RadToDeg*points[counter].theta;
00884                                 std::cout.width(6);
00885                                 std::cout << " psi=" << RadToDeg*points[counter].psi;
00886                                 std::cout << std::endl;
00887 
00888                                 if(useLinearMode) {
00889                                         katana->moveRobotLinearTo(points[counter].X, points[counter].Y, points[counter].Z, points[counter].phi, points[counter].theta, points[counter].psi);
00890                                 } else {
00891                                         katana->moveRobotTo(points[counter].X, points[counter].Y, points[counter].Z, points[counter].phi, points[counter].theta, points[counter].psi);
00892                                 }
00893                                 counter++;
00894                                 counter = counter % ((short) points.size());
00895                                 break;
00896 
00897                         case 'g':
00898                                 std::cout << "Opening gripper...\n";
00899                                 katana->openGripper();
00900                                 break;
00901                         case 'h':
00902                                 std::cout << "Close gripper...\n";
00903                                 katana->closeGripper();
00904                                 break;
00905                         case 'n':
00906                                 std::cout << "Set speed collision limit for motors to: \n";
00907                                 for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) {
00908                                         std::cout << motorNumber+1 << ": ";
00909                                         std::cin >> limit;
00910                                         katana->setSpeedCollisionLimit(motorNumber, limit);
00911                                 }
00912                                 break;
00913                         case 'N':
00914                                 std::cout << "Set speed collision limit for all motors to: \n";
00915                                 std::cin >> limit;
00916                                 for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) {
00917                                         katana->setSpeedCollisionLimit(motorNumber, limit);
00918                                 }
00919                                 break;
00920                         case 's':
00921                                 std::cout << "Set position collision limit for motors to: \n";
00922                                 for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) {
00923                                         std::cout << motorNumber+1 << ": ";
00924                                         std::cin >> limit;
00925                                         katana->setPositionCollisionLimit(motorNumber, limit);
00926                                 }
00927                                 break;
00928                         case 'S':
00929                                 std::cout << "Set position collision limit for all motors to: \n";
00930                                 std::cin >> limit;
00931                                 for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) {
00932                                         katana->setPositionCollisionLimit(motorNumber, limit);
00933                                 }
00934                                 break;
00935                         case 't':
00936                                 std::cout << "Collision detection enabled\n";
00937                                 katana->enableCrashLimits();
00938                                 break;
00939                         case 'T':
00940                                 std::cout << "WARNING: Collision detection disabled\n";
00941                                 katana->disableCrashLimits();
00942                                 break;
00943                         case 'u':
00944                                 std::cout << "Unblocking motors\n";
00945                                 katana->unBlock();
00946                                 break;
00947 
00948                         case 'f': {
00949                                         using namespace std;
00950 
00951                                         cout << "Loading which file?\n";
00952                                         string filename;
00953                                         cin >> filename;
00954 
00955                                         ifstream listfile(filename.c_str());
00956                                         if(!listfile) {
00957                                                 cout << "File not found or access denied." << endl;
00958                                                 break;
00959                                         }
00960 
00961                                         string line;
00962                                         vector<string> tokens;
00963                                         const string delimiter = ",";
00964 
00965                                         int lines = 0;
00966 
00967                                         while(!listfile.eof()) {
00968                                                 listfile >> line;
00969                                                 string::size_type lastPos = line.find_first_not_of(delimiter, 0);
00970                                                 string::size_type pos     = line.find_first_of(delimiter, lastPos);
00971 
00972                                                 while (string::npos != pos || string::npos != lastPos) {
00973                                                         // Found a token, add it to the vector.
00974                                                         tokens.push_back(line.substr(lastPos, pos - lastPos));
00975                                                         // Skip delimiters.  Note the "not_of"
00976                                                         lastPos = line.find_first_not_of(delimiter, pos);
00977                                                         // Find next "non-delimiter"
00978                                                         pos = line.find_first_of(delimiter, lastPos);
00979                                                 }
00980 
00981                                                 TPoint point;
00982                                                 point.X = atof((tokens.at(0)).data());
00983                                                 point.Y = atof((tokens.at(1)).data());
00984                                                 point.Z = atof((tokens.at(2)).data());
00985                                                 point.phi = atof((tokens.at(3)).data())/RadToDeg;
00986                                                 point.theta = atof((tokens.at(4)).data())/RadToDeg;
00987                                                 point.psi = atof((tokens.at(5)).data())/RadToDeg;
00988                                                 points.push_back( point );
00989                                                 ++lines;
00990                                                 tokens.clear();
00991                                         }
00992                                         cout << lines << " points loaded.\n";
00993 
00994                                         break;
00995                                 }
00996 
00997                         case '=': {
00998                                         using namespace std;
00999                                         cout << "Which file? WARNING: Will be overwritten!\n";
01000                                         string filename;
01001                                         cin >> filename;
01002 
01003                                         ofstream listfile(filename.c_str(), ios_base::out);
01004 
01005                                         int count = 0;
01006 
01007                                         for(std::vector<TPoint>::iterator iter = points.begin(); iter != points.end(); ++iter) {
01008                                                 listfile.precision(8);
01009                                                 if (count != 0)
01010                                                         listfile << endl;
01011                                                 listfile << iter->X << "," << iter->Y << "," << iter->Z << ",";
01012                                                 listfile << iter->phi << "," << iter->theta << "," << iter->psi;
01013                                                 ++count;
01014                                         }
01015                                         cout << count << " points saved.\n";
01016                                         listfile.close();
01017                                         break;
01018                                 }
01019                         case 'p': {
01020                                         using namespace std;
01021                                         //int available;
01022                                         //cout << "How many movements?\n";
01023                                         //int moves;
01024                                         //cin >> moves;
01025                                         std::cout << "Start playback!\n";
01026                                         for(int i = 1; i > 0/*i <= moves*/ ; ++i) {
01027                                                 if(useLinearMode) {
01028                                                         katana->moveRobotLinearTo( points[i%points.size()].X, points[i%points.size()].Y, points[i%points.size()].Z, points[i%points.size()].phi, points[i%points.size()].theta, points[i%points.size()].psi, true, 10000 );
01029                                                 }
01030                                                 else{
01031                                                         katana->moveRobotTo( points[i%points.size()].X, points[i%points.size()].Y, points[i%points.size()].Z, points[i%points.size()].phi, points[i%points.size()].theta, points[i%points.size()].psi, true, 10000 );
01032                                                 }
01033                                                 if (i%100 == 0) {
01034                                                         std::cout << i << ", " << std::flush;
01035                                                 }
01036                                         }
01037 
01038                                         break;
01039 
01040                                 }
01041 
01042                         case 'd': {
01043                                         long motor;
01044                                         double degrees;
01045                                         std::cout << "\nMoving motor to degrees\n";
01046                                         std::cout << " motor: ";
01047                                         std::cin >> motor;
01048                                         std::cout << " degrees: ";
01049                                         std::cin >> degrees;
01050                                         if ((motor > 0) && (motor < 7)) {
01051                                                 katana->movDegrees(motor - 1, degrees);
01052                                         } else {
01053                                                 std::cout << "motor has to be a number from 1 to 6\n";
01054                                         }
01055 
01056                                         break;
01057                                 }
01058 
01059                         case 'z': {
01060                                         double xoff, yoff, zoff, psioff;
01061                                         std::cout << "X offset (m): ";
01062                                         std::cin >> xoff;
01063                                         std::cout << "Y offset (m): ";
01064                                         std::cin >> yoff;
01065                                         std::cout << "Z offset (m): ";
01066                                         std::cin >> zoff;
01067                                         std::cout << "psi offset around x axis (rad): ";
01068                                         std::cin >> psioff;
01069                                         katana->setTcpOffset(xoff, yoff, zoff, psioff);
01070                                         
01071                                         break;
01072                                 }
01073                         
01074                         default: //Error message
01075                                 std::cout << "\n'" << input << "' is not a valid command.\n" << std::endl;
01076                                 break;
01077                         }
01078 
01079                 } catch (Exception &e) {
01080                         std::cout << "\nERROR: " << e.message() << std::endl;
01081                 }
01082 
01083         }
01084         /*//Q&D test to launch program:
01085         if(runProgram){
01086                 StartProgram(0);
01087         }
01088         */
01089         return 0;
01090 }
01092 void StartProgram(int index){
01093         //Q&D test to launch program:
01094         //std::system("/home/katprog katana6M180.cfg 1");
01095         progRunning = true;
01096         pthread_create(&tid, NULL, RunProgram, (void*)&retVal);//(&tid, NULL, start_func, arg);
01097         pthread_detach(tid);
01098 }
01100 void* RunProgram(void*){
01101         //katana->calibrate();
01102         std::cout << "\nProgram running...type any key to stop after the next cycle\n";
01103         while(progRunning){
01104                 if(progRunning) katana->moveRobotToEnc(Tpos::x, true);
01105                 if(progRunning) katana->moveRobotToEnc(Tpos::y, true);
01106                 if(progRunning) katana->moveRobotToEnc(Tpos::z, true);
01107                 if(progRunning) katana->moveRobotToEnc(Tpos::u, true);
01108                 if(progRunning) katana->moveRobotToEnc(Tpos::v, true);
01109                 if(progRunning) katana->moveRobotToEnc(Tpos::w, true);
01110         }
01111         pthread_exit((void*) &retVal);
01112         return ((void*) &retVal);
01113 }
01115 


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