Merge branch 'main' of https://github.com/Joaopmoliveira/CuranSDK

Author: Joaopmoliveira

libraries/robotutils/CMakeLists.txt

@@ -9,6 +9,8 @@ src/ImpedanceController.cpp
 src/CartesianVelocityController.cpp
 src/RippleFilter.cpp
 src/ViewFiltering.cpp
+src/InertiaAwareRippleFilter.cpp
+src/JointImpedanceControl.cpp
 )
 
 target_link_libraries(robotutils 

libraries/robotutils/include/robotutils/InertiaAwareRippleFilter.h

@@ -16,28 +16,27 @@ namespace massripple{
             double delta = 0.0;
             size_t critical_index = 0;
 
-            Eigen::Matrix<double,n_joints,n_joints> B;
-            Eigen::Matrix<double,n_joints,n_joints> K;
-            Eigen::Matrix<double,n_joints,n_joints> D;
-            Eigen::Matrix<double,n_joints,n_joints> Kt;
-            Eigen::Matrix<double,n_joints,n_joints> R;
+            Eigen::Matrix<double,n_joints,1> B;
+            Eigen::Matrix<double,n_joints,1> K;
+            Eigen::Matrix<double,n_joints,1> D;
+            Eigen::Matrix<double,n_joints,1> Kt;
+            Eigen::Matrix<double,n_joints,1> R;
 
-            Damper(const Eigen::Matrix<double,n_joints,n_joints>& in_B,
-                    const Eigen::Matrix<double,n_joints,n_joints>& in_K,
-                    const Eigen::Matrix<double,n_joints,n_joints>& in_D,
-                    const Eigen::Matrix<double,n_joints,n_joints>& in_Kt,
-                    const Eigen::Matrix<double,n_joints,n_joints>& in_R) : B{in_B}, K{in_K}, D{in_D}, Kt{in_Kt}, R{in_R}
+            Damper(const Eigen::Matrix<double,n_joints,1>& in_B,
+                    const Eigen::Matrix<double,n_joints,1>& in_K,
+                    const Eigen::Matrix<double,n_joints,1>& in_D,
+                    const Eigen::Matrix<double,n_joints,1>& in_Kt,
+                    const Eigen::Matrix<double,n_joints,1>& in_R) : B{in_B}, K{in_K}, D{in_D}, Kt{in_Kt}, R{in_R}
             {
             }
 
 
             void compute(const Eigen::Matrix<double,n_joints,n_joints>& M,const Eigen::Matrix<double,n_joints,1>& dq,double sample_time){
                 Eigen::Matrix<double,n_joints,1> w = 2*R(dq.array().abs().matrix());
                 Eigen::Matrix<double,n_joints,1> G = Eigen::Matrix<double,n_joints,1>::Zero();
-                for(double& g : G.rowwise()){
-                    
-                }
-                double amplitude = G.array().abs().maxCoeff(critical_index);
+                for(size_t j = 0; j < n_joints; ++j)
+                    G[j] = Kt[j]/std::sqrt(std::pow(1+Kt[j]+B[j]/M(j,j)-w[j]*w[j]/(K[j]*B[j]),2)+std::pow(w[j]*D[j]/K[j]+w[j]*B[j]*D[j]/(K[j]*M(j,j)),2));
+                G.array().abs().maxCoeff(critical_index);
                 value = std::min((dq[critical_index]*dq[critical_index])/(0.1*0.1),1.0);
                 delta = std::abs(dq[critical_index]*sample_time);
             };

libraries/robotutils/src/JointImpedanceControl.cpp

@@ -1,4 +1,4 @@
-#include "robotutils/JointImpedanceController.h"
+#include "robotutils/JointImpedanceControl.h"
 
 namespace curan {
 namespace robotic {
@@ -11,12 +11,11 @@ namespace robotic {
         /*
         We remove some energy from the system whilst moving the robot in free space. Thus we guarantee that the system is passive
         */
-        Eigen::Matrix<double,7,1> desired_velocity = Eigen::Matrix<double,7,1>::Ones()*actuation;
+        //Eigen::Matrix<double,7,1> desired_velocity = Eigen::Matrix<double,7,1>::Ones()*actuation;
 
         //state.cmd_tau =  35*(desired_velocity-filtered_velocity);
-        state.cmd_tau =  35*iiwa.mass()*(desired_velocity-filtered_velocity);
-        state.user_defined = filtered_velocity;
-        timer += iiwa.sample_time();
+        state.cmd_tau =  Eigen::Matrix<double,7,1>::Zero();
+        //state.user_defined = filtered_velocity;
 
 
         /*