Commit changes to robot API

Author: Joaopmoliveira

CMakeLists.txt

@@ -2,13 +2,18 @@ cmake_minimum_required(VERSION 3.24)
 cmake_policy(SET CMP0091 NEW)
 cmake_policy(SET CMP0075 NEW)
 
+#Language definitions used throught the entire project
+set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_CXX_STANDARD_REQUIRED YES)
+
+set(VCPKG_OVERLAY_TRIPLETS "${CMAKE_CURRENT_LIST_DIR}/vcpkg_custom_triplet/")
+
 if(UNIX)
-  string(VCPKG_TARGET_TRIPLET "x64-linux")
-  string(VCPKG_HOST_TRIPLET "x64-linux")
+  set(VCPKG_TARGET_TRIPLET "x64-linux-mixed")
+  set(VCPKG_HOST_TRIPLET "x64-linux-mixed")
 endif(UNIX)
 
 if(WIN32)
-  set(VCPKG_OVERLAY_TRIPLETS "${CMAKE_CURRENT_LIST_DIR}/vcpkg_custom_triplet/")
   set(VCPKG_TARGET_TRIPLET "x64-windows-mixed")
   set(VCPKG_HOST_TRIPLET "x64-windows-mixed")
 endif(WIN32)
@@ -17,9 +22,7 @@ set(VCPKG_ROOT "${CMAKE_CURRENT_LIST_DIR}/vcpkg")
 set(CMAKE_TOOLCHAIN_FILE "${VCPKG_ROOT}/scripts/buildsystems/vcpkg.cmake")
 
 
-#Language definitions used throught the entire project
-set(CMAKE_CXX_STANDARD 20)
-set(CMAKE_CXX_STANDARD_REQUIRED YES)
+
 
 project(Curan)
 
@@ -44,19 +47,12 @@ 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(glfw3 3.3 REQUIRED)
-find_package(Boost COMPONENTS filesystem)
 find_package(Vulkan REQUIRED)
 find_package(vsg CONFIG REQUIRED)
 find_package(vsgXchange CONFIG REQUIRED)
 find_package(vsgImGui CONFIG REQUIRED)
-find_package(pugixml CONFIG REQUIRED)
-find_package(PCL CONFIG REQUIRED)
-find_package(GTest CONFIG REQUIRED)
-find_package(rbdl CONFIG REQUIRED)
+find_package(RBDL CONFIG REQUIRED)
 #find_package(Torch CONFIG REQUIRED)
 #find_package(OpenMP REQUIRED)
 #find_package(LAPACK REQUIRED)
@@ -102,16 +98,16 @@ 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
 
 # Add medical applications
-add_subdirectory(unittests)
+#add_subdirectory(unittests)
 
 # Add tests for submodules of curan, i.e. utilities, communication, etc...
 add_subdirectory(tests)
 
 # Add medical applications
-add_subdirectory(tutorials)
+#add_subdirectory(tutorials)

README.md

@@ -206,6 +206,17 @@ When using VScode to compile the project, if the previous order of the build ins
 the build fails due to incorrect configurations. When you find yourself faced with these problems the simplest solution is to delete the 
 .vscode folder and the build folder and reconfigure the project, this usually solves all the problems. 
 
+# Ninja 
+According to this [link](https://public.kitware.com/Bug/view.php?id=13910) on linux sometimes cmake fails to execute. 
+```bash
+sudo chmod o+r /your/path/to/ninja
+```
+
+to find the path to ninja you can run on your terminal
+``` bash
+which ninja
+```
+
 ## Acknowledgments 
 The volume reconstruction code is essencially a copy of [IGSIO](https://github.com/IGSIO/IGSIO) modified to work with the interal classes and structures of Curan. 
 

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/CMakeLists.txt

@@ -7,13 +7,14 @@ src/JointVelocityController.cpp
 src/RobotState.cpp
 src/ImpedanceController.cpp
 src/CartesianVelocityController.cpp
+src/RippleFilter.cpp
+src/ViewFiltering.cpp
 )
 
-message("MARK--------- RBDL string ${RBDL_LIBRARY}")
-
 target_link_libraries(robotutils 
 PUBLIC
 ${RBDL_LIBRARY}
+utils
 Eigen3::Eigen
 FRILibrary
 gaussianmixture

libraries/robotutils/include/robotutils/GenericStateDerivative.h

@@ -0,0 +1,61 @@
+#ifndef CURAN_GENERIC_STATE_DERIVATIVE_
+#define CURAN_GENERIC_STATE_DERIVATIVE_
+
+#include <Eigen/Dense>
+#include <tuple>
+
+namespace curan {
+namespace robotic {
+
+template<size_t state_size>
+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, 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){
+        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) = 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)+
+                                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)};
+    }
+};
+
+
+}
+}
+
+#endif

