Revision 0:843c9f29adde, committed 2021-10-04

Comitter:

marcodesilva

Date:

Mon Oct 04 13:34:22 2021 +0000

Commit message:

arm control rendi libreria ;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FollowFilter.cpp	Mon Oct 04 13:34:22 2021 +0000
@@ -0,0 +1,113 @@
+#include "FollowFilter.h"
+
+FollowFilter::FollowFilter(VectorXf _omega, VectorXf _zita, VectorXf _maxJerk, VectorXf _maxAcc, VectorXf _maxVel, VectorXf _maxPos, VectorXf _minPos){
+    omega = _omega;
+    zita = _zita;
+    maxJerk = _maxJerk;
+    maxAcc = _maxAcc;
+    maxVel = _maxVel;
+    maxPos = _maxPos;
+    minPos = _minPos;    
+    dq_filtered = VectorXf::Zero( _omega.size(),1);
+    ddq_filtered = VectorXf::Zero( _omega.size(),1);
+    q_filtered = VectorXf::Zero( _omega.size(),1);
+}
+
+void FollowFilter::setFollowFilterParameters(VectorXf _omega, VectorXf _zita, VectorXf _maxJerk, VectorXf _maxAcc, VectorXf _maxVel, VectorXf _maxPos, VectorXf _minPos){
+    
+    omega = _omega;
+    zita = _zita;
+    maxJerk = _maxJerk;
+    maxAcc = _maxAcc;
+    maxVel = _maxVel;
+    maxPos = _maxPos;
+    minPos = _minPos;    
+   
+    
+}
+
+void FollowFilter::initFollowFilter(VectorXf _first_q){
+    
+    first_q = _first_q;
+    dq_filtered = VectorXf::Zero( _first_q.size(),1);
+    ddq_filtered = VectorXf::Zero( _first_q.size(),1);
+    q_filtered = VectorXf::Zero( _first_q.size(),1);
+    q_filtered = first_q;
+    
+    
+}
+
+void FollowFilter::getJointCmd(VectorXf &q_cmd, VectorXf &dq_cmd,VectorXf &ddq_cmd){
+    q_cmd.resize(q_filtered.size(),1);
+    dq_cmd.resize(q_filtered.size(),1);
+    ddq_cmd.resize(q_filtered.size(),1);
+    q_cmd = q_filtered;
+    dq_cmd = dq_filtered;
+    ddq_cmd = ddq_filtered;    
+
+}
+
+
+void FollowFilter::updateFollowFilter(VectorXf q_in, float dT ) {
+
+    float ddq = 0.0;
+    float dq  = 0.0;
+    float posError = 0.0;
+    float jerk = 0.0;
+
+    for(int j = 0; j<q_in.size(); j++){
+        posError = q_in(j) - q_filtered(j);
+        
+        ddq = omega(j) * omega(j) * posError - 2.0 * zita(j) * omega(j) * dq_filtered(j);
+        
+        //Jerk
+        /*jerk = (ddq - ddq_filtered(j))/dT;
+        
+        //---jerk saturation
+        if( fabs( jerk )  > maxJerk(j) ) {
+            if( jerk > 0.0 )
+                jerk = (maxJerk(j) ); // / jerk[j_id] );
+            else
+                jerk = -(maxJerk(j) ); // / jerk[j_id] );
+        }  
+        //---
+        
+        //acceleration
+        ddq = ddq_filtered(j) + jerk*dT;*/
+           
+          //---acc saturation    
+        if( fabs( ddq )  > maxAcc(j) ) {
+            if( ddq > 0.0 )
+                ddq_filtered(j) = maxAcc(j); // / ddq[j_id];
+            else
+                ddq_filtered(j) = -maxAcc(j); // / ddq[j_id];
+        }else {
+                ddq_filtered(j) = ddq;
+        }      
+          //---
+        
+        //velocity
+        dq = dq_filtered(j) + ddq_filtered(j) * dT;
+           
+        //---veloticyt saturation    
+        if( fabs( dq ) > maxVel(j) ) {    
+            if( dq > 0.0 )
+                dq_filtered(j) = maxVel(j);
+            else  
+                dq_filtered(j) =  -maxVel(j);
+        }else {
+                dq_filtered(j) = dq;
+        }        
+          //---
+        
+          //Aggiornamento posizione
+        q_filtered(j)  += dq_filtered(j) * dT;
+        
+        if(q_filtered(j) > maxPos(j)){ q_filtered(j) = maxPos(j);}
+        if(q_filtered(j) < minPos(j)){ q_filtered(j) = minPos(j);}
+        
+
+        
+    }  
+    
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FollowFilter.h	Mon Oct 04 13:34:22 2021 +0000
@@ -0,0 +1,34 @@
+#ifndef FOLLOW_FILTER_H
+#define FOLLOW_FILTER_H
+
+#include "mbed.h"
+#include <Eigen/Dense.h>
+
+using namespace Eigen;
+
+class FollowFilter
+{
+public:
+    FollowFilter(VectorXf _omega, VectorXf _zita, VectorXf _maxJerk, VectorXf _maxAcc, VectorXf _maxVel, VectorXf _maxPos, VectorXf _minPos);
+    void setFollowFilterParameters(VectorXf _omega, VectorXf _zita, VectorXf _maxJerk, VectorXf _maxAcc, VectorXf _maxVel, VectorXf _maxPos, VectorXf _minPos);
+    void initFollowFilter(VectorXf _first_q);
+    void updateFollowFilter(VectorXf q_in, float deltaT);
+    void getJointCmd(VectorXf &q_cmd, VectorXf &dq_cmd,VectorXf &ddq_cmd);
+
+private:
+
+    VectorXf q_filtered;
+    VectorXf dq_filtered;
+    VectorXf ddq_filtered;
+    VectorXf first_q;
+    VectorXf omega;
+    VectorXf zita;
+    VectorXf maxJerk;
+    VectorXf maxAcc;
+    VectorXf maxVel;
+    VectorXf maxPos;
+    VectorXf minPos;
+
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/InverseKinematicsControl.cpp	Mon Oct 04 13:34:22 2021 +0000
@@ -0,0 +1,267 @@
+#include "InverseKinematicsControl.h"
+
+InverseKinematicsControl::InverseKinematicsControl(ARAP180_WITH_ROVER *_arm ,Vector6f _K_first, Vector6f _K_second, Vector6f _D, Vector8f _maxAcc, Vector8f _maxVel, Vector8f _maxPos, Vector8f _minPos, int _n_max_iter, float _max_pError, float _max_oError){
+    
+    arm = _arm;
+    maxAcc = _maxAcc;
+    maxVel = _maxVel;
+    maxPos = _maxPos;
+    minPos = _minPos;  
+    
+    K_first = _K_first;
+    K_second = _K_second;
+
+    D = _D;
+    
+    n_max_iter = _n_max_iter;
+    max_pError = _max_pError;
+    max_oError = _max_oError;
+    
+    q = Vector8f::Zero();
+    dq = Vector8f::Zero();
+    ddq = Vector8f::Zero();
+    
+    ddx = Vector6f::Zero();
+    
+    Te = Matrix4f::Identity();
+    
+    J = Matrix<float,6,8>::Zero();
+    pinvJ = Matrix<float,8,6>::Zero();
+    J_T = Matrix<float,8,6>::Zero();
+    L = Matrix3f::Zero();
+    pinvL = Matrix3f::Zero();
+    
+    Jp = Matrix<float,6,8>::Zero();
+    
+    Vector8f W_vec;
+    W_vec << 10,1000,1,1,1,1,1,1;
+    
+    W = W_vec.cast<float>().asDiagonal();
+}
+
+void InverseKinematicsControl::setInverseKinematicsControlParameters(Vector6f _K_first, Vector6f _K_second, Vector6f _D, Vector8f WeightVector, Vector8f _maxAcc, Vector8f _maxVel, Vector8f _maxPos, Vector8f _minPos, int _n_max_iter, float _max_pError, float _max_oError){
+
+    maxAcc = _maxAcc;
+    maxVel = _maxVel;
+    maxPos = _maxPos;
+    minPos = _minPos; 
+
+    K_first = _K_first;
+    K_second = _K_second;
+    
+    n_max_iter = _n_max_iter;
+    max_pError = _max_pError;
+    max_oError = _max_oError; 
+    
+    W = WeightVector.cast<float>().asDiagonal();
+
+}
+
+
+void InverseKinematicsControl::initInverseKinematicsControl(Vector8f _first_q){
+    
+    q = _first_q;
+    dq = Vector8f::Zero();
+    ddq = Vector8f::Zero();
+    ddx = Vector6f::Zero();
+
+}
+
+void InverseKinematicsControl::getJointCmd(VectorXf &q_cmd, VectorXf &dq_cmd,VectorXf &ddq_cmd){
+    q_cmd.resize(q.size(),1);
+    dq_cmd.resize(q.size(),1);
+    ddq_cmd.resize(q.size(),1);
+    q_cmd = q;
+    dq_cmd = dq;
+    ddq_cmd = ddq;    
+
+}
+
+void InverseKinematicsControl::getDebugVector(Matrix<float, 24, 1>  &_debugVector){
+    _debugVector = debugVector;  
+
+}
+
+
+
+void InverseKinematicsControl::updateInverseKinematicsControl_firstOrder(Matrix4f Td, Vector6f dx_d, Vector8f dq_null_space, float deltaT) {
+    
+    Matrix3f Kp = Matrix3f::Zero();
+    Matrix3f Ko = Matrix3f::Zero();
+    Kp = K_first.block(0,0,3,1).cast<float>().asDiagonal();
+    Ko = K_first.block(3,0,3,1).cast<float>().asDiagonal();
+
+    //Matrix6f W_inv = Matrix6f::Identity();
+
+    Vector3f dxp = Vector3f::Zero();
+    Vector3f dxo = Vector3f::Zero();
+    dx = Vector6f::Zero();
+
+    int n_iter = 0;
+
+    float pError = 0.0;
+    float oError = 0.0;
+
+    Te = arm->forwardKinematics(q);
+    J = arm->jacobianMatrix(q);
+     
+    ep = Td.block(0,3,3,1) - Te.block(0,3,3,1);     
+    eo = utilities::rotationMatrixError(Td, Te);  
+
+    pError = ep.norm();                                     //L_2 norm of vector
+    oError = eo.norm();
+
+    //while(n_iter<n_max_iter && (pError>max_pError || oError>max_oError)){
+
+        L = utilities::L_matrix(Td.block(0,0,3,3), Te.block(0,0,3,3));
+
+        pinvL = L.inverse();
+
+        dxp =  dx_d.block(0,0,3,1) + Kp*ep;
+
+        dxo = pinvL*(L.transpose()*dx_d.block(3,0,3,1) + Ko*eo);
+
+        dx << dxp[0], dxp[1], dxp[2], dxo[0], dxo[1], dxo[2];
+
+        J_T = J.transpose();
+
+        // Pseudo inversa a destra perchè ho più giunti che gradi di libertà
+        FullPivLU<Matrix6f> Core_J(J*W.inverse()*J_T);
+        pinvJ = W.inverse()*J_T*Core_J.inverse();
+        
+        //pinvJ = J.inverse();
+       
+        dq = pinvJ*dx + (Matrix<float,8,8>::Identity() - pinvJ*J)*dq_null_space;
+       
+        /*for(int j = 0; j<dq.size(); j++){
+            if(dq(j) > maxVel(j)){ dq(j) = maxVel(j);}
+            if(dq(j) < -maxVel(j)){ dq(j) = -maxVel(j);}
+        }*/
+
+            
+        q += dq*deltaT;  
+        
+        for(int j = 0; j<q.size(); j++){
+            if(q(j) > maxPos(j)){ q(j) = maxPos(j);}
+            if(q(j) < minPos(j)){ q(j) = minPos(j);}
+        }
+        
+        
+        /*Te = arm->forwardKinematics(q);
+        J = arm->jacobianMatrix(q);
+        
+        ep = Td.block(0,3,3,1) - Te.block(0,3,3,1);     
+        eo = utilities::rotationMatrixError(Td, Te);  
+  
+        pError = ep.norm();                                     //L_2 norm of vector
+        oError = eo.norm();
+
+        debugVector.block(0,0,6,1) << ep[0], ep[1], ep[2], eo[0], eo[1], eo[2];
+        debugVector.block(6,0,6,1) << de;
+        //debugVector.block(12,0,6,1) << dq;
+        //debugVector.block(18,0,6,1) <<  ddq;
+                
+        n_iter++;*/
+
+    //}
+
+}
+
+
+//SECOND ORDER CLICK
+
+void InverseKinematicsControl::updateInverseKinematicsControl_secondOrder(Matrix4f Td, Vector6f dx_d, Vector6f ddx_d, float deltaT) {
+    
+
+    Matrix6f K_mat = Matrix6f::Zero();
+    Matrix6f D_mat = Matrix6f::Zero();
+    K_mat = K_second.cast<float>().asDiagonal();
+    D_mat = D.cast<float>().asDiagonal();
+
+    int n_iter = 0;
+
+    float pError = 0.0;
+    float oError = 0.0;
+
+    Te = arm->forwardKinematics(q);
+    J = arm->jacobianMatrix(q);    
+    L = utilities::L_matrix(Td.block(0,0,3,3), Te.block(0,0,3,3));
+
+    ep = Td.block(0,3,3,1) - Te.block(0,3,3,1);     
+    eo = utilities::rotationMatrixError(Td, Te); 
+    
+    dep = dx_d.block(0,0,3,1) - J.block(0,0,3,8)*dq; //dq deve entrare     
+    deo = L.transpose()*dx_d.block(3,0,3,1) - L*J.block(3,0,3,8)*dq;    //  
+    de << dep[0], dep[1], dep[2], deo[0], deo[1], deo[2];
+    e << ep[0], ep[1], ep[2], eo[0], eo[1], eo[2];
+
+    pError = ep.norm();                                     //L_2 norm of vector
+    oError = eo.norm();
+    Matrix<float,6,8> J_dot;
+
+    while(n_iter<n_max_iter && (pError>max_pError || oError>max_oError)){
+        
+        
+        J_dot = (J - Jp)/deltaT;
+        Jp = J;
+
+        ddx = ddx_d + D_mat*de + K_mat*e - J_dot*dq; //arm->jacobianTimeDerivativeMatrix(q, dq)*dq
+        
+        J_T = J.transpose();
+        
+        FullPivLU<Matrix6f> Core_J(J*W.inverse()*J_T);
+        pinvJ = W.inverse()*J_T*Core_J.inverse();
+        
+
+        //pinvJ = J.inverse();
+        
+        ddq = pinvJ*ddx;
+ 
+
+        /*for(int j = 0; j<ddq.size(); j++){
+            if(ddq(j) > maxAcc(j)){ ddq(j) = maxAcc(j);}
+            if(ddq(j) < -maxAcc(j)){ ddq(j) = -maxAcc(j);}
+        }*/
+        
+        dq += ddq*deltaT;
+            
+        /*for(int j = 0; j<dq.size(); j++){
+            if(dq(j) > maxVel(j)){ dq(j) = maxVel(j);}
+            if(dq(j) < -maxVel(j)){ dq(j) = -maxVel(j);}
+        }*/
+            
+        q += dq*deltaT;  
+        
+        for(int j = 0; j<q.size(); j++){
+            if(q(j) > maxPos(j)){ q(j) = maxPos(j);}
+            if(q(j) < minPos(j)){ q(j) = minPos(j);}
+        }
+        
+        
+        Te = arm->forwardKinematics(q);
+        J = arm->jacobianMatrix(q);
+        L = utilities::L_matrix(Td.block(0,0,3,3), Te.block(0,0,3,3));
+
+        
+        ep = Td.block(0,3,3,1) - Te.block(0,3,3,1);     
+        eo = utilities::rotationMatrixError(Td, Te);  
+    
+        dep = dx_d.block(0,0,3,1) - J.block(0,0,3,8)*dq; //dq deve entrare     
+        deo = L.transpose()*dx_d.block(3,0,3,1) - L*J.block(3,0,3,8)*dq;    //  
+        de << dep[0], dep[1], dep[2], deo[0], deo[1], deo[2];
+        e << ep[0], ep[1], ep[2], eo[0], eo[1], eo[2];
+  
+        pError = ep.norm();                                     //L_2 norm of vector
+        oError = eo.norm();
+        
+        debugVector.block(0,0,6,1) << e;
+        debugVector.block(6,0,6,1) << de;
+        //debugVector.block(12,0,6,1) << dq;
+        //debugVector.block(18,0,6,1) <<  ddq;
+                
+        n_iter++;
+
+    }
+
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/InverseKinematicsControl.h	Mon Oct 04 13:34:22 2021 +0000
@@ -0,0 +1,80 @@
+#ifndef INVERSE_KINEMATICS_CONTROL_H
+#define INVERSE_KINEMATICS_CONTROL_H
+
+#include "mbed.h"
+#include <Eigen/Dense.h>
+#include "utilities.hpp"
+#include "ARAP180_with_rover.h"
+
+using namespace Eigen;
+
+class InverseKinematicsControl
+{
+public:
+    InverseKinematicsControl(ARAP180_WITH_ROVER *_arm ,Vector6f _K_first, Vector6f _K_second, Vector6f _D, Vector8f _maxAcc, Vector8f _maxVel, Vector8f _maxPos, Vector8f _minPos, int _n_max_iter, float _max_pError, float _max_oError);
+    void setInverseKinematicsControlParameters(Vector6f _K_first, Vector6f _K_second, Vector6f _D, Vector8f WeightVector, Vector8f _maxAcc, Vector8f _maxVel, Vector8f _maxPos, Vector8f _minPos, int _n_max_iter, float _max_pError, float _max_oError);
+    void initInverseKinematicsControl(Vector8f _first_q);
+
+    void updateInverseKinematicsControl_firstOrder(Matrix4f Td, Vector6f dx_d, Vector8f dq_null_space, float deltaT);
+    void updateInverseKinematicsControl_secondOrder(Matrix4f Td, Vector6f dx_d, Vector6f ddx_d, float deltaT);
+
+    void getJointCmd(VectorXf &q_cmd, VectorXf &dq_cmd,VectorXf &ddq_cmd);
+    void getDebugVector(Matrix<float, 24, 1>  &_debugVector);
+
+
+private:
+
+    ARAP180_WITH_ROVER *arm;
+
+    Vector8f q;
+    Vector8f dq;
+    Vector8f ddq;
+    Vector6f h_s_s;
+    Vector6f h_e_b;
+    
+    Vector6f ddx;
+    Vector6f dx;
+
+    Vector3f ep;     
+    Vector3f eo; 
+    
+    Vector3f dep;     
+    Vector3f deo;
+    Vector6f de;
+    Vector6f e;
+        
+    Vector8f maxAcc;
+    Vector8f maxVel;
+    Vector8f maxPos;
+    Vector8f minPos;
+    
+    Vector6f K_first;
+    Vector6f K_second;
+
+    Vector6f D;
+    
+    int n_max_iter;
+    float max_pError;
+    float max_oError;
+    
+    Matrix4f Te;
+    Matrix4f T_5_b;    
+    Matrix6f Q_s_b; 
+    Matrix6f Q_b_s;  
+    
+    Matrix<float,6,8> J;
+    Matrix<float,8,6> pinvJ;
+    Matrix<float,8,6> J_T;
+    
+    Matrix<float,8,8> W;
+    Matrix3f L;
+    Matrix3f pinvL;
+    
+    Matrix<float, 24, 1>  debugVector;
+    
+    Matrix<float,6,8> Jp;
+
+
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SafeCheck.cpp	Mon Oct 04 13:34:22 2021 +0000
@@ -0,0 +1,56 @@
+#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;
+   
+    
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SafeCheck.h	Mon Oct 04 13:34:22 2021 +0000
@@ -0,0 +1,41 @@
+#ifndef SAFE_CHECK_H
+#define SAFE_CHECK_H
+
+#include "mbed.h"
+#include <Eigen/Dense.h>
+#include "ARAP180_with_rover.h"
+
+using namespace Eigen;
+
+class SafeCheck
+{
+public:
+
+    enum checkMode {
+        JOINT_MODE,
+        CARTESIAN_MODE,
+    };
+    
+    SafeCheck(ARAP180_WITH_ROVER *_arm, VectorXf _maxJointPos,VectorXf _minJointPos, VectorXf _maxJointVel, VectorXf _maxJointAcc, Vector3f _maxCartPos, Vector3f _minCartPos);
+    void setSafeCheckParameters(VectorXf _maxJointPos,VectorXf _minJointPos, VectorXf _maxJointVel, VectorXf _maxJointAcc, Vector3f _maxCartPos, Vector3f _minCartPos);
+    void initSafeCheck();
+    bool check(VectorXf q, VectorXf dq, VectorXf ddq, checkMode mode);
+    
+
+   
+
+private:
+    ARAP180_WITH_ROVER *arm;
+
+    VectorXf maxJointPos;
+    VectorXf minJointPos;
+    VectorXf maxJointVel;
+    VectorXf maxJointAcc;
+    Vector3f maxCartPos;
+    Vector3f minCartPos;
+    
+    bool isSafe;
+
+};
+
+#endif