1 #include "header/coopTask.h" 3 CoopTask::CoopTask(
int dim,
bool eqType, std::string robotName)
4 :
Task(dim, eqType, robotName,
"COOP_CONSTRAINT"){
10 int CoopTask::updateMatrices(
Infos *
const robInfo){
13 setJacobian(robInfo->robotState.w_Jtool_robot);
14 setReference(robInfo->exchangedInfo.coopCartVel);
17 void CoopTask::setReference(Eigen::Matrix<double, VEHICLE_DOF, 1> coopCartVel){
20 reference = CONV::matrix_eigen2cmat(coopCartVel);
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);
33 void CoopTask::setActivation(){
35 double vectDiag[dimension];
36 std::fill_n(vectDiag, dimension, 1);
37 this->A.SetDiag(vectDiag);
42 void CoopTask::setJacobian(Eigen::Matrix<double, 6, TOT_DOF> w_Jtool_robot){
46 J = CONV::matrix_eigen2cmat(w_Jtool_robot);
48 }
else if (dimension == 5){
49 auto temp = CONV::matrix_eigen2cmat(w_Jtool_robot);
50 J = temp.DeleteRow(4);
ABSTRACT class task. Each task is a derived class of this class. It contains all the variable that th...