1 #include "header/pipeReachTask.h" 3 PipeReachTask::PipeReachTask(
int dim,
bool eqType, std::string robotName, VehArmType vehArmType, AngLinType angLinType)
4 :
Task(dim, eqType, robotName,
"PIPE_REACHING_GOAL"){
8 this->vehArmType = vehArmType;
9 this->angLinType = angLinType;
14 int PipeReachTask::updateMatrices(
Infos *
const robInfo){
17 setJacobian(robInfo->robotState.w_Jtool_robot);
18 setReference(robInfo->transforms.wTgoalTool_eigen, robInfo->transforms.wTt_eigen);
21 int PipeReachTask::setActivation(){
23 double vectDiag[dimension];
24 std::fill_n(vectDiag, dimension, 1);
25 this->A.SetDiag(vectDiag);
31 void PipeReachTask::setJacobian(Eigen::Matrix<double, 6, TOT_DOF> w_J_robot){
35 w_J_robot.leftCols<ARM_DOF>() = Eigen::MatrixXd::Zero(6, ARM_DOF);
39 w_J_robot.rightCols<VEHICLE_DOF>() = Eigen::MatrixXd::Zero(6, VEHICLE_DOF);
47 J = CONV::matrix_eigen2cmat(w_J_robot);
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){
56 J = CONV::matrix_eigen2cmat(w_J_robot.topRows<3>());
61 J = CONV::matrix_eigen2cmat(w_J_robot.bottomRows<3>());
66 std::cerr <<
"ERROR: dimension 3 and not lin ang specified\n";
79 void PipeReachTask::setReference(Eigen::Matrix4d wTgoaltool_eigen, Eigen::Matrix4d wTtool_eigen){
81 CMAT::TransfMatrix wTgoaltool_cmat = CONV::matrix_eigen2cmat(wTgoaltool_eigen);
82 CMAT::TransfMatrix wTtool_cmat = CONV::matrix_eigen2cmat(wTtool_eigen);
86 CMAT::Vect6 errorSwapped = CMAT::CartError(wTgoaltool_cmat, wTtool_cmat);
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);
97 CMAT::Vect3 vect3_lin;
98 CMAT::Vect3 vect3_ang;
99 vect3_lin = (this->gainLin * errorSwapped.GetSecondVect3());
100 vect3_ang= (this->gainAng * errorSwapped.GetFirstVect3());
102 vect3_lin = FRM::saturateCmat(vect3_lin, 0.1);
103 vect3_ang = FRM::saturateCmat(vect3_ang, 0.1);
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);
112 }
else if (dimension == 5){
115 CMAT::Vect3 vect3_lin;
116 CMAT::Matrix vect2_ang(2,1);
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);
124 vect3_lin = (this->gainLin * errorSwapped.GetSecondVect3());
125 vect2_ang(1) = this->gainAng * errorSwapped(2);
126 vect2_ang(2) = this->gainAng * errorSwapped(3);
128 vect3_lin = FRM::saturateCmat(vect3_lin, 0.1);
129 vect2_ang = FRM::saturateCmat(vect2_ang, 0.1);
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);
137 }
else if (dimension == 3){
141 error(1)= errorSwapped(4);
142 error(2)= errorSwapped(5);
143 error(3)= errorSwapped(6);
145 CMAT::Vect3 vect3_lin;
146 vect3_lin = (this->gainLin * errorSwapped.GetSecondVect3());
148 vect3_lin = FRM::saturateCmat(vect3_lin, 0.1);
150 this->reference(1) = vect3_lin(1);
151 this->reference(2) = vect3_lin(2);
152 this->reference(3) = vect3_lin(3);
158 error(1)= errorSwapped(1);
159 error(2)= errorSwapped(2);
160 error(3)= errorSwapped(3);
162 CMAT::Vect3 vect3_ang;
163 vect3_ang= (this->gainAng * errorSwapped.GetFirstVect3());
165 vect3_ang = FRM::saturateCmat(vect3_ang, 0.1);
167 this->reference(1) = vect3_ang(1);
168 this->reference(2) = vect3_ang(2);
169 this->reference(3) = vect3_ang(3);
175 std::cerr <<
"ERROR: dimension 3 and not lin ang specified\n";
ABSTRACT class task. Each task is a derived class of this class. It contains all the variable that th...