Merge pull request #126 in SWDEV/franka_ros from SRR-1037 to franka-g…

…azebo * commit 'ef756fb937df84ea8a0d1df305d01124bf93bd38': (35 commits) FIX: Remove TODO CHANGE: Use default controller configs too avoid code duplication ADD: Default case if joint type is unknown ADD: Example Launch file to franka_gazebo ADD: TODO regarding NE/EE/K Frames FIX: Conditional Build for older Gazebo Versions (kinetic) CHANGE: Lookup equilibrium_pose topic relatively ADD: Lint & Format franka_gazebo FIX: Insert default case for `segment` CHANGE: Lookup contact & collision thresholds similar to franka_hw FIX: Use `eigen_conversions` to convert KDL::Frame -> Eigen::Affine3d FIX: Remove TODOs ADD: Gravity Tests for franka_gazebo::ModelKDL CHANGE: Move math util functions as static functions in header CHANGE: Split initialization of FrankaHWSim into sub-methods ADD: Dependencies to Docker FIX: Review Comments FIX: Remove outdated non-template methods FIX: Naming for private member of ModelKDL::chain CHANGE: Move util methods inside class namespace ...
frankaemika · May 27, 2021 · b4a4f91 · b4a4f91
2 parents 3483dba + ef756fb
commit b4a4f91
Show file tree

Hide file tree

Showing 27 changed files with 2,607 additions and 17 deletions.
diff --git a/.ci/Dockerfile.kinetic b/.ci/Dockerfile.kinetic
@@ -4,4 +4,8 @@ RUN apt-get update && apt-get install -y \
     clang-format-6.0 \
     clang-tidy-6.0 \
     ros-kinetic-ros-control \
