Commit final ripple experiment

Author: Joaopmoliveira

.gitignore

@@ -3,4 +3,6 @@ build/
 .vscode/
 _site/
 resources/swing/R4F.glb
-plus_config/wire_reconstructions/wire_reconstruction_recording/RecordingTest.igs_20240508_215732.mha
+plus_config/wire_reconstructions/wire_reconstruction_recording/RecordingTest.igs_20240508_215732.mha
+
+isofurface_extraction.exe "C:\Dev\experiments_for_thesis\registration\vol\reconstruction_results1.mha" "C:\Dev\experiments_for_thesis\registration\vol\masked_volume.mha"

CMakeLists.txt

@@ -22,8 +22,6 @@ set(VCPKG_ROOT "${CMAKE_CURRENT_LIST_DIR}/vcpkg")
 set(CMAKE_TOOLCHAIN_FILE "${VCPKG_ROOT}/scripts/buildsystems/vcpkg.cmake")
 
 
-
-
 project(Curan)
 
 if(WIN32)
@@ -47,18 +45,18 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
 #Find all required third parties (this should be moved elsewhere)
 find_package(asio CONFIG REQUIRED)
 find_package(Eigen3 CONFIG REQUIRED)
+find_package(Ceres CONFIG REQUIRED)
+find_package(unofficial-skia CONFIG REQUIRED)
 find_package(nlohmann_json REQUIRED)
 find_package(Vulkan REQUIRED)
+find_package(glfw3 3.3 REQUIRED)
+find_package(Boost COMPONENTS filesystem)
 find_package(vsg CONFIG REQUIRED)
 find_package(vsgXchange CONFIG REQUIRED)
 find_package(vsgImGui CONFIG REQUIRED)
 find_package(RBDL CONFIG REQUIRED)
-#find_package(Torch CONFIG REQUIRED)
-#find_package(OpenMP REQUIRED)
-#find_package(LAPACK REQUIRED)
-#find_package(BLAS REQUIRED)
-#find_package(gflags REQUIRED)
-#find_package(glog REQUIRED)
+find_package(gflags REQUIRED)
+find_package(glog REQUIRED)
 
 
 #Externally provided targets
@@ -98,7 +96,7 @@ add_custom_target(
 add_subdirectory(libraries)
 
 # Add medical applications
-#add_subdirectory(applications)
+add_subdirectory(applications)
 
 # Most of the time there is no need to compile the tests and the tutorials
 # folder so you can comment out these subfolders from the build directory

libraries/CMakeLists.txt

@@ -1,9 +1,9 @@
 add_subdirectory(utils)
 add_subdirectory(communication)
-#add_subdirectory(userinterface)
+add_subdirectory(userinterface)
 add_subdirectory(rendering)
-#add_subdirectory(imageprocessing)
-#add_subdirectory(optimization)
-#add_subdirectory(geometry)
+add_subdirectory(imageprocessing)
+add_subdirectory(optimization)
+add_subdirectory(geometry)
 add_subdirectory(gaussianmixtures)
 add_subdirectory(robotutils)

libraries/robotutils/include/robotutils/GenericStateDerivative.h

@@ -0,0 +1,82 @@
+#ifndef CURAN_MASSRIPPLE_FILTER_
+#define CURAN_MASSRIPPLE_FILTER_
+
+#include <tuple>
+#include <array>
+#include <numbers>
+#include <Eigen/Dense>
+
+namespace curan {
+namespace robotic {
+namespace massripple{
+
+        template<size_t n_joints>
+        struct Damper{
+            double value = 0.0; 
+            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;
+
+            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}
+            {
+            }
+
+
+            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);
+                value = std::min((dq[critical_index]*dq[critical_index])/(0.1*0.1),1.0);
+                delta = std::abs(dq[critical_index]*sample_time);
+            };
+
+        };
+
+        struct Properties{
+            double width;
+            double frequency;
+            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};
+        };
+
+        void shift_filter_data(Data& data);
+
+        void update_filter_data(Data& data, const double& torque);
+
+        /*
+        this function returns a tuple with the following rules:
+        [ filtered_torque , ficticious_torque_deriv_by_activation_function ]
+
+        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
+        */
+        template<size_t n_joints>
+        double execute(const std::array<Properties,n_joints>& props,const Damper<n_joints>& damper, Data& data){
+            const double A = 2.0 * props[damper.critical_index].width * props[damper.critical_index].frequency * damper.delta;
+            const double B = 4.0 + props[damper.critical_index].frequency * props[damper.critical_index].frequency * damper.delta * damper.delta;
+            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 y_f2;
+        }
+
+}
+}
+}
+
+#endif

