1 #include "header/jointLimitTask.h" 3 JointLimitTask::JointLimitTask(
int dim,
bool eqType, std::string robotName)
4 :
Task(dim, eqType, robotName,
"JOINT_LIMIT"){
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;
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;
21 halfPoint =
new double[this->dimension];
22 for(
int i=0; i< dimension; i++){
23 halfPoint[i] = (safeGuardUp[i] + safeGuardLow[i])/2.0;
28 JointLimitTask::~JointLimitTask(){
29 delete []safeGuardLow;
34 int JointLimitTask::updateMatrices(
struct Infos*
const robInfo){
36 setActivation(robInfo->robotState.jState);
38 setReference(robInfo->robotState.jState);
43 int JointLimitTask::setActivation(std::vector<double> jState){
45 double jLimUP[] = {JLIM1_MAX, JLIM2_MAX, JLIM3_MAX, JLIM4_MAX};
46 double jLimLOW[] = {JLIM1_MIN, JLIM2_MIN, JLIM3_MIN, JLIM4_MIN};
48 for (
int i=1; i<=dimension; i++){
49 A(i,i) = CMAT::DecreasingBellShapedFunction(jLimLOW[i-1], safeGuardLow[i-1],
51 CMAT::IncreasingBellShapedFunction(safeGuardUp[i-1], jLimUP[i-1],
57 int JointLimitTask::setJacobian(){
59 CMAT::Matrix eye = CMAT::Matrix::Eye(dimension);
60 CMAT::Matrix zero = CMAT::Matrix::Zeros(dimension,VEHICLE_DOF);
62 CMAT::Matrix tot = eye.RightJuxtapose(zero);
73 int JointLimitTask::setReference(std::vector<double> jState){
81 for (
int i=0; i<dimension; i++){
83 if (jState[i] <= halfPoint[i]){
84 reference(i+1) = gain * (safeGuardLow[i] - jState[i]);
87 reference(i+1) = gain * (safeGuardUp[i] - jState[i]);
ABSTRACT class task. Each task is a derived class of this class. It contains all the variable that th...