manipulabilityTask.cpp

#include "header/manipulabilityTask.h"

ManipulabilityTask::ManipulabilityTask(int dim, bool eqType, std::string robotName)
  : Task(dim, eqType, robotName, "MANIPULABILITY"){
  gain = 0.2;

  mu = 0.0;
}


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

  setActivation();
  setJacobian();
  setReference();
  return 0;

}

int ManipulabilityTask::setJacobian(){
  //CASINO mi serve la derivata della jacobiana
}


void ManipulabilityTask::setReference(){
  reference = gain * (0.12 - mu)

}

void ManipulabilityTask::setActivation(){
  A = CMAT::DecreasingBellShapedFunction(0.02, 0.05, 0, 1, mu);
}