Retreive tests

Author: Joaopmoliveira

libraries/robotutils/include/robotutils/GenericStateDerivative.h

@@ -12,46 +12,27 @@ struct LowPassDerivativeFilter{
     Eigen::Matrix<double,state_size,3> raw_state;
     Eigen::Matrix<double,state_size,3> filtered_state;
 
-    Eigen::Matrix<double,state_size,3> deriv_raw_state;
-    Eigen::Matrix<double,state_size,3> deriv_filtered_state;
     bool is_first = true;
 
-    std::tuple<Eigen::Matrix<double,state_size,1>,Eigen::Matrix<double,state_size,1>> update(Eigen::Matrix<double,state_size,1> in_raw_state, Eigen::Matrix<double,state_size,1> deriv_in_raw_state, double sample_time){
+    Eigen::Matrix<double,state_size,1> update(Eigen::Matrix<double,state_size,1> in_raw_state, double sample_time){
         if(is_first){
             is_first = false;
             raw_state.col(2) = in_raw_state;
             raw_state.col(1) = raw_state.col(2);
             raw_state.col(0) = raw_state.col(2);
-
-            deriv_raw_state.col(2) = deriv_in_raw_state;
-            deriv_raw_state.col(1) = deriv_raw_state.col(2);
-            deriv_raw_state.col(0) = deriv_raw_state.col(2);
-
         } else {
             raw_state.col(2) = in_raw_state;
-            deriv_raw_state.col(2) = deriv_in_raw_state;
         }     
         filtered_state.col(2) = 0.222955 * raw_state.col(2)+
                                 0.445910 * raw_state.col(1)+
                                 0.222955 * raw_state.col(0)+
                                 0.295200 * filtered_state.col(1)+
                                -0.187020 * filtered_state.col(0);
-
-        deriv_filtered_state.col(2) = 0.222955 * deriv_raw_state.col(2)+
-                                0.445910 * deriv_raw_state.col(1)+
-                                0.222955 * deriv_raw_state.col(0)+
-                                0.295200 * deriv_filtered_state.col(1)+
-                               -0.187020 * deriv_filtered_state.col(0);
-
         for (size_t i = 0; i < 2; ++i){
             raw_state.col(i) = raw_state.col(i + 1);
             filtered_state.col(i) = filtered_state.col(i + 1);
-
-            deriv_raw_state.col(i) = deriv_raw_state.col(i + 1);
-            deriv_filtered_state.col(i) = deriv_filtered_state.col(i + 1);
-
         }
-        return {filtered_state.col(2),deriv_filtered_state.col(2)};
+        return filtered_state.col(2);
     }
 };
 

libraries/robotutils/include/robotutils/LBRController.h

@@ -53,11 +53,11 @@ class RobotLBR : public KUKA::FRI::LBRClient
     }
 
     inline operator bool() const {
-        return continue_robot_motion.load(std::memory_order_relaxed); 
+        return continue_robot_motion.load(); 
     }
 
     inline void cancel(){
-        continue_robot_motion.store(false,std::memory_order_relaxed);
+        continue_robot_motion.store(false);
     }
 
 private:

libraries/robotutils/include/robotutils/RippleFilter.h

@@ -3,6 +3,7 @@
 
 #include <tuple>
 #include <array>
