-remove old ft_sensor_controller

-implement new ft_sensor_calib_controller
-add ft_calib_{data, meas, poses, q}.yaml required by ft_sensor_calib_controller and load them in single_lwr.launch
-add ft_calib in CMakeLists.txt and package.xml required by ft_sensor_calib_controller
-add fields required by ft_sensor_calib_controller in controllers.yaml
-use /lwr/ft_sensor topic name in cartesian_position_controller

Author: xEnVrE

CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
   lwr_controllers
   roscpp
   kdljacdot
+  ft_calib
 )
 
 add_definitions (-fpermissive -std=c++11)
@@ -46,10 +47,10 @@ generate_messages(
   lwr_controllers
 )
 
-
 catkin_package(
   CATKIN_DEPENDS
     kdljacdot
+    # ft_calib
     controller_interface
     lwr_controllers
     kdl_conversions
@@ -59,7 +60,7 @@ catkin_package(
 )
 
 add_library(${PROJECT_NAME}
-  src/ft_sensor_controller.cpp
+  src/ft_sensor_calib_controller.cpp
   src/cartesian_position_controller.cpp	
   src/cartesian_inverse_dynamics_controller.cpp	
   src/hybrid_impedance_controller.cpp
@@ -69,7 +70,6 @@ add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp)
 
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} yaml-cpp)
 
-
 install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
 

config/controllers.yaml

@@ -28,16 +28,14 @@ lwr:
       - lwr_a6_joint_stiffness
 
 ## OTHER CUSTOM CONTROLLERS LEFT HERE AS EXAMPLES
-  # FtSensor Controller
-  ft_sensor_controller:
-    type: lwr_force_position_controllers/FtSensorController
+  # FtSensorCalib Controller
+  ft_sensor_calib_controller:
+    type: lwr_force_position_controllers/FtSensorCalibController
     root_name: vito_anchor
     tip_name: lwr_7_link
     topic_name: /lwr/ft_sensor
-    ft_sensor_bias: [0, 0, 0, 0, 0, 0]
-    base_tool_weight_com: [0, 0, 0, 0, 0, 0]
-    p_wrist_toolcom: [0, 0, 0.1]
-    publish_rate: 200 #Hz
+    calibration_loop_rate: 100.0
+    recover_existing_data: false
 
   # Cartesian Position Controller
   cartesian_position_controller:

config/ft_calib_data.yaml

@@ -0,0 +1,16 @@
+gripper_com_pose:
+  - 0.002621568387731212
+  - -0.0003515309468810598
+  - 0.004137037404081813
+  - 0
+  - 0
+  - 0
+gripper_mass: 6.509143994937928e-07
+bias:
+  - 1.584460183819159e-08
+  - -5.707289241210769e-06
+  - 1.498097625527359e-06
+  - 3.64819715407862e-08
+  - -2.265979235962194e-08
+  - 2.964348029762117e-09
+gripper_com_frame_id: lwr_7_link

config/ft_calib_meas.yaml

@@ -0,0 +1,54 @@
+calib_meas_number: 4
+calib_meas:
+  pose1:
+    force_avg:
+      - 0.1521934294330592
+      - 0.04844507382844252
+      - 0.006304600412419553
+    torque_avg:
+      - -5.635950990290948e-05
+      - 0.0006962514245404868
+      - 8.068064758058578e-10
+    gravity:
+      - -9.806649064205548
+      - -0.001074826044471037
+      - 0.004147139431339473
+  pose3:
+    gravity:
+      - -3.819371657673907
+      - 0.01484989664177515
+      - -9.032306673470906
+    torque_avg:
+      - 0.0005761121246524151
+      - -0.0009301992653528642
+      - 1.174364091457587e-09
+    force_avg:
+      - -0.07540153437470619
+      - 0.1406359873112517
+      - 0.06706553139931667
+  pose2:
+    force_avg:
+      - -0.05367795006809145
+      - 0.1390910760892196
+      - 0.1147201923215103
+    gravity:
+      - -7.036608223269595
+      - 0.01092343614638948
+      - -6.830549729945423
+    torque_avg:
+      - 0.001152721264360326
+      - -0.0009670574213896801
+      - -4.417889570482863e-08
+  pose0:
+    gravity:
+      - -7.212658566379069
+      - -5.208985211634143
+      - -4.124853172212982
+    force_avg:
+      - 0.07576230852691779
+      - 0.09179926084696172
+      - 0.05619690902839117
+    torque_avg:
+      - -1.350657547028525e-05
+      - -0.001701637664215409
+      - -1.980222698922983e-08

