Gamma support

CMakeLists.txt

@@ -8,30 +8,93 @@ find_package(catkin REQUIRED COMPONENTS
   message_generation 
   std_msgs
   geometry_msgs 
+  controller_interface
+  controller_manager
+  hardware_interface
+  force_torque_sensor_controller
+  filters
+  gazebo_ros
+  #gazebo_plugins
 )
 
-catkin_package()
+catkin_package(
+INCLUDE_DIRS include
+  LIBRARIES force_torque_sensor_sim_ros_control
+  CATKIN_DEPENDS roscpp std_msgs hardware_interface controller_manager 
+  DEPENDS system_lib)
 
 
 include_directories(include 
+  include
   ${LIBXML2_INCLUDE_DIR}
   ${catkin_INCLUDE_DIRS}
 )
 
+
 add_library(FTSensor
   src/FTSensor.cpp
-  )
+)
+
+add_library(force_torque_sensor_sim_ros_control
+  src/force_torque_sensor_sim_ros_control.cpp
+)
+
+add_library(force_torque_sensor_filter
+  src/trans_function_filter.cpp
+)
+add_library(imu_sensor_filter
+  src/imu_trans_function_filter.cpp
+)
+
 
 add_executable(ft_sensor_hw 
   src/ft_sensor_hw.cpp
 )
 
+add_executable(force_torque_sensor_sim_ros_control_node src/force_torque_sensor_sim_ros_control.cpp)
+add_executable(force_torque_sensor_filter_node src/trans_function_filter.cpp)
+add_executable(imu_sensor_filter_node src/imu_trans_function_filter.cpp)
+
 add_dependencies(ft_sensor_hw 
   FTSensor
 )
 
+add_dependencies(force_torque_sensor_sim_ros_control_node 
+  force_torque_sensor_sim_ros_control
+)
+
+add_dependencies(force_torque_sensor_filter_node 
+  force_torque_sensor_filter
+)
+
+add_dependencies(imu_sensor_filter_node 
+  imu_sensor_filter
+)
+
 target_link_libraries(ft_sensor_hw
   FTSensor
   ${LIBXML2_LIBRARIES}
   ${catkin_LIBRARIES}
-)
+)
+
+
+
+target_link_libraries(force_torque_sensor_sim_ros_control_node
+  ${catkin_LIBRARIES}
+)
+target_link_libraries(force_torque_sensor_filter_node
+  ${catkin_LIBRARIES}
+)
+target_link_libraries(imu_sensor_filter_node
+  ${catkin_LIBRARIES}
+)
+
+
+add_dependencies(force_torque_sensor_sim_ros_control_node ${catkin_EXPORTED_TARGETS})
+add_dependencies(force_torque_sensor_filter_node  ${catkin_EXPORTED_TARGETS})
+add_dependencies(imu_sensor_filter_node  ${catkin_EXPORTED_TARGETS})
+
+
+
+add_library(gazebo_ros_imu_sensor src/gazebo_ros_imu_sensor.cpp)
+target_link_libraries(gazebo_ros_imu_sensor ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES})

README.md

@@ -3,7 +3,31 @@ force-torque-sensor
 
 A catkin package to read measurements from any ATI FT sensor via XML
 
-ToDO:
- - [ ] Use force-torque-sensor controller from ros_control
+Done:
+ - [x] Use force-torque-sensor controller from ros_control
  - [x] Create an URDF of the sensor to visualize the forces and frames
- - [ ] Create plugin to be used in simulation
+ - [x] Create plugin to be used in simulation
+ - [x] Merge nano17 and gamma sensor 
+
+Use in simulation:
+
+1) on first terminal for nano17 ft sensor type:
+
+roslaunch force_torque_sensor ft_simulation_display.launch use_gamma:=false 
+
+or for gamma ft sensor type:
+
+roslaunch force_torque_sensor ft_simulation_display.launch 
+
+
+2) open a second terminal and type:
+
+rqt
+
+and add to rqt_plot the topic my_sensor/ft_sensor_topic/wrench/force my_sensor/ft_sensor_topic/wrench/torque
+
+3) open a third terminal and type
+
+roslaunch force_torque_sensor cylinder_spawner.launch
+
+A matlab esample of tuning the filter coefficient has been presented in matlab folder.