+#include <numbers>
 #include <Eigen/Dense>
 
 namespace curan {
@@ -12,22 +13,16 @@ namespace ripple{
         struct Damper{
             double damper_derivative = 0.0;
             double value = 0.0; 
-            double prev_vel = 0.0; 
+            double prev_value = 0.0; 
             double delta = 0.0;
 
             template<size_t siz>
             void compute(const Eigen::Matrix<double,siz,1>& dq,double sample_time){
-                const double trigger_point = 0.1;
                 double max_vel = dq.array().abs().maxCoeff();
-                double possible_damper_value = std::pow(max_vel/trigger_point,2.0);
-                if(possible_damper_value < 1.0){
-                    value = possible_damper_value;
-                    damper_derivative = (2.0*max_vel)/(trigger_point*trigger_point)*((max_vel-prev_vel)/sample_time);
-                } else {
-                    value = 1.0;
-                    damper_derivative = 0.0;
-                }
-                prev_vel = max_vel;
+                //value = 0.5-0.5*cos(std::min(3.14159265358979323,std::abs(max_vel)*20.0));
+                value = std::min(std::abs(max_vel)*5,1.0);
+                damper_derivative = (value-prev_value)/sample_time;
+                prev_value = value;
                 delta = std::abs(max_vel*sample_time);
             };
 
@@ -43,13 +38,13 @@ namespace ripple{
             std::array<double,3> y_f = {0.0,0.0,0.0};
             std::array<double,3> y = {0.0,0.0,0.0};
 
-            std::array<double,3> dy_f = {0.0,0.0,0.0};
-            std::array<double,3> dy = {0.0,0.0,0.0};
+            //std::array<double,3> dy_f = {0.0,0.0,0.0};
+            //std::array<double,3> dy = {0.0,0.0,0.0};
         };
 
         void shift_filter_data(Data& data);
 
-        void update_filter_data(Data& data, const double& torque, const double& dtorque);
+        void update_filter_data(Data& data, const double& torque);
 
         /*
         this function returns a tuple with the following rules:

libraries/robotutils/src/RippleFilter.cpp

@@ -10,31 +10,21 @@ void shift_filter_data(Data& data){
     data.y[0] = data.y[1];
     data.y[1] = data.y[2];
 
-    data.dy[0] = data.dy[1];
-    data.dy[1] = data.dy[2];
-
     data.y_f[0] = data.y_f[1];
     data.y_f[1] = data.y_f[2];
     data.y_f[2] = 0.0;
-
-    data.dy_f[0] = data.dy_f[1];
-    data.dy_f[1] = data.dy_f[2];
-    data.dy_f[2] = 0.0;
 }
 
-void update_filter_data(Data& data, const double& torque, const double& dtorque){
+void update_filter_data(Data& data, const double& torque){
     data.y[2] = torque;
-    data.dy[2] = dtorque;
 }
 
 std::tuple<double,double> execute(const Properties& props,const Damper& damper, Data& data){
     const double A = 2.0 * props.width * props.frequency * damper.delta;
     const double B = 4.0 + props.frequency * props.frequency * damper.delta * damper.delta;
-    double y_f2 = (1.0/(A+B)) * (A*data.y[2] - A*data.y[0] - 2.0*(B-8.0) * data.y_f[1] - (B-A)*data.y_f[0] );
-    double dy_f2 = (1.0/(A+B)) * (A*data.dy[2] - A*data.dy[0] - 2.0*(B-8.0) * data.dy_f[1] - (B-A)*data.dy_f[0] );
-    data.y_f[2] = damper.value*y_f2;
-    data.dy_f[2] = damper.value*dy_f2;
-    return std::make_tuple(data.y_f[2],data.dy_f[2]);
+    double y_f2 = (damper.value/(A+B)) * (A*data.y[2] - A*data.y[0] - (2.0*(B-8.0) * data.y_f[1] + (B-A)*data.y_f[0]) );
+    data.y_f[2] = y_f2;
+    return std::make_tuple(y_f2,0.0);
 }
 
 }

libraries/robotutils/src/ViewFiltering.cpp

@@ -37,17 +37,17 @@ namespace robotic {
             double filtered_torque_value = iiwa.measured_torque()[j];
             double deriv_filtered_torque_value = (iiwa.measured_torque()[j]-prev_tau[j])/(iiwa.sample_time());
             curan::robotic::ripple::shift_filter_data(joint_data_first_harmonic[j]);
-            curan::robotic::ripple::update_filter_data(joint_data_first_harmonic[j], filtered_torque_value,deriv_filtered_torque_value);
+            curan::robotic::ripple::update_filter_data(joint_data_first_harmonic[j], filtered_torque_value);
             auto [filtered_harm_1,partial_derivative_1] = curan::robotic::ripple::execute(first_harmonic[j],damper, joint_data_first_harmonic[j]);
             filtered_torque_value -= filtered_harm_1;
             deriv_filtered_torque_value -= partial_derivative_1;
             curan::robotic::ripple::shift_filter_data(joint_data_second_harmonic[j]);
-            curan::robotic::ripple::update_filter_data(joint_data_second_harmonic[j], filtered_torque_value,deriv_filtered_torque_value);
+            curan::robotic::ripple::update_filter_data(joint_data_second_harmonic[j], filtered_torque_value);
             auto [filtered_harm_2,partial_derivative_2]= curan::robotic::ripple::execute(second_harmonic[j],damper, joint_data_second_harmonic[j]);
             filtered_torque_value -= filtered_harm_2;
             deriv_filtered_torque_value -= partial_derivative_2;
             curan::robotic::ripple::shift_filter_data(joint_data_third_harmonic[j]);
-            curan::robotic::ripple::update_filter_data(joint_data_third_harmonic[j], filtered_torque_value,deriv_filtered_torque_value);
+            curan::robotic::ripple::update_filter_data(joint_data_third_harmonic[j], filtered_torque_value);
             auto [ filtered_harm_3,partial_derivative_3] = curan::robotic::ripple::execute(third_harmonic[j],damper, joint_data_third_harmonic[j]);
             filtered_torque_value -= filtered_harm_3;
             deriv_filtered_torque_value -= partial_derivative_3;
@@ -58,12 +58,12 @@ namespace robotic {
         prev_tau = iiwa.measured_torque();
 
         //const auto& [filtered_torque, filtered_torque_derivative] = filtering_mechanism.update(raw_filtered_torque,partial_derivative_torque,iiwa.sample_time());
-        const auto& [filtered_torque, filtered_torque_derivative] = filtering_mechanism.update(raw_filtered_torque,raw_deriv_filtered_torque,iiwa.sample_time());
+        const auto& filtered_torque = filtering_mechanism.update(raw_filtered_torque,iiwa.sample_time());
         //state.cmd_q = iiwa.joints() + Eigen::Matrix<double,curan::robotic::number_of_joints,1>::Constant(0.5 / 180.0 * M_PI * sin(2 * M_PI * 10 * currentTime));
         state.cmd_q = init_q;
         state.cmd_q[0] = iiwa.joints()[0]+ (0.5 / 180.0 * M_PI * sin(2 * M_PI * 10 * currentTime));
         state.user_defined = raw_filtered_torque;
-        state.user_defined2 = filtered_torque_derivative;
+        //state.user_defined2 = filtered_torque_derivative;
         state.user_defined4 = (iiwa.measured_torque()-prev_tau)/iiwa.sample_time();
         currentTime += iiwa.sample_time();
         return std::move(state);