Restructure common folder

fr3_impedance_controller_sim/CMakeLists.txt

-################################################################################
-## Set minimum required version of cmake, project name and compile options
-################################################################################
+# --- Set minimum required version of cmake, project name and compile options
 cmake_minimum_required(VERSION 3.10)
 project(fr3_impedance_controller_sim)
 
-# Set C++ standard
+# --- Set C++ standard
 set(CMAKE_BUILD_TYPE Release)
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED True)
 
-################################################################################
-## Find required CMake packages : pinocchio, casadi, catkin, ROS-related ...
-################################################################################
+# --- Find required CMake packages : pinocchio, casadi, catkin, ROS-related ...
 find_package(pinocchio REQUIRED)
 find_package(casadi REQUIRED)
 find_package(catkin REQUIRED COMPONENTS
@@ -23,29 +19,23 @@ find_package(catkin REQUIRED COMPONENTS
   geometry_msgs
 )
 
-################################################################################
-## Find source and header files to create executable binary file
-################################################################################
+# --- Include sources and headers to create executable binary file
 file(GLOB_RECURSE SOURCES
   ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp
-  ${CMAKE_CURRENT_SOURCE_DIR}/../common/controller/src/*.cpp
-  ${CMAKE_CURRENT_SOURCE_DIR}/../common/utilities/src/*.cpp
-  ${CMAKE_CURRENT_SOURCE_DIR}/../common/robot/src/*.cpp
-  ${CMAKE_CURRENT_SOURCE_DIR}/../common/math/src/*.cpp
+  ${CMAKE_CURRENT_SOURCE_DIR}/../common/src/*.cpp
 )
 
 include_directories(
-  ${CMAKE_CURRENT_SOURCE_DIR}/include/fr3_impedance_controller_sim # ! check this out
-  ${CMAKE_CURRENT_SOURCE_DIR}/../common/controller/include
-  ${CMAKE_CURRENT_SOURCE_DIR}/../common/utilities/include
-  ${CMAKE_CURRENT_SOURCE_DIR}/../common/robot/include
-  ${CMAKE_CURRENT_SOURCE_DIR}/../common/math/include
+  ${CMAKE_CURRENT_SOURCE_DIR}/include/fr3_impedance_controller_sim
+  ${CMAKE_CURRENT_SOURCE_DIR}/../common/include/controller
+  ${CMAKE_CURRENT_SOURCE_DIR}/../common/include/math
+  ${CMAKE_CURRENT_SOURCE_DIR}/../common/include/motion
+  ${CMAKE_CURRENT_SOURCE_DIR}/../common/include/robot
+  ${CMAKE_CURRENT_SOURCE_DIR}/../common/include/utilities
   ${catkin_INCLUDE_DIRS}
 )
 
-################################################################################
-## Define executable, Add source files & Link libraries
-################################################################################
+# --- Define executable, Add source files & Link libraries
 add_executable(${PROJECT_NAME}_node ${SOURCES})
 
 target_link_libraries(${PROJECT_NAME}_node

fr3_impedance_controller_sim/src/fr3_impedance_controller_sim.cpp

@@ -49,8 +49,8 @@ FR3ImpedanceControllerSim::FR3ImpedanceControllerSim(ros::NodeHandle & nh, const
   ee_quat_world_des_.coeffs() << 0.690574, 0.723058, -0.000361, 0.017139;
 
   /* Tuning parameters  */
-  const double k_amp = 1.0;  // PD gain ratio to real HW gain
-  K_task_.setIdentity().diagonal() << 200, 200, 200, 100, 100, 100;  // real HW value
+  const double k_amp = 1.0;                                       // PD gain ratio to real HW gain
+  K_task_.setIdentity().diagonal() << 200, 200, 200, 10, 10, 10;  // real HW value
   K_task_.diagonal() *= k_amp;
   B_task_.setIdentity().diagonal() = 2.0 * K_task_.diagonal().array().sqrt();