ForceController/demoVisualizationServer.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
#include "include/defs/defs.h"
#include "include/board/boardDynamixel.h"
#include "include/kinematic/kinematicLie.h"
#include "include/legModel/insectLeg.h"
#include "include/robotModel/robotMessor2.h"
#include "include/visualization/visualizerGL.h"
#include "include/visualization/visualizerIrrlicht.h"
#include <ctime>
#include <iostream>
#include <string>
#include <stdio.h>
#include <thread>
#include <time.h>
#include <boost/array.hpp>
#include <boost/asio.hpp>


/*
 Paulina Jankowska
 Tomasz Chrosniak
 */

using boost::asio::ip::udp;
Board* board;

int k = 0;

vector<float_type> give_position()
{
    std::vector<float_type> configuration;
    std::vector<float_type> configSingleLeg;

    k++;
    k = k%20;
    for(int i=5;i>=0;--i)
    {

        configSingleLeg.clear();
        board->readPositions(i,configSingleLeg);
        //configSingleLeg = {(0+k)*PI/180,(24+k)*PI/180,(-114+k)*PI/180};
        for(int j=0;j<configSingleLeg.size();++j)
        {
            configuration.push_back(configSingleLeg[j]);
        }
    }
    return configuration;
}

int main()
{
    board = createBoardDynamixel();
    std::vector<float_type> complianceVector = {120,120,120};
    for(int k=0;k<6;++k)
    {
      board->setComplianceMargin(k,complianceVector);
    }

    try
    {
        boost::asio::io_service io_service;

        udp::socket socket(io_service, udp::endpoint(udp::v4(), 6512));

        for(;;)
        {
            boost::array<char, 1> recv_buf;
            udp::endpoint remote_endpoint;
            boost::system::error_code error;
            vector<float_type> conf = give_position();
            for (int i=0;i<6;i++)
                conf[i*3]=-conf[i*3];
            std::vector<float_type> confTmp;
            confTmp.push_back(conf[9]); confTmp.push_back(conf[10]); confTmp.push_back(conf[11]);
            conf[9]=conf[15]; conf[10]=conf[16]; conf[11]=conf[17];
            conf[15]=confTmp[0]; conf[16]=confTmp[1]; conf[17]=confTmp[2];

            socket.receive_from(boost::asio::buffer(recv_buf),
                                remote_endpoint, 0, error);

            if (error && error != boost::asio::error::message_size)
                throw boost::system::system_error(error);

            boost::system::error_code ignored_error;
            socket.send_to(boost::asio::buffer(conf),
                           remote_endpoint, 0, ignored_error);

        }

    }

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