Update filter parameters for paper

Author: Joaopmoliveira

libraries/robotutils/include/robotutils/GenericStateDerivative.h

@@ -16,26 +16,27 @@ struct LowPassDerivativeFilter{
     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, double sample_time){
-        return update(in_raw_state,Eigen::Matrix<double,state_size,1>::Zero(),sample_time);
-    }
-
-    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> partial_removal, double sample_time){
+    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){
         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_raw_state.col(2) = (1.0/sample_time)*(raw_state.col(2)-raw_state.col(1))-partial_removal;
         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)+

libraries/robotutils/include/robotutils/RippleFilter.h

@@ -3,36 +3,53 @@
 
 #include <tuple>
 #include <array>
+#include <Eigen/Dense>
 
 namespace curan {
 namespace robotic {
 namespace ripple{
+
+        struct Damper{
+            double damper_derivative = 0.0;
+            double value = 0.0; 
+            double prev_vel = 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;
+                delta = std::abs(max_vel*sample_time);
+            };
+
+        };
+
         struct Properties{
-            double damper_prev;
-            double damper;
             double width;
             double frequency;
-            double delta;
-
-            Properties(double in_width = 5.0, double in_frequency = 320.0) : damper_prev{0.0},damper{0.0}, width{in_width}, frequency{in_frequency}, delta{0.0}  {};
-
+            Properties(double in_width = 5.0, double in_frequency = 320.0) : width{in_width}, frequency{in_frequency}{};
         };
 
         struct Data{
             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};
         };
 
-        struct Observation{
-            double current_vel;
-            double current_delta;
-        };
-
-        void update_filter_properties(Properties& properties,const Observation& observation);
-
-        void shift_filter_data(Data& data, const double& filtered_torque);
+        void shift_filter_data(Data& data);
 
-        void update_filter_data(Data& data, const double& torque);
+        void update_filter_data(Data& data, const double& torque, const double& dtorque);
 
         /*
         this function returns a tuple with the following rules:
@@ -41,7 +58,7 @@ namespace ripple{
         basically the derivative of the filtered torque has two components, the derivative due to the changes in the real torque
         and the changes due to the scalling function that shuts down the filter
         */
-        std::tuple<double,double> execute(const Properties& props, Data& data);
+        std::tuple<double,double> execute(const Properties& props,const Damper& damper, Data& data);
 
 }
 }

libraries/robotutils/include/robotutils/ViewFiltering.h