config/LowPass_Filter.yaml

@@ -0,0 +1,13 @@
+LowPass:
+  name: "LowPass"
+  type: "MultiChannelTransferFunctionFilterDouble"
+  params:
+   # param for PassbandFrequency 10 Hz
+   #a: [ 1.0, -1.6217, 0.68383]
+   #b: [ 0.015524, 0.031047, 0.015524]
+  # param for PassbandFrequency 5 Hz
+   a: [ 1.0, -1.8091, 0.82619]
+   b: [ 0.004266, 0.0085319, 0.004266]  
+   # param for PassbandFrequency 1 Hz
+   #a: [1.0, -1.34, 0.50912]
+   #b: [0.042291, 0.084582, 0.042291]

config/force_torque_sensor_controller.yaml

@@ -0,0 +1,5 @@
+force_torque_sensor_controller:
+  type: force_torque_sensor_controller/ForceTorqueSensorController
+  publish_rate: 1000
+
+# Settings for ros_control hardware interface   

config/ft_sensor_params.yaml

@@ -0,0 +1,6 @@
+force_torque_sensor_controller_params:
+  ft_sensor_name: "my_sensor"
+  ft_sensor_frame_id: "my_sensor_measure"
+  ft_sensor_topic: "/my_sensor/my_sensor/ft_sensor_topic"
+
+

config/hardware_interface.yaml

@@ -0,0 +1,12 @@
+# make the namespace match the name you use to instantiate the sensor in the xacro
+#my_sensor:
+#ForceTorqueSensorInterface:
+  #type: hardware_interface/ForceTorqueSensorInterface
+  #type: hardware_interface/ForceTorqueSensorHandle
+#hardware_interface:
+# type: hardware_interface/ForceTorqueSensorInterface 
+ root: my_sensor_base
+ tip:  my_sensor_measure
+ joints:
+  - my_sensor_measure_joint
+

config/imu_sensor_params.yaml

@@ -0,0 +1,4 @@
+imu_sensor_params:
+  imu_sensor_name: "my_sensor"
+  imu_sensor_frame_id: "my_sensor_imu_measure"
+  imu_sensor_topic: "/my_sensor/my_sensor/imu"

config/joint_names.yaml

@@ -0,0 +1,2 @@
+joints:
+  - my_sensor_measure_joint

ft_sensor.rviz

@@ -0,0 +1,178 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /TF1
+        - /WrenchStamped1
+        - /WrenchStamped1/Status1
+        - /RobotModel1
+      Splitter Ratio: 0.5
+    Tree Height: 595
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        my_sensor_base:
+          Value: true
+        my_sensor_measure:
+          Value: true
+        world:
+          Value: true
+      Marker Scale: 0.02
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        world:
+          my_sensor_base:
+            my_sensor_measure:
+              {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Arrow Width: 0.5
+      Class: rviz/WrenchStamped
+      Enabled: true
+      Force Arrow Scale: 2
+      Force Color: 204; 51; 51
+      History Length: 1
+      Name: WrenchStamped
+      Topic: /my_sensor/sensor_measurements
+      Torque Arrow Scale: 2
+      Torque Color: 204; 204; 51
+      Value: true
+    - Alpha: 0.5
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        my_sensor_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        my_sensor_measure:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: my_sensor_measure
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 0.102991
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.145398
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.650398
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 876
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000013c000002e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002e2000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002e2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000003efc0100000002fb0000000800540069006d00650100000000000005ff000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003a8000002e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1535
+  X: 55
+  Y: 24