AUV-Coop-Assembly
Master Thesis for Robotics Engineering. Cooperative peg-in-hole assembly with two underwater vehicles guided by vision
coopTask.cpp
1 #include "header/coopTask.h"
2 
3 CoopTask::CoopTask(int dim, bool eqType, std::string robotName)
4  : Task(dim, eqType, robotName, "COOP_CONSTRAINT"){
5 
6  //no gain, it is a non reactive task;
7 
8 }
9 
10 int CoopTask::updateMatrices(Infos * const robInfo){
11 
12  setActivation();
13  setJacobian(robInfo->robotState.w_Jtool_robot);
14  setReference(robInfo->exchangedInfo.coopCartVel);
15 }
16 
17 void CoopTask::setReference(Eigen::Matrix<double, VEHICLE_DOF, 1> coopCartVel){
18 
19  if (dimension == 6){
20  reference = CONV::matrix_eigen2cmat(coopCartVel);
21 
22  } else if (dimension ==5){
23  auto temp = CONV::matrix_eigen2cmat(coopCartVel);
24  reference(1)= temp(1);
25  reference(2)= temp(2);
26  reference(3)= temp(3);
27  reference(4)= temp(5);
28  reference(5)= temp(6);
29  }
30 
31 }
32 
33 void CoopTask::setActivation(){
34 
35  double vectDiag[dimension];
36  std::fill_n(vectDiag, dimension, 1);
37  this->A.SetDiag(vectDiag);
38 
39 
40 }
41 
42 void CoopTask::setJacobian(Eigen::Matrix<double, 6, TOT_DOF> w_Jtool_robot){
43 
44  if (dimension ==6){
45  //w_Jtool_robot.row(4) << 0, 0,0,0,0,0,0,0,0,0; //TRY ASK DEBUG
46  J = CONV::matrix_eigen2cmat(w_Jtool_robot);
47 
48  } else if (dimension == 5){
49  auto temp = CONV::matrix_eigen2cmat(w_Jtool_robot);
50  J = temp.DeleteRow(4);
51  }
52 }
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