config/ft_calib_poses.yaml

@@ -0,0 +1,39 @@
+lwr:
+  ft_sensor_calib_controller:
+    calib_number_poses: 34
+    calib_poses:
+      pose0: [-0.9, 0.3, 0.3, 1.571, 1.571, 3.130]
+      pose1: [-0.9, 0.3, 0.3, 1.571, 0.8, 3.140]
+      pose2: [-0.9, 0.3, 0.3, 1.571, 0.4, 3.140]
+      pose3: [-0.9, 0.3, 0.3, 1.571, 0.0, 3.140]
+      pose4: [-0.9, 0.3, 0.3, 1.571, -0.4, 3.140]
+      pose5: [-0.9, 0.3, 0.3, 1.571, -0.8, 3.140]
+      pose6: [-0.9, 0.3, 0.3, 1.571, -1.571, 3.140]
+      pose7: [-0.9, 0.3, 0.3, 2.8, 0.0, 3.140]
+      pose8: [-0.9, 0.3, 0.3, 2.3, 0.0, 3.140]
+      pose9: [-0.9, 0.3, 0.3, 2.0, 0.0, 3.140]
+      pose10: [-0.9, 0.3, 0.3, 1.1, 0.0, 3.140]
+      pose11: [-0.9, 0.3, 0.3, 0.8, 0.0, 3.140]
+      pose12: [-0.9, 0.3, 0.3, 0.4, 0.0, 3.140]
+      pose13: [-0.9, 0.3, 0.3, 0.01, 0.0, 3.140]
+      pose14: [-0.9, 0.3, 0.3, 3.093, -0.012, -2.467]
+      pose15: [-0.9, 0.3, 0.3, 2.843, -0.012, -2.467]
+      pose16: [-0.9, 0.3, 0.3, 2.372, -0.012, -2.467]
+      pose17: [-0.9, 0.3, 0.3, 2.0, -0.012, -2.467]
+      pose18: [-0.9, 0.3, 0.3, 1.6, -0.012, -2.467]
+      pose19: [-0.9, 0.3, 0.3, 1.2, -0.012, -2.467]
+      pose20: [-0.9, 0.3, 0.3, 0.8, -0.012, -2.467]
+      pose21: [-0.9, 0.3, 0.3, 0.4, -0.012, -2.467]
+      pose22: [-0.9, 0.3, 0.3, 0.0, -0.012, -2.467]
+      pose23: [-0.9, 0.3, 0.3, 2.399, -0.568, 1.058]
+      pose24: [-0.9, 0.3, 0.3, 2.083, -0.858, 1.531]
+      pose25: [-0.9, 0.3, 0.3, 1.480, -0.962, 2.280]
+      pose26: [-0.9, 0.3, 0.3, 0.890, -0.948, 3.043]
+      pose27: [-0.9, 0.3, 0.3, 0.677, -0.429, -2.865]
+      pose28: [-0.9, 0.3, 0.3, 0.607, -0.049, -2.576]
+      pose29: [-0.9, 0.3, 0.3, 0.621, 0.207, -2.396]
+      pose30: [-0.9, 0.3, 0.3, 2.252, 0.384, -2.121]
+      pose31: [-0.9, 0.3, 0.3, 2.258, -0.402, -2.726]
+      pose32: [-0.9, 0.3, 0.3, 2.425, -0.685, -3.043]
+      pose33: [-0.9, 0.3, 0.3, 2.785, -0.898, 2.742]
+      pose34: [-0.9, 0.3, 0.3, -2.869, -0.919, 1.958]