vehicleConstrainVelTask.cpp

#include "header/vehicleConstrainVelTask.h"

VehicleConstrainVelTask::VehicleConstrainVelTask(int dim, bool eqType, std::string robotName)
  : Task(dim, eqType, robotName, "VEHICLE_CONSTRAIN_VEL") {
  //no gain: non reactive task

}


int VehicleConstrainVelTask::updateMatrices(struct Infos* const robInfo){

  setActivation();
  setJacobian();
  setReference(robInfo->robotState.vehicleVel);
  return 0;
}

void VehicleConstrainVelTask::setJacobian(){

  Eigen::Matrix<double, 6, TOT_DOF> J_eigen = Eigen::Matrix<double, 6, TOT_DOF>::Zero();
  J_eigen.rightCols(dimension) = Eigen::Matrix<double, 6, 6>::Identity();

  J = CONV::matrix_eigen2cmat(J_eigen);


}

void VehicleConstrainVelTask::setActivation(){

  double vectDiag[dimension];
  std::fill_n(vectDiag, dimension, 1);
  this->A.SetDiag(vectDiag);
}
void VehicleConstrainVelTask::setReference(std::vector<double> actualVel){

  for (int i=1; i<=dimension; ++i){
    this->reference(i) = actualVel[i-1];
  }
}