+    ros-kinetic-eigen-conversions \
+    ros-kinetic-gazebo-dev \
+    ros-kinetic-gazebo-ros-control \
+    ros-kinetic-orocos-kdl \
     && rm -rf /var/lib/apt/lists/*
diff --git a/.ci/Dockerfile.melodic b/.ci/Dockerfile.melodic
@@ -4,4 +4,8 @@ RUN apt-get update && apt-get install -y \
     clang-format-6.0 \
     clang-tidy-6.0 \
     ros-melodic-ros-control \
+    ros-melodic-eigen-conversions \
+    ros-melodic-gazebo-dev \
+    ros-melodic-gazebo-ros-control \
+    ros-melodic-orocos-kdl \
     && rm -rf /var/lib/apt/lists/*
diff --git a/.ci/Dockerfile.noetic b/.ci/Dockerfile.noetic
@@ -3,5 +3,9 @@ RUN apt-get update && apt-get install -y \
     clang-6.0 \
     clang-format-6.0 \
     clang-tidy-6.0 \
+    liborocos-kdl-dev \
     ros-noetic-ros-control \
+    ros-noetic-eigen-conversions \
+    ros-noetic-gazebo-dev \
+    ros-noetic-gazebo-ros-control \
     && rm -rf /var/lib/apt/lists/*
diff --git a/franka_example_controllers/launch/cartesian_impedance_example_controller.launch b/franka_example_controllers/launch/cartesian_impedance_example_controller.launch
@@ -12,6 +12,7 @@
   <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/rviz/franka_description_with_marker.rviz"/>
   <node name="interactive_marker" pkg="franka_example_controllers" type="interactive_marker.py" required="true" output="screen">
     <param name="link_name" value="panda_link0" />
+    <remap from="equilibrium_pose" to="/cartesian_impedance_example_controller/equilibrium_pose" />
   </node>
   <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
 </launch>
diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
@@ -20,7 +20,7 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo
   std::vector<double> cartesian_damping_vector;
 
   sub_equilibrium_pose_ = node_handle.subscribe(
-      "/equilibrium_pose", 20, &CartesianImpedanceExampleController::equilibriumPoseCallback, this,
+      "equilibrium_pose", 20, &CartesianImpedanceExampleController::equilibriumPoseCallback, this,
       ros::TransportHints().reliable().tcpNoDelay());
 
   std::string arm_id;

diff --git a/franka_gazebo/CMakeLists.txt b/franka_gazebo/CMakeLists.txt
@@ -0,0 +1,111 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(franka_gazebo)
+
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+# Load catkin and all dependencies required for this package
+find_package(catkin REQUIRED COMPONENTS
+  gazebo_dev
+  roscpp
+  std_msgs
+  gazebo_ros_control
+  kdl_parser
+  control_toolbox
+  controller_manager
+  hardware_interface
+  transmission_interface
+  pluginlib
+  joint_limits_interface
+  eigen_conversions
+  urdf
+  angles
+  franka_hw
+)
+
+find_package(Franka 0.7.0 REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(orocos_kdl REQUIRED)
+
+catkin_package(
+  CATKIN_DEPENDS
+    roscpp
+    std_msgs
+    gazebo_ros_control
+    kdl_parser
+    controller_manager
+    control_toolbox
+    pluginlib
+    hardware_interface
+    transmission_interface
+    joint_limits_interface
+    eigen_conversions
+    urdf
+    angles
+    franka_hw
+  INCLUDE_DIRS include
+  LIBRARIES franka_hw_sim
+  DEPENDS Franka orocos_kdl
+)
+
+link_directories(
+  ${catkin_LIBRARY_DIRS}
+)
+
+include_directories(include
+  ${Boost_INCLUDE_DIR}
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Libraries
+add_library(franka_hw_sim
+  src/franka_hw_sim.cpp
+  src/joint.cpp
+  src/model_kdl.cpp
+  )
+target_link_libraries(franka_hw_sim
+  ${catkin_LIBRARIES}
+  ${Franka_LIBRARIES}
+  ${orocos_kdl_LIBRARIES}
+  )
+target_include_directories(franka_hw_sim SYSTEM PUBLIC
+  ${Franka_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+  ${orocos_kdl_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+  )
+add_dependencies(franka_hw_sim
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+  )
+
+if(CATKIN_ENABLE_TESTING)
+  add_subdirectory(test)
+endif()
+
+## Install
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+install(FILES franka_hw_sim_plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+
+## Tools
+include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL
+  RESULT_VARIABLE CLANG_TOOLS
+  )
+if(CLANG_TOOLS)
+  file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)
+  file(GLOB_RECURSE HEADERS
+    ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h
+    )
+  add_format_target(franka_gazebo FILES ${SOURCES} ${HEADERS})
+  add_tidy_target(franka_gazebo
+    FILES ${SOURCES}
+    DEPENDS franka_hw_sim
+    )
+endif()
diff --git a/franka_gazebo/config/franka_sim_description_with_marker.rviz b/franka_gazebo/config/franka_sim_description_with_marker.rviz
@@ -0,0 +1,195 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+        - /InteractiveMarkers1
+      Splitter Ratio: 0.5
+    Tree Height: 724
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        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
+    - Alpha: 1
+      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
+        panda_link0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link8:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Name: RobotModel
+      Robot Description: /panda/robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /panda/equilibrium_pose_marker/update
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: panda_link0
+    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
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - 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: 3.1647839546203613
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.6603980660438538
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.4603980481624603
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1025
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000035ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d00000042fc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004b80000035f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1853
+  X: 67
+  Y: 455
diff --git a/franka_gazebo/franka_hw_sim_plugin.xml b/franka_gazebo/franka_hw_sim_plugin.xml
@@ -0,0 +1,12 @@
+<library path="lib/libfranka_hw_sim">
+
+  <class
+      name="franka_gazebo/FrankaHWSim"
+      type="franka_gazebo::FrankaHWSim"
+      base_class_type="gazebo_ros_control::RobotHWSim">
+      <description>
+          A custom implementation of a gazebo_ros_control plugin, which is able to simulate franka interfaces in Gazebo.
+      </description>
+  </class>
+
+</library>