00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00019
00020
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
00053 std::unique_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
00071
00072
00073
00074 std::unique_ptr<CCdlSocket> device;
00075 std::unique_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
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());
00095 std::cout << "-------------------------------------------\n";
00096 std::cout << "success: communication with Katana initialized\n";
00097 std::cout << "-------------------------------------------\n";
00098
00099
00100
00101
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
00116 katana->setMaximumLinearVelocity(60);
00117
00118
00119 int nOfMot = katana->getNumberOfMotors();
00120 int controller;
00121 const char* model;
00122 std::vector<TPoint> points(0);
00123
00124
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
00136 std::cout << "- Calibrating Katana, please wait for termination..." << std::endl;
00137 katana->calibrate();
00138 std::cout << " ...done." << std::endl;
00139
00140
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
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
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
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
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
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
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
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
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
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
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
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
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
00283 tokens.push_back(line.substr(lastPos, pos - lastPos));
00284
00285 lastPos = line.find_first_not_of(delimiter, pos);
00286
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
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
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