Diff: InverseKinematicsControl.h

Revision:

0:843c9f29adde

--- /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