ForceController/demoRobot.cpp at master · dominikbelter/ForceController · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
#include "include/defs/defs.h"
#include "include/robotModel/robot.h"
#include "include/robotModel/robotMessor2.h"
#include "include/legModel/insectLeg.h"
#include <iostream>
#include <stdio.h>
#include "include/visualization/visualizerIrrlicht.h"
#include <irrlicht.h>
#include "include/board/board.h"
#include "include/board/boardDynamixel.h"
#include "3rdParty/dynamixel/dynamixel.h"
#include "3rdParty/dynamixel/dxl_hal.h"
#include <thread>

/*
Joregus
Emil
*/
using namespace std;
using namespace irr;
using namespace core;
using namespace scene;
using namespace video;
using namespace io;
using namespace gui;

Visualizer* visualizer;

void drawRobot(){
    std::vector<double> configuration(18,0);
    for (int i=0;i<6;i++){
        configuration.push_back(0);
        configuration.push_back(24*3.14/180);
        configuration.push_back(-114*3.14/180);
    }
    visualizer->drawRobot(Mat34::Identity(), configuration);
}

int main( int argc, const char** argv ){
    try {
        std::string filename = "../../resources/configGlobal.xml";
        tinyxml2::XMLDocument config;
        config.LoadFile(filename.c_str());
        bool useVisualization;
        if (config.FirstChildElement() == nullptr)
            std::cout << "unable to load config global file.\n";
        else {
            tinyxml2::XMLElement * pRoot = config.FirstChildElement("parameters");
            pRoot->QueryBoolAttribute("useVisualization", &useVisualization);
        }
        std::cout << useVisualization << "\n";

        Robot* robot;
        robot = createRobotMessor("../resources/robotModel.xml");
        if (useVisualization)
            visualizer = createVisualizerIrrlicht("VisualizerWindow", 1024, 768, 0.01, false);
        Board *demo;
        if (!useVisualization)
            demo = createBoardDynamixel();
        std::vector<float_type> configuration;
        Mat34 robotPose(Mat34::Identity());

        if (!useVisualization){
            vector<float_type> motorSpeed(18,15.0);//set default speed
            demo->setSpeed(motorSpeed);
        }

        // tutaj macie katy 0,24,-114 dla kazdej nogi na sztywno wrzucone
        for (int i = 0; i<6; i++){
            configuration.push_back(0);
            configuration.push_back(24*3.14/180);
            configuration.push_back(-114*3.14/180);
        }
        std::unique_ptr<std::thread> visuThr;
        if (useVisualization){
            //std::thread visuThr(drawRobot);
            visuThr = std::unique_ptr<std::thread>(new std::thread(drawRobot));
            visualizer->setPosition(configuration);
        }
        else
            demo->setPosition(configuration);

        /*for (int i=0;i<45;i++){
            usleep(2000000);
            configuration[15]=i*3.14/180;
            visualizer->setPosition(configuration);
        }*/
        usleep(2000000);
        Mat34 motion(Mat34::Identity());
        motion(2,3)=0.07;
        configuration = robot->movePlatform(motion);
        if (useVisualization)
            visualizer->setPosition(configuration);
        else
            demo->setPosition(configuration);
        /*for (int j=0;j<move.size();j++){
            configurationtest.push_back(Rob->movePlatform(move[j]));
             for (int i=0;i<18;i++)
            {//pierwsze serwo powinno otrzymywac wartosci w oklicach zera (niezgodnosc kinematyki robota i sterownika)
                 if (configurationtest[j][i]>3.14)
                     configurationtest[j][i]-=6.28;
                 else if (configurationtest[j][i]<-3.14)
                     configurationtest[j][i]=+6.28;
            }
        }

         configuration2 = Rob->movePlatform(testmoveplatform);
         configurationneutral = Rob->movePlatform(moveneutral);
*/
        if (useVisualization)
            visuThr->join();
        return 0;
   }
    catch (const std::exception& ex) {
        std::cerr << ex.what() << std::endl;
        return 1;
    }
    return 0;
}