@@ -17,6 +17,8 @@ struct ViewFiltering : public UserData{
     std::array<ripple::Properties, number_of_joints> second_harmonic;
     std::array<ripple::Properties, number_of_joints> third_harmonic;
 
+    curan::robotic::ripple::Damper damper;
+
     LowPassDerivativeFilter<number_of_joints> filtering_mechanism;
     Eigen::Matrix<double,number_of_joints,1> previous_q;
 

libraries/robotutils/src/LBRController.cpp

@@ -79,7 +79,7 @@ void RobotLBR::command(){
     eigen_state = current_state.converteigen();
     Eigen::MatrixXd task_jacobian = Eigen::MatrixXd::Identity(number_of_joints,number_of_joints);
     eigen_state = std::move(user_data->update(robot_model,std::move(eigen_state),task_jacobian));
-    eigen_state.cmd_tau = add_constraints<number_of_joints>(robot_model,eigen_state.cmd_tau, 0.005);
+    //eigen_state.cmd_tau = add_constraints<number_of_joints>(robot_model,eigen_state.cmd_tau, 0.005);
     current_state.convertFrom(eigen_state);
     atomic_state.store(current_state,std::memory_order_relaxed);
     if (robotState().getClientCommandMode() == KUKA::FRI::TORQUE) {

libraries/robotutils/src/RippleFilter.cpp

@@ -6,34 +6,35 @@ namespace curan{
 namespace robotic{
 namespace ripple{
 
-void update_filter_properties(Properties& properties,const Observation& observation){
-    double damper_trigger = 0.2*properties.frequency;
-    double filtered_frequency = std::abs(observation.current_vel)*properties.frequency;
-    properties.damper_prev = properties.damper;
-    properties.damper =  filtered_frequency < damper_trigger ? std::pow(filtered_frequency/damper_trigger,2) : 1.0;
-    properties.delta = std::abs(observation.current_delta);
-}
-
-void shift_filter_data(Data& data, const double& filtered_torque){
+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] = filtered_torque;
+    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){
+void update_filter_data(Data& data, const double& torque, const double& dtorque){
     data.y[2] = torque;
+    data.dy[2] = dtorque;
 }
 
-std::tuple<double,double> execute(const Properties& props, Data& data, double sample_time){
-    const double A = 2.0 * props.width * props.frequency * props.delta;
-    const double B = 4.0 + props.frequency * props.frequency * props.delta * props.delta;
-    const double damper_derivative = (props.damper-props.damper_prev)/sample_time;
+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] );
-    data.y_f[2] = props.damper*y_f2;
-    return {data.y_f[2],damper_derivative*y_f2};
+    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]);
 }
 
 }

libraries/robotutils/src/ViewFiltering.cpp

@@ -15,56 +15,56 @@ namespace robotic {
     EigenState&& ViewFiltering::update(const RobotModel<number_of_joints>& iiwa, EigenState&& state, Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>& composed_task_jacobians){
         using vector_type = curan::robotic::RobotModel<curan::robotic::number_of_joints>::vector_type;
         static double currentTime = 0.0;
-        /*
-        We remove some energy from the system whilst moving the robot in free space. Thus we guarantee that the system is passive
-        */
-       
-        state.cmd_tau = -iiwa.mass() * iiwa.velocities();
+        
+        //We remove some energy from the system whilst moving the robot in free space. Thus we guarantee that the system is passive
+        
+        //state.cmd_tau = -iiwa.mass() * iiwa.velocities();
+        state.cmd_tau = vector_type::Zero();
         vector_type raw_filtered_torque = vector_type::Zero();
-        vector_type partial_derivative_torque = vector_type::Zero();
+        vector_type raw_deriv_filtered_torque = vector_type::Zero();
+
         if(filtering_mechanism.is_first)
             previous_q = iiwa.joints();
-        
-        double fastest_velocity_found = std::numeric_limits<double>::min();
-        size_t offset_fastest_filter = 0;
 
-        for (size_t joint_i = 0; joint_i < curan::robotic::number_of_joints; ++joint_i)
-        {
-            const curan::robotic::ripple::Observation obser_i_j{iiwa.velocities()[joint_i], iiwa.joints()[joint_i] - previous_q[joint_i]};
-            ripple::update_filter_properties(first_harmonic[joint_i], obser_i_j);
-            ripple::update_filter_properties(second_harmonic[joint_i], obser_i_j);
-            ripple::update_filter_properties(third_harmonic[joint_i], obser_i_j);
-            double vel_modulus_i = std::abs(iiwa.velocities()[joint_i]);
-            if (fastest_velocity_found < vel_modulus_i)
-            {
-                fastest_velocity_found = vel_modulus_i;
-                offset_fastest_filter = joint_i;
-            }
-        }
+        static vector_type init_q = iiwa.joints();
+        static vector_type prev_tau = iiwa.measured_torque();
+
+        damper.compute<curan::robotic::number_of_joints>(iiwa.velocities(),iiwa.sample_time());
         previous_q = iiwa.joints();
 
         for (size_t j = 0; j < curan::robotic::number_of_joints; ++j)
         {
-            double filtered_torque_value = iiwa.measured_state()->tau[j];
-            ripple::shift_filter_data(joint_data_first_harmonic[j], 0.0);
-            ripple::update_filter_data(joint_data_first_harmonic[j], filtered_torque_value);
-            auto [filtered_harm_1,partial_derivative_1] = ripple::execute(first_harmonic[offset_fastest_filter], joint_data_first_harmonic[j]);
+            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);
+            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;
-            ripple::shift_filter_data(joint_data_second_harmonic[j], 0.0);
-            ripple::update_filter_data(joint_data_second_harmonic[j], filtered_torque_value);
-            auto [filtered_harm_2,partial_derivative_2]= ripple::execute(second_harmonic[offset_fastest_filter], joint_data_second_harmonic[j]);
+            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);
+            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;
-            ripple::shift_filter_data(joint_data_third_harmonic[j], 0.0);
-            ripple::update_filter_data(joint_data_third_harmonic[j], filtered_torque_value);
-            auto [ filtered_harm_3,partial_derivative_3] = ripple::execute(third_harmonic[offset_fastest_filter], joint_data_third_harmonic[j]);
-            partial_derivative_torque[j] = partial_derivative_1 + partial_derivative_2 +partial_derivative_3;
+            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);
+            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;
             raw_filtered_torque[j] = -filtered_torque_value - iiwa.gravity()[j] - iiwa.coriolis()[j];
+            raw_deriv_filtered_torque[j] = -deriv_filtered_torque_value;
         }
 
-        const auto& [filtered_torque, filtered_torque_derivative] = filtering_mechanism.update(raw_filtered_torque,partial_derivative_torque,iiwa.sample_time());
-        state.cmd_q = iiwa.joints() + Eigen::Matrix<double,number_of_joints,1>::Constant(0.5 / 180.0 * M_PI * sin(2 * M_PI * 10 * currentTime));
+        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());
+        //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_defined4 = (iiwa.measured_torque()-prev_tau)/iiwa.sample_time();
         currentTime += iiwa.sample_time();
         return std::move(state);
     }

tests/robotcontrollers/CMakeLists.txt

@@ -207,4 +207,16 @@ communication
 renderable
 )
 
+add_executable(test_robot_controller test_robot_controller.cpp)
+
+target_compile_definitions(test_robot_controller PRIVATE SK_VULKAN)
+target_compile_definitions(test_robot_controller PRIVATE CURAN_COPIED_RESOURCE_PATH="${post_build_resource_path}")
+
+target_link_libraries(test_robot_controller PUBLIC
+utils
+robotutils
+communication
+renderable
+)
+