ForceController/demoSusceptibility.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
#include "include/defs/defs.h"
#include "include/robotModel/robot.h"
#include "include/robotModel/robotMessor2.h"
#include "include/board/board.h"
#include "include/board/boardDynamixel.h"
#include <iostream>
#include <stdio.h>
/*
Maciej Okoniewski
Marcin Zgolinski
*/
using namespace std;

int main( int argc, const char** argv )
{
    try {


        Robot* Rob;
        Rob = createRobotMessor();
        std::vector<float_type> configuration,C;
        std::vector<Mat34> pos,pos2;
        Mat34 move1;
        move1.setIdentity();
        move1(0, 3) = 0;
        move1(1, 3) = 0;
        move1(2, 3) = -0.05;
        Mat34 testmoveplatform;

        testmoveplatform.setIdentity();
        testmoveplatform(0, 3) = 0.0;
        testmoveplatform(1, 3) = 0;
        testmoveplatform(2, 3) = 0.0;


        Board *demo = createBoardDynamixel();

        for(int i = 0; i < 6; i++)
             {
                     demo->setSpeed(i, 0, 15);
                     demo->setSpeed(i, 1, 15);
                     demo->setSpeed(i, 2, 15);
             }

            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);
           }

            for(int i = 0; i < 6; i++)
            {
                int j = 3 * i;
                    demo->setPosition(i, 0, configuration[j] );
                    demo->setPosition(i, 1, configuration[j+1] );
                    demo->setPosition(i, 2, configuration[j+2] );
            }


         configuration = Rob->movePlatform(testmoveplatform);

         std::cout << configuration[0];
         for (int i=0;i<configuration.size();i++){

//             if (configuration[i]>3.14)
//                 configuration[i]-=6.28;
//             else if (configuration[i]<-3.14)
//                 configuration[i]=+6.28;
         }

         for(int i = 0; i < 6; i++)
         {
             int j = 3 * i;
                 demo->setPosition(i, 0, configuration[j] );
                 demo->setPosition(i, 1, configuration[j+1] );
                 demo->setPosition(i, 2, configuration[j+2] );
         }

           std::cout<<""<<std::endl;
           std::cout<<"Compliance:"<<std::endl;
           std::cout<<""<<std::endl;
           C=Rob->computeCompliance(configuration);

       for(int i=0;i<18;i++)
        {
               std::cout<<C[i]<<std::endl;
        }

        demo->setTorqueLimit(C);
       //demo->setComplianceSlope( C );


       return 0;

   }
    catch (const std::exception& ex) {
        std::cerr << ex.what() << std::endl;
        return 1;
    }
    return 0;
}