File content as of revision 0:843c9f29adde:

#include "SafeCheck.h"

SafeCheck::SafeCheck(ARAP180_WITH_ROVER *_arm, VectorXf _maxJointPos,VectorXf _minJointPos, VectorXf _maxJointVel, VectorXf _maxJointAcc, Vector3f _maxCartPos, Vector3f _minCartPos){
    
    arm = _arm;
    maxJointPos = _maxJointPos; 
    minJointPos = _minJointPos; 
    maxJointVel = _maxJointVel; 
    maxJointAcc = _maxJointAcc; 
    maxCartPos  = _maxCartPos; 
    minCartPos  = _minCartPos;     
}

void SafeCheck::setSafeCheckParameters(VectorXf _maxJointPos,VectorXf _minJointPos, VectorXf _maxJointVel, VectorXf _maxJointAcc, Vector3f _maxCartPos, Vector3f _minCartPos){

    maxJointPos = _maxJointPos; 
    minJointPos = _minJointPos; 
    maxJointVel = _maxJointVel; 
    maxJointAcc = _maxJointAcc; 
    maxCartPos  = _maxCartPos; 
    minCartPos  = _minCartPos;     
}

void SafeCheck::initSafeCheck(){
    isSafe = true;      
}

bool SafeCheck::check(VectorXf q, VectorXf dq, VectorXf ddq, checkMode mode){
    
    
    Matrix4f Te;
    Vector3f cartPos;
    
    for(int i = 0; i < q.size(); i++){
    
        if(q(i) > maxJointPos(i) || q(i) < minJointPos(i)) isSafe = false;
        if(fabs(dq(i)) > maxJointVel(i)) isSafe = false;
        if(fabs(ddq(i)) > maxJointAcc(i)) isSafe = false;

    }    
    
    
    if(mode == CARTESIAN_MODE){
        Te = arm->forwardKinematics(q);
        cartPos = Te.block(0,3,3,1);
     
        for(int i = 0; i < cartPos.size(); i++){
            if(cartPos(i) > maxCartPos(i) || cartPos(i) < minCartPos(i)) isSafe = false;
        }
    }
    
    return isSafe;
   
    
}