libraries/robotutils/include/robotutils/RippleFilter.h

@@ -1,20 +1,20 @@
-#ifndef CURAN_RIPPLE_FILTER
-#define CURAN_RIPPLE_FILTER
+#ifndef CURAN_RIPPLE_FILTER_
+#define CURAN_RIPPLE_FILTER_
 
+#include <tuple>
 #include <array>
 
 namespace curan {
 namespace robotic {
 namespace ripple{
-namespace detail{
-
         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{0.0}, width{in_width}, frequency{in_frequency}, delta{0.0}  {};
+            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}  {};
 
         };
 
@@ -34,10 +34,14 @@ namespace detail{
 
         void update_filter_data(Data& data, const double& torque);
 
-        double execute(const Properties& props, Data& data);
-} // namespace detail
-
+        /*
+        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
+        */
+        std::tuple<double,double> execute(const Properties& props, Data& data);
 
 }
 }

libraries/robotutils/include/robotutils/ViewFiltering.h

@@ -0,0 +1,32 @@
+#ifndef CURAN_VIEW_FILTERING_
+#define CURAN_VIEW_FILTERING_
+
+#include "LBRController.h"
+#include "RippleFilter.h"
+#include "GenericStateDerivative.h"
+#include <array>
+
+namespace curan {
+namespace robotic {
+
+struct ViewFiltering : public UserData{
+    std::array<ripple::Data, number_of_joints> joint_data_first_harmonic;
+    std::array<ripple::Data, number_of_joints> joint_data_second_harmonic;
+    std::array<ripple::Data, number_of_joints> joint_data_third_harmonic;
+    std::array<ripple::Properties, number_of_joints> first_harmonic;
+    std::array<ripple::Properties, number_of_joints> second_harmonic;
+    std::array<ripple::Properties, number_of_joints> third_harmonic;
+
+    LowPassDerivativeFilter<number_of_joints> filtering_mechanism;
+    Eigen::Matrix<double,number_of_joints,1> previous_q;
+
+    bool is_first_loop = true;
+    ViewFiltering();
+    virtual ~ViewFiltering(){};
+    EigenState&& update(const RobotModel<number_of_joints>& iiwa, EigenState&& state, Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>& composed_task_jacobians) override;
+};
+
+}
+}
+
+#endif

libraries/robotutils/src/RippleFilter.cpp

@@ -5,11 +5,11 @@
 namespace curan{
 namespace robotic{
 namespace ripple{
-namespace detail{
 
 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);
 }
@@ -27,12 +27,13 @@ void update_filter_data(Data& data, const double& torque){
     data.y[2] = torque;
 }
 
-double execute(const Properties& props, Data& data){
+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;
-    data.y_f[2] = (props.damper/(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] );
-    return data.y_f[2];
-}
+    const double damper_derivative = (props.damper-props.damper_prev)/sample_time;
+    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};
 }
 
 }

libraries/robotutils/src/ViewFiltering.cpp

@@ -0,0 +1,73 @@
+#include "robotutils/ViewFiltering.h"
+
+namespace curan {
+namespace robotic {
+
+    ViewFiltering::ViewFiltering(){
+        for (size_t filter_index = 0; filter_index < number_of_joints; ++filter_index)
+        {
+            first_harmonic[filter_index].frequency = (filter_index == 4) ? 320.0 : 320.0;
+            second_harmonic[filter_index].frequency = (filter_index == 4) ? 640.0 : 640.0;
+            third_harmonic[filter_index].frequency = (filter_index == 4) ? 1280.0 : 1280.0;
+        }
+    }
+
+    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();
+        vector_type raw_filtered_torque = vector_type::Zero();
+        vector_type partial_derivative_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;
+            }
+        }
+        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]);
+            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]);
+            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;
+            filtered_torque_value -= filtered_harm_3;
+            raw_filtered_torque[j] = -filtered_torque_value - iiwa.gravity()[j] - iiwa.coriolis()[j];
+        }
+
+        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));
+
+        currentTime += iiwa.sample_time();
+        return std::move(state);
+    }
+
+}
+}