kinematics.cpp

Go to the documentation of this file.

#include "kinematics6M180.h"
#include "kinematics6M90G.h"
#include "kinematics6M90T.h"


#include <iostream>
#include <cstdio>
#include <stdlib.h>
#include <memory>
#include <vector>
#include <string>

#include "keyboard.h"

std::vector<std::string> explode( const std::string & in, const std::string & delim) {
        typedef std::string::size_type size_type;

        const size_type delim_len = delim.length();
        std::vector<std::string> result;

        size_type i = 0, j;
        for (;;) {
                j = in.find(delim, i);
                result.push_back(in.substr(i, j-i));
                if (j == std::string::npos)
                        break;
                i = j + delim_len;
        }

        return result;
}

void DisplayHelp() {
        std::cout << "\n";
        std::cout << "-------------------------------------\n";
        std::cout << "?: Display this help\n";
        std::cout << "e: Enc2rad\n";
        std::cout << "r: Rad2enc\n";
        std::cout << "d: Direct kinematics\n";
        std::cout << "i: Inverse kinematics\n";
        std::cout << "q: Quit\n";
        std::cout << "Esc: Quit\n";
        std::cout << "-------------------------------------\n";
}


int main(int argc, char *argv[]) {

        using namespace AnaGuess;

        if (argc != 2) {
                std::cout << "-------------------------------------\n";
                std::cout << "usage: kinematics [6M180|6M90G|6M90T]\n";
                std::cout << "-------------------------------------\n";
                return 0;
        }

        std::cout << "-----------------------\n";
        std::cout << "KINEMATICS DEMO STARTED\n";
        std::cout << "-----------------------\n";

        //Kinematics objects
        Kinematics* kinematics;
        {
        char* t = argv[1];
        if ((t[0]=='6')&&(t[1]=='M')&&(t[2]=='1')&&(t[3]=='8')&&(t[4]=='0')) {
                kinematics = (Kinematics*) new Kinematics6M180();
        } else if ((t[0]=='6')&&(t[1]=='M')&&(t[2]=='9')&&(t[3]=='0')&&(t[4]=='G')) {
                kinematics = (Kinematics*) new Kinematics6M90G();
        } else if ((t[0]=='6')&&(t[1]=='M')&&(t[2]=='9')&&(t[3]=='0')&&(t[4]=='T')) {
                kinematics = (Kinematics*) new Kinematics6M90T();
        } else {
                std::cout << "-------------------------------------\n";
                std::cout << "unknown type: " << t << "\n";
                std::cout << "usage: kinematics [6M180|6M90G|6M90T]\n";
                std::cout << "-------------------------------------\n";
                return 0;
        }
        }

        std::cout << "--------------------------------------------\n";
        std::cout << "success: katana kinematics" << argv[1] << " initialized\n";
        std::cout << "--------------------------------------------\n";

        DisplayHelp();

        bool loop = true;
        int input;
        std::string strtemp;
        std::vector<std::string> strvectemp;
        double doubletemp;
        int size;
        std::vector<double> pose;
        pose.reserve(6);
        std::vector<int> encoder;
        encoder.reserve(6);
        std::vector<double> angle;
        angle.reserve(6);
        std::vector<double> start;
        start.reserve(6);

        while (loop) {
                input = _getch();
                try {
                        switch (input) {
                        case 27: //VK_ESCAPE
                        case 'q': //VK_Q
                                loop = false;
                                continue;

                        case '?':
                                DisplayHelp();
                                break;

                        case 'e': //VK_E (enc2rad)
                                encoder.clear();
                                angle.clear();
                                std::cout << "\nInsert encoder values (comma separated, without spaces): \n";
                                std::cin >> strtemp;
std::cout << "> strtemp " << strtemp << "\n";
                                strvectemp = explode(strtemp,",");
                                size = strvectemp.size();
std::cout << "> strvectemp.size " << size << "\n";
                                for (int i = 0; i < size; i++) {
                                        encoder.push_back(atoi(strvectemp.at(i).c_str())); // double: atof()
std::cout << "> encoder.at(" << i << ") " << encoder.at(i) << "\n";
                                }
                                if (!kinematics->enc2rad(angle, encoder)) {
                                        std::cout << "\nConverting encoders to angles failed.\n";
                                        break;
                                }
                                std::cout << "\nAngles:\n";
                                size = angle.size();
std::cout << "> angle.size " << size << "\n"; // !!! is 0
                                for (int i = 0; i < size; i++) {
                                        std::cout << angle.at(i);
                                        if (i != size - 1)
                                                std::cout << ",";
                                }
                                std::cout << "\n";
                                break;

                        case 'r': //VK_R (rad2enc)
                                break;

                        case 'd': //VK_D (direct kinematics)
                                std::cout << "\nInsert angle values: \n";
                                angle.clear();
                                pose.clear();
                                std::cout << "Axis 1: ";
                                std::cin >> doubletemp;
                                angle.push_back(doubletemp);
                                std::cout << "Axis 2: ";
                                std::cin >> doubletemp;
                                angle.push_back(doubletemp);
                                std::cout << "Axis 3: ";
                                std::cin >> doubletemp;
                                angle.push_back(doubletemp);
                                std::cout << "Axis 4: ";
                                std::cin >> doubletemp;
                                angle.push_back(doubletemp);
                                std::cout << "Axis 5: ";
                                std::cin >> doubletemp;
                                angle.push_back(doubletemp);
                                std::cout << "Axis 6: ";
                                std::cin >> doubletemp;
                                angle.push_back(doubletemp);
                                if (!kinematics->directKinematics(pose, angle)) {
                                        std::cout << "\nCalculating direct kinematics failed.\n";
                                        break;
                                }
                                std::cout.precision(6);
                                std::cout << "\n------------------------------------\n";
                                std::cout << "X:     " << pose[0] << "\n";
                                std::cout << "Y:     " << pose[1] << "\n";
                                std::cout << "Z:     " << pose[2] << "\n";
                                std::cout << "phi:   " << pose[3] << "\n";
                                std::cout << "theta: " << pose[4] << "\n";
                                std::cout << "psi:   " << pose[5] << "\n";
                                std::cout << "------------------------------------\n";
                                break;

                        case 'i':  //VK_I (inverse kinematics)
                                std::cout << "\nInsert cartesian parameters: \n";
                                angle.clear();
                                pose.clear();
                                std::cout << "X: ";
                                std::cin >> doubletemp;
                                pose.push_back(doubletemp);
                                std::cout << "Y: ";
                                std::cin >> doubletemp;
                                pose.push_back(doubletemp);
                                std::cout << "Z: ";
                                std::cin >> doubletemp;
                                pose.push_back(doubletemp);
                                std::cout << "phi: ";
                                std::cin >> doubletemp;
                                pose.push_back(doubletemp);
                                std::cout << "theta: ";
                                std::cin >> doubletemp;
                                pose.push_back(doubletemp);
                                std::cout << "psi: ";
                                std::cin >> doubletemp;
                                pose.push_back(doubletemp);
                                angle.reserve(6);
                                for (int i = 0; i < 6; i++) {
                                        start[i] = 1.5;
                                }
                                try {
                                        if (!kinematics->inverseKinematics(angle, pose, start)) {
                                                std::cout << "\nCalculating inverse kinematics failed.\n";
                                                break;
                                        }
                                } catch (NoSolutionException &nse) {
                                        std::cout << "\nNo solution found.\n";
                                        break;
                                }
                                std::cout << "\n------------------------------------\n";
                                std::cout << "Axis 1: " << angle[0] << "\n";
                                std::cout << "Axis 2: " << angle[1] << "\n";
                                std::cout << "Axis 3: " << angle[2] << "\n";
                                std::cout << "Axis 4: " << angle[3] << "\n";
                                std::cout << "Axis 5: " << angle[4] << "\n";
                                std::cout << "Axis 6: " << angle[5] << "\n";
                                std::cout << "------------------------------------\n";
                                break;

                        default: //Error message
                                std::cout << "\n'" << input << "' is not a valid command.\n" << std::endl;
                                break;

                        }
                } catch (Exception &e) {
                        std::cout << "\nERROR: " << e.message() << "\n";
                }
        }
        delete kinematics;
        return 0;
}