AUV-Coop-Assembly
Master Thesis for Robotics Engineering. Cooperative peg-in-hole assembly with two underwater vehicles guided by vision
jointLimitTask.cpp
1 #include "header/jointLimitTask.h"
2 
3 JointLimitTask::JointLimitTask(int dim, bool eqType, std::string robotName)
4  : Task(dim, eqType, robotName, "JOINT_LIMIT"){
5  gain = 0.5;
6 
7  safeGuardUp = new double[this->dimension];
8  safeGuardUp[0] = JLIM1_MAX - 0.2;
9  safeGuardUp[1] = JLIM2_MAX - 0.2;
10  safeGuardUp[2] = JLIM3_MAX - 0.2;
11  safeGuardUp[3] = JLIM4_MAX - 0.4;
12 
13  safeGuardLow = new double[this->dimension];
14  safeGuardLow[0] = JLIM1_MIN + 0.25;
15  safeGuardLow[1] = JLIM2_MIN + 0.25;
16  safeGuardLow[2] = JLIM3_MIN + 0.25;
17  safeGuardLow[3] = JLIM4_MIN + 0.4;
18 
19  //We set as reference the middle point between the two safe guards that is,
20  //the point farther from both safe guard limits
21  halfPoint = new double[this->dimension];
22  for(int i=0; i< dimension; i++){
23  halfPoint[i] = (safeGuardUp[i] + safeGuardLow[i])/2.0;
24  //std::cout << "HALFPOINT : " << halfPoint << "\n";
25  }
26 
27 }
28 JointLimitTask::~JointLimitTask(){
29  delete []safeGuardLow;
30  delete []safeGuardUp;
31  delete []halfPoint;
32 }
33 
34 int JointLimitTask::updateMatrices(struct Infos* const robInfo){
35 
36  setActivation(robInfo->robotState.jState);
37  setJacobian();
38  setReference(robInfo->robotState.jState);
39  return 0;
40 
41 }
42 
43 int JointLimitTask::setActivation(std::vector<double> jState){
44 
45  double jLimUP[] = {JLIM1_MAX, JLIM2_MAX, JLIM3_MAX, JLIM4_MAX};
46  double jLimLOW[] = {JLIM1_MIN, JLIM2_MIN, JLIM3_MIN, JLIM4_MIN};
47 
48  for (int i=1; i<=dimension; i++){
49  A(i,i) = CMAT::DecreasingBellShapedFunction(jLimLOW[i-1], safeGuardLow[i-1],
50  0, 1, jState[i-1]) +
51  CMAT::IncreasingBellShapedFunction(safeGuardUp[i-1], jLimUP[i-1],
52  0, 1, jState[i-1]);
53  }
54 }
55 
56 
57 int JointLimitTask::setJacobian(){
58 
59  CMAT::Matrix eye = CMAT::Matrix::Eye(dimension);
60  CMAT::Matrix zero = CMAT::Matrix::Zeros(dimension,VEHICLE_DOF);
61 
62  CMAT::Matrix tot = eye.RightJuxtapose(zero);
63  J = tot;
64 
65 }
66 
73 int JointLimitTask::setReference(std::vector<double> jState){
74 
76 // for (int i=0; i<4; i++){
77 // std::cout << jState[i] << "\t";
78 // }
79 // std::cout << "\n";
80 
81  for (int i=0; i<dimension; i++){
82 
83  if (jState[i] <= halfPoint[i]){
84  reference(i+1) = gain * (safeGuardLow[i] - jState[i]);
85 
86  } else {
87  reference(i+1) = gain * (safeGuardUp[i] - jState[i]);
88 
89 
90  }
91  }
92  return 0;
93 }
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