ros-industrial · tingelst · Jun 9, 2015
diff --git a/kuka_kr16_gazebo/CMakeLists.txt b/kuka_kr16_gazebo/CMakeLists.txt
@@ -0,0 +1,159 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(kuka_kr16_gazebo)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependencies might have been
+##     pulled in transitively but can be declared for certainty nonetheless:
+##     * add a build_depend tag for "message_generation"
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES kuka_kr16_gazebo
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+
+## Declare a cpp library
+# add_library(kuka_kr16_gazebo
+#   src/${PROJECT_NAME}/kuka_kr16_gazebo.cpp
+# )
+
+## Declare a cpp executable
+# add_executable(kuka_kr16_gazebo_node src/kuka_kr16_gazebo_node.cpp)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+# add_dependencies(kuka_kr16_gazebo_node kuka_kr16_gazebo_generate_messages_cpp)
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(kuka_kr16_gazebo_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY controller DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS kuka_kr16_gazebo kuka_kr16_gazebo_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_kuka_kr16_gazebo.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/kuka_kr16_gazebo/README.md b/kuka_kr16_gazebo/README.md
@@ -0,0 +1,13 @@
+kuka_kr16_gazebo
+================
+
+![gazebo screenshot](doc/kuka_kr16_gazebo_ros_control.png)
+
+## Usage
+The simulation starts up paused by default. This enables us to turn off the physics engine, this is important to get the desired behaviour from the ros-control position controller.
+
+
+To bring up the simulated robot in Gazebo, run:
+```
+roslaunch kuka_kr16_gazebo test_kr16_2_gazebo.launch
+```
diff --git a/kuka_kr16_gazebo/config/joint_state_controller.yaml b/kuka_kr16_gazebo/config/joint_state_controller.yaml
@@ -0,0 +1,3 @@
+joint_state_controller:
+    type: joint_state_controller/JointStateController
+    publish_rate: 50 
diff --git a/kuka_kr16_gazebo/config/joint_trajectory_controller.yaml b/kuka_kr16_gazebo/config/joint_trajectory_controller.yaml
@@ -0,0 +1,12 @@
+position_trajectory_controller:
+    type: "position_controllers/JointTrajectoryController"
+    joints:
+        - joint_a1
+        - joint_a2
+        - joint_a3
+        - joint_a4
+        - joint_a5
+        - joint_a6
+    state_publish_rate: 50 # Defaults to 50
+    action_monitor_rate: 20 # Defaults to 20
+
diff --git a/kuka_kr16_gazebo/doc/kuka_kr16_gazebo_ros_control.png b/kuka_kr16_gazebo/doc/kuka_kr16_gazebo_ros_control.png
diff --git a/kuka_kr16_gazebo/launch/controller_utils.launch b/kuka_kr16_gazebo/launch/controller_utils.launch
@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<launch>
+    <!-- Robot state publisher -->
+    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
+        <param name="publish_frequency" type="double" value="50.0" />
+        <param name="tf_prefix" type="string" value="" />
+    </node>
+    <!-- joint_state_controller -->
+    <rosparam file="$(find kuka_kr16_gazebo)/config/joint_state_controller.yaml" command="load"/>
+    <node name="joint_state_controller_spawner" pkg="controller_manager" type="spawner" args="joint_state_controller" />
+</launch>
+
diff --git a/kuka_kr16_gazebo/launch/load_kr16_2_gazebo.launch b/kuka_kr16_gazebo/launch/load_kr16_2_gazebo.launch
@@ -0,0 +1,4 @@
+<?xml version="1.0"?>
+<launch>
+    <param name="robot_description" command="$(find xacro)/xacro.py '$(find kuka_kr16_gazebo)/urdf/kr16_2_gazebo_robot.xacro'"/>
+</launch>
diff --git a/kuka_kr16_gazebo/launch/test_kr16_2_gazebo.launch b/kuka_kr16_gazebo/launch/test_kr16_2_gazebo.launch
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<launch>
+
+    <arg name="paused" default="true"/>
+    <!-- startup simulated world -->
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" default="worlds/empty.world"/>
+        <arg name="paused" value="$(arg paused)"/>
+    </include>
+
+    <!-- send robot urdf to param server -->
+    <include file="$(find kuka_kr16_gazebo)/launch/load_kr16_2_gazebo.launch"/>
+
+    <!-- push robot_description to factory and spawn robot in gazebo -->
+    <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -z 0.1" respawn="false" output="screen" />
+
+    <include file="$(find kuka_kr16_gazebo)/launch/controller_utils.launch"/>
+
+    <rosparam file="$(find kuka_kr16_gazebo)/config/joint_trajectory_controller.yaml" command="load"/>
+    <node name="joint_trajectory_controller_spawner" pkg="controller_manager" type="spawner" args="position_trajectory_controller" />
+</launch>
diff --git a/kuka_kr16_gazebo/package.xml b/kuka_kr16_gazebo/package.xml
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package>
+  <name>kuka_kr16_gazebo</name>
+  <version>0.0.1</version>
+  <description>
+    Gazebo wrapper for the KUKA KR16 manipulators. Currently only the KR16-2 manipulator is supported
+  </description>
+  <author>Lars Tingelstad</author>
+  <maintainer email="[email protected]">Lars Tingelstad</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/kuka_kr16_gazebo</url>
+  <url type="bugtracker">https://github.com/ros-industrial/kuka_experimental/issues</url>
+  <url type="repository">https://github.com/ros-industrial/kuka_experimental</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <run_depend>kuka_kr16_support</run_depend>
+  <run_depend>gazebo_ros_pkgs</run_depend>
+  <run_depend>gazebo_ros</run_depend>
+  <run_depend>gazebo_ros_control</run_depend>
+  <run_depend>ros_controllers</run_depend>
+  <run_depend>joint_state_controller</run_depend>
+  <run_depend>joint_trajectory_controller</run_depend>
+  <run_depend>position_controllers</run_depend>
+  <run_depend>robot_state_publisher</run_depend>
+
+  <export>
+  </export>
+</package>
diff --git a/kuka_kr16_gazebo/urdf/gazebo.xacro b/kuka_kr16_gazebo/urdf/gazebo.xacro
@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <gazebo>
+        <plugin name="ros_control" filename="libgazebo_ros_control.so">
+            <!--<robotNamespace>/</robotNamespace>-->
+            <!--<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>-->
+        </plugin>
+    </gazebo>
+</robot>
diff --git a/kuka_kr16_gazebo/urdf/kr16_2_gazebo_macro.xacro b/kuka_kr16_gazebo/urdf/kr16_2_gazebo_macro.xacro
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<!--Generates a urdf from the macro in kr16_2_macro.xacro -->
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:include filename="$(find kuka_kr16_support)/urdf/kr16_2_macro.xacro" />
+  <xacro:include filename="$(find kuka_kr16_gazebo)/urdf/kr16_2_transmission_macro.xacro" />
+  <xacro:include filename="$(find kuka_kr16_gazebo)/urdf/gazebo.xacro" />
+
+  <xacro:macro name="kuka_kr16_2_gazebo" params="prefix" >
+
+    <xacro:kuka_kr16_2 prefix=""/>
+    <xacro:kr16_2_transmission prefix=""/>
+
+    <link name="world" />
+    <joint name="world_joint" type="fixed">
+      <parent link="world" />
+      <child link = "base_link" />
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+    </joint>
+
+  </xacro:macro>
+</robot>
diff --git a/kuka_kr16_gazebo/urdf/kr16_2_gazebo_robot.xacro b/kuka_kr16_gazebo/urdf/kr16_2_gazebo_robot.xacro
@@ -0,0 +1,6 @@
+<?xml version="1.0"?>
+<!--Generates a urdf from the macro in kr16_2_macro.xacro -->
+<robot name="kuka_kr16_2_gazebo" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:include filename="$(find kuka_kr16_gazebo)/urdf/kr16_2_gazebo_macro.xacro" />
+    <xacro:kuka_kr16_2_gazebo prefix=""/>
+</robot>
diff --git a/kuka_kr16_gazebo/urdf/kr16_2_transmission_macro.xacro b/kuka_kr16_gazebo/urdf/kr16_2_transmission_macro.xacro
@@ -0,0 +1,79 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="kr16_2_transmission" params="prefix">
+
+        <!--joint_a1 transmission-->
+        <transmission name="${prefix}joint_a1_trans">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${prefix}joint_a1">
+                <hardwareInterface>PositionJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${prefix}motor_a1">
+                <hardwareInterface>PositionJointInterface</hardwareInterface>
+                <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <!--joint_a2 transmission-->
+        <transmission name="${prefix}joint_a2_trans">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${prefix}joint_a2">
+                <hardwareInterface>PositionJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${prefix}motor_a2">
+                <hardwareInterface>PositionJointInterface</hardwareInterface>
+                <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <!--joint_a3 transmission-->
+        <transmission name="${prefix}joint_a3_trans">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${prefix}joint_a3">
+                <hardwareInterface>PositionJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${prefix}motor_a3">
+                <hardwareInterface>PositionJointInterface</hardwareInterface>
+                <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <!--joint_a4 transmission-->
+        <transmission name="${prefix}joint_a4_trans">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${prefix}joint_a4">
+                <hardwareInterface>PositionJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${prefix}motor_a4">
+                <hardwareInterface>PositionJointInterface</hardwareInterface>
+                <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <!--joint_a5 transmission-->
+        <transmission name="${prefix}joint_a5_trans">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${prefix}joint_a5">
+                <hardwareInterface>PositionJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${prefix}motor_a5">
+                <hardwareInterface>PositionJointInterface</hardwareInterface>
+                <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <!--joint_a6 transmission-->
+        <transmission name="${prefix}joint_a6_trans">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${prefix}joint_a6">
+                <hardwareInterface>PositionJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${prefix}motor_a6">
+                <hardwareInterface>PositionJointInterface</hardwareInterface>
+                <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+    </xacro:macro>
+</robot>