libraries/robotutils/include/robotutils/InertiaAwareRippleFilter.h

@@ -0,0 +1,17 @@
+#ifndef CURAN_JOINT_IMPEDANCE_CONTROLLER_
+#define CURAN_JOINT_IMPEDANCE_CONTROLLER_
+
+#include "LBRController.h"
+
+namespace curan {
+namespace robotic {
+
+struct JointImpedanceController : public UserData{
+    JointImpedanceController();
+    EigenState&& update(const RobotModel<number_of_joints>& iiwa, EigenState&& state, Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>& composed_task_jacobians) override;
+};
+
+}
+}
+
+#endif

libraries/robotutils/include/robotutils/JointImpedanceControl.h

@@ -0,0 +1,51 @@
+#ifndef CURAN_GENERIC_STATE_DERIVATIVE_
+#define CURAN_GENERIC_STATE_DERIVATIVE_
+
+#include <Eigen/Dense>
+#include <tuple>
+
+namespace curan {
+namespace robotic {
+
+template<size_t filter_size>
+struct LowPassDerivativeFilter{
+    Eigen::Matrix<double,filter_size,3> raw_state;
+    Eigen::Matrix<double,filter_size,3> filtered_state;
+    Eigen::Matrix<double,1,5> coefficient;
+
+    bool is_first = true;
+
+    LowPassDerivativeFilter(double nyquist_frequency,double cutoff){
+        double c = std::cos(0.5*cutoff/nyquist_frequency)/std::sin(0.5*cutoff/nyquist_frequency);
+        coefficient = Eigen::Matrix<double,1,5>::Zero();
+        double a0 = c*c*2+1.4142*c+1;
+        coefficient << 1/a0 , 2/a0 , 1/a0 , (2-2*c*c)/a0 , (c*c-1.4142*c+1)/a0; 
+    }
+
+    Eigen::Matrix<double,filter_size,1> update(Eigen::Matrix<double,filter_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);
+        } else {
+            raw_state.col(2) = in_raw_state;
+        }     
+        filtered_state.col(2) = coefficient[0] * raw_state.col(2)+
+                                coefficient[1] * raw_state.col(1)+
+                                coefficient[2] * raw_state.col(0)+
+                                coefficient[3] * filtered_state.col(1)+
+                                coefficient[4] * 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);
+        }
+        return filtered_state.col(2);
+    }
+};
+
+
+}
+}
+
+#endif

libraries/robotutils/include/robotutils/LowPassDerivativeFilter.h

@@ -3,7 +3,7 @@
 
 #include "LBRController.h"
 #include "RippleFilter.h"
-#include "GenericStateDerivative.h"
+#include "LowPassDerivativeFilter.h"
 #include <array>
 
 namespace curan {

libraries/robotutils/include/robotutils/ViewFiltering.h

@@ -0,0 +1,24 @@
+#include "robotutils/InertiaAwareRippleFilter.h"
+
+#include <cmath>
+
+namespace curan{
+namespace robotic{
+namespace massripple{
+
+void shift_filter_data(Data& data){
+    data.y[0] = data.y[1];
+    data.y[1] = data.y[2];
+
+    data.y_f[0] = data.y_f[1];
+    data.y_f[1] = data.y_f[2];
+    data.y_f[2] = 0.0;
+}
+
+void update_filter_data(Data& data, const double& torque){
+    data.y[2] = torque;
+}
+
+}
+}
+}

libraries/robotutils/src/InertiaAwareRippleFilter.cpp

@@ -0,0 +1,39 @@
+#include "robotutils/JointImpedanceController.h"
+
+namespace curan {
+namespace robotic {
+
+    JointImpedanceController::JointImpedanceController(){
+    }
+
+    EigenState&& JointImpedanceController::update(const RobotModel<number_of_joints>& iiwa, EigenState&& state, Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>& composed_task_jacobians){
+        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
+        */
+        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();
+
+
+        /*
+        The Java controller has two values which it reads, namely: 
+        1) commanded_joint_position 
+        2) commanded_torque 
+        The torque is computed in the previous line, but the position can remain empty. One problem with this approach is that if the deviation
+        between the reference position and the commanded position is larger than 5 degrees the robot triggers a safety stop 1. To avoid this
+        the first solution is to set the commanded position to be equal to the current position. This approach has a drawback where the error between
+        both commanded and current position is always zero, which results in the friction compensator being "shut off". We avoid this problem
+        by adding a small perturbation to the reference position with a relative high frequency. 
+        */
+        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));
+
+        currentTime += iiwa.sample_time();
+        return std::move(state);
+    }
+
+}
+}