AUV-Coop-Assembly
Master Thesis for Robotics Engineering. Cooperative peg-in-hole assembly with two underwater vehicles guided by vision
pipeReachTask.cpp
1 #include "header/pipeReachTask.h"
2 
3 PipeReachTask::PipeReachTask(int dim, bool eqType, std::string robotName, VehArmType vehArmType, AngLinType angLinType)
4  : Task(dim, eqType, robotName, "PIPE_REACHING_GOAL"){
5  //gain = 0.05;
6  gainLin = 0.05;
7  gainAng = 0.08;
8  this->vehArmType = vehArmType;
9  this->angLinType = angLinType;
10 
11 }
12 
13 
14 int PipeReachTask::updateMatrices(Infos * const robInfo){
15 
16  setActivation();
17  setJacobian(robInfo->robotState.w_Jtool_robot);
18  setReference(robInfo->transforms.wTgoalTool_eigen, robInfo->transforms.wTt_eigen);
19 }
20 
21 int PipeReachTask::setActivation(){
22 
23  double vectDiag[dimension];
24  std::fill_n(vectDiag, dimension, 1);
25  this->A.SetDiag(vectDiag);
26 
27  return 0;
28 
29 }
30 
31 void PipeReachTask::setJacobian(Eigen::Matrix<double, 6, TOT_DOF> w_J_robot){
32 
33  switch (vehArmType){
34  case ONLYVEH:
35  w_J_robot.leftCols<ARM_DOF>() = Eigen::MatrixXd::Zero(6, ARM_DOF);
36  break;
37 
38  case ONLYARM:
39  w_J_robot.rightCols<VEHICLE_DOF>() = Eigen::MatrixXd::Zero(6, VEHICLE_DOF);
40  break;
41 
42  case BOTH:
43  break;
44  }
45 
46  if (dimension == 6){
47  J = CONV::matrix_eigen2cmat(w_J_robot);
48  }
49  else if (dimension == 5){
50  CMAT::Matrix J_temp = CONV::matrix_eigen2cmat(w_J_robot);
51  J = J_temp.DeleteRow(4);
52  } else if (dimension == 3){
53  switch (angLinType){
54  case LIN:
55  {
56  J = CONV::matrix_eigen2cmat(w_J_robot.topRows<3>());
57  break;
58  }
59  case ANG:
60  {
61  J = CONV::matrix_eigen2cmat(w_J_robot.bottomRows<3>());
62  break;
63  }
64  default:
65  {
66  std::cerr << "ERROR: dimension 3 and not lin ang specified\n";
67  exit(-1);
68  }
69 
70  }
71 
72  }
73 
74 }
75 
76 
77 
78 
79 void PipeReachTask::setReference(Eigen::Matrix4d wTgoaltool_eigen, Eigen::Matrix4d wTtool_eigen){
80 
81  CMAT::TransfMatrix wTgoaltool_cmat = CONV::matrix_eigen2cmat(wTgoaltool_eigen);
82  CMAT::TransfMatrix wTtool_cmat = CONV::matrix_eigen2cmat(wTtool_eigen);
83 
84 
85 
86  CMAT::Vect6 errorSwapped = CMAT::CartError(wTgoaltool_cmat, wTtool_cmat);//ang;lin
87  // ang and lin must be swapped because in yDot and jacob linear part is before
88 
89  if (dimension == 6){
90  error(1)= errorSwapped(4);
91  error(2)= errorSwapped(5);
92  error(3)= errorSwapped(6);
93  error(4)= errorSwapped(1);
94  error(5)= errorSwapped(2);
95  error(6)= errorSwapped(3);
96 
97  CMAT::Vect3 vect3_lin;
98  CMAT::Vect3 vect3_ang;
99  vect3_lin = (this->gainLin * errorSwapped.GetSecondVect3());
100  vect3_ang= (this->gainAng * errorSwapped.GetFirstVect3());
101 
102  vect3_lin = FRM::saturateCmat(vect3_lin, 0.1); //0.1 before
103  vect3_ang = FRM::saturateCmat(vect3_ang, 0.1);
104 
105  this->reference(1) = vect3_lin(1);
106  this->reference(2) = vect3_lin(2);
107  this->reference(3) = vect3_lin(3);
108  this->reference(4) = vect3_ang(1);
109  this->reference(5) = vect3_ang(2);
110  this->reference(6) = vect3_ang(3);
111 
112  } else if (dimension == 5){
113 
114 
115  CMAT::Vect3 vect3_lin;
116  CMAT::Matrix vect2_ang(2,1);
117 
118  error(1)= errorSwapped(4);
119  error(2)= errorSwapped(5);
120  error(3)= errorSwapped(6);
121  error(4)= errorSwapped(2);
122  error(5)= errorSwapped(3);
123 
124  vect3_lin = (this->gainLin * errorSwapped.GetSecondVect3());
125  vect2_ang(1) = this->gainAng * errorSwapped(2);
126  vect2_ang(2) = this->gainAng * errorSwapped(3);
127 
128  vect3_lin = FRM::saturateCmat(vect3_lin, 0.1);
129  vect2_ang = FRM::saturateCmat(vect2_ang, 0.1);
130 
131  this->reference(1) = vect3_lin(1);
132  this->reference(2) = vect3_lin(2);
133  this->reference(3) = vect3_lin(3);
134  this->reference(4) = vect2_ang(1);
135  this->reference(5) = vect2_ang(2);
136 
137  } else if (dimension == 3){
138  switch (angLinType){
139  case LIN:
140  {
141  error(1)= errorSwapped(4);
142  error(2)= errorSwapped(5);
143  error(3)= errorSwapped(6);
144 
145  CMAT::Vect3 vect3_lin;
146  vect3_lin = (this->gainLin * errorSwapped.GetSecondVect3());
147 
148  vect3_lin = FRM::saturateCmat(vect3_lin, 0.1); //0.1 before
149 
150  this->reference(1) = vect3_lin(1);
151  this->reference(2) = vect3_lin(2);
152  this->reference(3) = vect3_lin(3);
153 
154  break;
155  }
156  case ANG:
157  {
158  error(1)= errorSwapped(1);
159  error(2)= errorSwapped(2);
160  error(3)= errorSwapped(3);
161 
162  CMAT::Vect3 vect3_ang;
163  vect3_ang= (this->gainAng * errorSwapped.GetFirstVect3());
164 
165  vect3_ang = FRM::saturateCmat(vect3_ang, 0.1);
166 
167  this->reference(1) = vect3_ang(1);
168  this->reference(2) = vect3_ang(2);
169  this->reference(3) = vect3_ang(3);
170 
171  break;
172  }
173  default:
174  {
175  std::cerr << "ERROR: dimension 3 and not lin ang specified\n";
176  exit(-1);
177  }
178 
179  }
180  }
181 
182 }
183 
184 
185 
186 
187 
188 
189 /*** old with cout
190  *
191 void PipeReachTask::setJacobian(Eigen::Matrix<double, 6, TOT_DOF> w_J_robot){
192 
193 // Eigen::Matrix<double, 5, TOT_DOF> totJ;
194 // totJ.topRows<3>() = w_J_robot.topRows<3>();
195 
196 // Eigen::Matrix<double, 2, TOT_DOF> excludeRoll;
197 // excludeRoll << 0, 1, 0,
198 // 0, 0, 1;
199 
200 
201 // totJ.bottomRows<2>() = excludeRoll * w_J_robot.bottomRows<3>();
202  if (dimension == 6){
203  //debug
204 // Eigen::Matrix<double, 6, TOT_DOF> temp = w_J_robot;
205 // temp.leftCols<4>() << 0, 0, 0, 0,
206 // 0,0,0,0,
207 // 0,0,0,0,
208 // 0,0,0,0,
209 // 0,0,0,0,
210 // 0,0,0,0,
211  J = CONV::matrix_eigen2cmat(w_J_robot);
212  }
213  else if (dimension == 5){
214  CMAT::Matrix J_temp = CONV::matrix_eigen2cmat(w_J_robot);
215  J = J_temp.DeleteRow(4);
216  }
217 }
218 */
Definition: infos.h:117
ABSTRACT class task. Each task is a derived class of this class. It contains all the variable that th...
Definition: task.h:17