AUV-Coop-Assembly
Master Thesis for Robotics Engineering. Cooperative peg-in-hole assembly with two underwater vehicles guided by vision
main_old.cpp
1 #include "header/main_old.h"
2 #include <chrono>
3 
4 
5 int main(int argc, char **argv)
6 {
7 
8  ros::init(argc, argv, "main"); //necessary for real ros node robotInterface
9  std::cout << "[girona500_A][MISSION_MANAGER] Start" << std::endl;
10 
13  double goalLinearVect[] = {-0.287, -0.062, 7.424};
14  Eigen::Matrix4d wTgoalVeh_eigen = Eigen::Matrix4d::Identity();
15  //rot part
16  wTgoalVeh_eigen.topLeftCorner(3,3) = Eigen::Matrix3d::Identity();
17  //trasl part
18  wTgoalVeh_eigen(0, 3) = goalLinearVect[0];
19  wTgoalVeh_eigen(1, 3) = goalLinearVect[1];
20  wTgoalVeh_eigen(2, 3) = goalLinearVect[2];
21 
23  double goalLinearVectEE[] = {-0.27, -0.102, 2.124};
24  Eigen::Matrix4d wTgoalEE_eigen = Eigen::Matrix4d::Identity();
25  //rot part
26  wTgoalEE_eigen.topLeftCorner(3,3) = Eigen::Matrix3d::Identity();
27 // wTgoalEE_eigen.topLeftCorner(3,3) << 0.5147, 0 , -0.8574,
28 // 0 ,1, 0 ,
29 // 0.8573 , 0 , 0.5147;
30  //trasl part
31  wTgoalEE_eigen(0, 3) = goalLinearVectEE[0];
32  wTgoalEE_eigen(1, 3) = goalLinearVectEE[1];
33  wTgoalEE_eigen(2, 3) = goalLinearVectEE[2];
35 
37  Infos robInfo;
38 
40  std::string filename = "/home/tori/UWsim/Peg/model/g500ARM5.urdf";
41  KDLHelper kdlHelper(filename);
42  std::string vehicle = "base_link";
43  std::string link0 = "part0";
44  std::string endEffector = "end_effector";
45  kdlHelper.setSolvers(link0, endEffector);
47  //TODO Maybe exist an easier method to parse fixed frame from urdf without needed of kdl solver
48  kdlHelper.getFixedFrame(vehicle, link0, &(robInfo.robotStruct.vTlink0));
49 
50 
51  //struct transforms to pass them to Controller class
52  robInfo.transforms.wTgoalVeh_eigen = wTgoalVeh_eigen;
53  robInfo.transforms.wTgoalEE_eigen = wTgoalEE_eigen;
54 
56  //std::string pathLog = "logPeg/" + PRT::getCurrentDateFormatted();
57  //Controller controller("girona500_A", &pathLog);
58  Controller controller("girona500_A");
59 
60 
62  RobotInterface robotInterface("/uwsim/g500_A/", "girona500_A", "girona500_B", "pipe", argc, argv);
63  robotInterface.init();
64 
65  robotInterface.getwTt(&(robInfo.transforms.wTt_eigen));
66 
67  int ms = 100;
68  boost::asio::io_service io;
69  while(1){
70 // auto start = std::chrono::steady_clock::now();
71 
72  // this must be inside loop
73  boost::asio::deadline_timer loopRater(io, boost::posix_time::milliseconds(ms));
74 
75  robotInterface.getJointState(&(robInfo.robotState.jState));
76  robotInterface.getwTv(&(robInfo.robotState.wTv_eigen));
77  robotInterface.getOtherRobPos(&(robInfo.exchangedInfo.otherRobPos));
78  //robotInterface.getvTee(&(transf.vTee_eigen));
79 
80 // std::vector <Eigen::Matrix4d> vTjoints;
81 // robotInterface.getvTjoints(&vTjoints);
82 // transf.vTjoints = vTjoints;
83 
84  //get ee pose RESPECT LINK 0
85  kdlHelper.getEEpose(robInfo.robotState.jState, &(robInfo.robotState.link0Tee_eigen));
86 
87  // useful have vTee: even if it is redunant, tasks use it a lot
88  robInfo.robotState.vTee_eigen = robInfo.robotStruct.vTlink0 * robInfo.robotState.link0Tee_eigen;
89 
90  // get jacobian respect link0
91  kdlHelper.getJacobian(robInfo.robotState.jState, &(robInfo.robotState.link0_J_man));
92 
93  computeWholeJacobian(&robInfo);
94 
95 
96 
97  controller.updateTransforms(&robInfo);
98 
99  std::vector<double> yDot = controller.execAlgorithm();
100 
101  robotInterface.sendyDot(yDot);
102 
103  robotInterface.spinOnce(); // actually the spinonce is called here and not in sendyDot
104 
105  // auto end = std::chrono::steady_clock::now();
106  // auto diff = end - start;
107  // std::cout << std::chrono::duration<double, std::milli> (diff).count()
108  // << " ms" << std::endl;
109 
110 
111  loopRater.wait(); //wait for the remaning time until period setted (ms)
112  }
113 
114  return 0;
115 }
116 
The Controller class is responsabile of 1) taking matrices and giving them to Tasks classes...
Definition: controller.h:20
robotInterface: a ros node responsible of taken info from simulator and robot sensors, and of given commands back. It is the intermiate layer between actual robot and mission manager ("main")
Definition: infos.h:117
Eigen::Matrix4d wTv_eigen
Transforms.
Definition: infos.h:34