ros-industrial · wkentaro · Jun 6, 2016 · Jun 6, 2016 · Jul 6, 2016 · Jul 7, 2016
diff --git a/fanuc_lrmate200id_gazebo/CMakeLists.txt b/fanuc_lrmate200id_gazebo/CMakeLists.txt
@@ -0,0 +1,7 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(fanuc_lrmate200id_gazebo)
+find_package(catkin REQUIRED)
+catkin_package()
+
+install(DIRECTORY config launch urdf
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/fanuc_lrmate200id_gazebo/config/lrmate200id_controllers.yaml b/fanuc_lrmate200id_gazebo/config/lrmate200id_controllers.yaml
@@ -0,0 +1,26 @@
+# Publish all joint states -----------------------------------
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50
+
+arm_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+     - joint_1
+     - joint_2
+     - joint_3
+     - joint_4
+     - joint_5
+     - joint_6
+  constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      joint_1: {trajectory: 0.1, goal: 0.1}
+      joint_2: {trajectory: 0.1, goal: 0.1}
+      joint_3: {trajectory: 0.1, goal: 0.1}
+      joint_4: {trajectory: 0.1, goal: 0.1}
+      joint_5: {trajectory: 0.1, goal: 0.1}
+      joint_6: {trajectory: 0.1, goal: 0.1}
+  stop_trajectory_duration: 0.5
+  state_publish_rate:  25
+  action_monitor_rate: 10
diff --git a/fanuc_lrmate200id_gazebo/launch/lrmate200id_control.launch b/fanuc_lrmate200id_gazebo/launch/lrmate200id_control.launch
@@ -0,0 +1,12 @@
+<launch>
+
+  <!-- Load joint controller configurations from YAML file to parameter server -->
+  <rosparam file="$(find fanuc_lrmate200id_gazebo)/config/lrmate200id_controllers.yaml" command="load"/>
+
+  <!-- load the controllers -->
+  <node name="controller_spawner"
+        pkg="controller_manager" type="spawner"
+        args="joint_state_controller arm_controller"
+        respawn="false" output="screen" />
+
+</launch>
diff --git a/fanuc_lrmate200id_gazebo/launch/lrmate200id_gazebo.launch b/fanuc_lrmate200id_gazebo/launch/lrmate200id_gazebo.launch
@@ -0,0 +1,42 @@
+<launch>
+
+  <arg name="load_robot_model" default="true" />
+
+  <!-- remap topics to conform to ROS-I specifications -->
+  <remap from="/arm_controller/follow_joint_trajectory" to="/joint_trajectory_action" />
+  <remap from="/arm_controller/state" to="/feedback_states" />
+  <remap from="/arm_controller/command" to="/joint_path_command"/>
+
+  <arg name="debug" default="false"/>
+  <arg name="gui" default="true"/>
+  <arg name="paused" default="false" />
+  <arg name="headless" default="false"/>
+  <arg name="world_name" default="worlds/empty.world" />
+
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(arg world_name)" />
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="use_sim_time" value="true"/>
+    <arg name="headless" value="$(arg headless)"/>
+  </include>
+
+  <group if="$(arg load_robot_model)">
+    <param name="robot_description"
+           command="$(find xacro)/xacro.py $(find fanuc_lrmate200id_gazebo)/urdf/lrmate200id.xacro" />
+  </group>
+
+  <node name="urdf_spawner"
+        pkg="gazebo_ros" type="spawn_model"
+        args="-urdf -model lrmate200id -param robot_description"
+        respawn="false" output="screen" />
+
+  <include file="$(find fanuc_lrmate200id_gazebo)/launch/lrmate200id_control.launch" />
+
+  <!-- convert joint states to TF transforms for rviz, etc -->
+  <node name="robot_state_publisher"
+        pkg="robot_state_publisher" type="robot_state_publisher"
+        respawn="false" output="screen" />
+
+</launch>
diff --git a/fanuc_lrmate200id_gazebo/package.xml b/fanuc_lrmate200id_gazebo/package.xml
@@ -0,0 +1,48 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>fanuc_lrmate200id_gazebo</name>
+  <version>0.1.0</version>
+  <description>
+    <p>
+      ROS-Industrial Gazebo support for the Fanuc LR Mate 200iD (and variants).
+    </p>
+    <p>
+      This package contains configuration data and launch files for Gazebo
+      for Fanuc LR Mate 200iD manipulators.
+    </p>
+  </description>
+  <author>G.A. vd. Hoorn (TU Delft Robotics Institute)</author>
+  <author email="[email protected]">Kentaro Wada</author>
+  <maintainer email="[email protected]">G.A. vd. Hoorn (TU Delft Robotics Institute)</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://wiki.ros.org/fanuc_lrmate200id_gazebo</url>
+  <url type="bugtracker">https://github.com/ros-industrial/fanuc_experimental/issues</url>
+  <url type="repository">https://github.com/ros-industrial/fanuc_experimental</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <test_depend>roslaunch</test_depend>
+
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>fanuc_lrmate200id_support</exec_depend>
+  <exec_depend>gazebo_ros</exec_depend>
+  <exec_depend>joint_trajectory_controller</exec_depend>
+  <exec_depend>joint_state_controller</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <export>
+    <architecture_independent />
+    <rosindex>
+      <tags>
+        <tag>gazebo</tag>
+        <tag>fanuc</tag>
+        <tag>industrial</tag>
+        <tag>ros-industrial</tag>
+        <tag>lrmate200id</tag>
+        <tag>experimental</tag>
+      </tags>
+    </rosindex>
+  </export>
+</package>
diff --git a/fanuc_lrmate200id_gazebo/urdf/lrmate200id.xacro b/fanuc_lrmate200id_gazebo/urdf/lrmate200id.xacro
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<robot name="fanuc_lrmate200id" xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:include filename="$(find fanuc_lrmate200id_support)/urdf/lrmate200id_macro.xacro"/>
+  <xacro:include filename="$(find fanuc_lrmate200id_gazebo)/urdf/lrmate200id_macro.xacro"/>
+  <xacro:fanuc_lrmate200id prefix=""/>
+
+  <!-- fix the base to the Gazebo world -->
+  <link name="world" />
+  <joint name="fixed" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="world"/>
+    <child link="base_link"/>
+  </joint>
+
+</robot>
diff --git a/fanuc_lrmate200id_gazebo/urdf/lrmate200id_macro.xacro b/fanuc_lrmate200id_gazebo/urdf/lrmate200id_macro.xacro
@@ -0,0 +1,106 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!-- controllers -->
+
+  <gazebo>
+    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+      <robotNamespace>/</robotNamespace>
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+    </plugin>
+  </gazebo>
+
+  <!-- links -->
+
+  <gazebo reference="base_link">
+    <material>Gazebo/Grey</material>
+  </gazebo>
+
+  <gazebo reference="link_1">
+    <material>Gazebo/Yellow</material>
+  </gazebo>
+
+  <gazebo reference="link_2">
+    <material>Gazebo/Yellow</material>
+  </gazebo>
+
+  <gazebo reference="link_3">
+    <material>Gazebo/Yellow</material>
+  </gazebo>
+
+  <gazebo reference="link_4">
+    <material>Gazebo/Yellow</material>
+  </gazebo>
+
+  <gazebo reference="link_5">
+    <material>Gazebo/Yellow</material>
+  </gazebo>
+
+  <gazebo reference="link_6">
+    <material>Gazebo/Black</material>
+  </gazebo>
+
+  <!-- joints -->
+
+  <transmission name="tran_1">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint_1">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor_1">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="tran_2">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint_2">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor_2">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="tran_3">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint_3">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor_3">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="tran_4">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint_4">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor_4">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="tran_5">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint_5">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor_5">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="tran_6">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint_6">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor_6">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+</robot>
diff --git a/fanuc_lrmate200id_gazebo_demo/CMakeLists.txt b/fanuc_lrmate200id_gazebo_demo/CMakeLists.txt
@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(fanuc_lrmate200id_gazebo_demo)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(
+  DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(roslint REQUIRED)
+  roslint_python(${PROJECT_SOURCE_DIR}/scripts)
+  roslint_add_test()
+endif()
diff --git a/fanuc_lrmate200id_gazebo_demo/launch/apc_stow.launch b/fanuc_lrmate200id_gazebo_demo/launch/apc_stow.launch
@@ -0,0 +1,31 @@
+<launch>
+
+  <env name="GAZEBO_MODEL_PATH" value="$(find fanuc_lrmate200id_gazebo_demo)/models:$(optenv GAZEBO_MODEL_PATH)" />
+
+  <param name="/move_group/trajectory_execution/execution_duration_monitoring" value="false" />
+
+  <arg name="reactive" default="true" />
+  <arg name="rviz" default="true" />
+  <arg name="gui" default="true" />
+
+  <include file="$(find fanuc_lrmate200id_gazebo)/launch/lrmate200id_gazebo.launch">
+    <arg name="load_robot_model" value="false" />
+    <arg name="world_name" value="$(find fanuc_lrmate200id_gazebo_demo)/worlds/apc_stow.world" />
+    <arg name="gui" value="$(arg gui)" />
+  </include>
+
+  <param name="robot_description"
+         command="$(find xacro)/xacro.py $(find fanuc_lrmate200id_gazebo_demo)/urdf/lrmate200id_apc_stow.xacro" />
+
+  <include file="$(find fanuc_lrmate200id_moveit_config)/launch/moveit_planning_execution_gazebo.launch">
+    <arg name="rviz" value="false" />
+  </include>
+
+  <group if="$(arg rviz)">
+    <node name="rviz"
+          pkg="rviz" type="rviz"
+          args="-d $(find fanuc_lrmate200id_gazebo_demo)/rvizconfig/apc_stow.rviz">
+    </node>
+  </group>
+
+</launch>
diff --git a/fanuc_lrmate200id_gazebo_demo/launch/touch_the_floor.launch b/fanuc_lrmate200id_gazebo_demo/launch/touch_the_floor.launch
@@ -0,0 +1,32 @@
+<launch>
+
+  <arg name="reactive" default="true" />
+  <arg name="rviz" default="true" />
+
+  <include file="$(find fanuc_lrmate200id_gazebo)/launch/lrmate200id_gazebo.launch">
+    <arg name="load_robot_model" value="false" />
+  </include>
+
+  <param name="robot_description"
+         command="$(find xacro)/xacro.py $(find fanuc_lrmate200id_gazebo_demo)/urdf/lrmate200id_ft.xacro" />
+
+  <include file="$(find fanuc_lrmate200id_moveit_config)/launch/moveit_planning_execution_gazebo.launch">
+    <arg name="rviz" value="false" />
+  </include>
+
+  <node name="touch_the_floor_main"
+        pkg="fanuc_lrmate200id_gazebo_demo" type="touch_the_floor_main.py"
+        output="screen">
+    <rosparam subst_value="true">
+      reactive: $(arg reactive)
+    </rosparam>
+  </node>
+
+  <group if="$(arg rviz)">
+    <node name="rviz"
+          pkg="rviz" type="rviz"
+          args="-d $(find fanuc_lrmate200id_gazebo_demo)/rvizconfig/touch_the_floor.rviz">
+    </node>
+  </group>
+
+</launch>
diff --git a/fanuc_lrmate200id_gazebo_demo/models/kiva_pod/meshes/pod_lowres.stl b/fanuc_lrmate200id_gazebo_demo/models/kiva_pod/meshes/pod_lowres.stl
diff --git a/fanuc_lrmate200id_gazebo_demo/models/kiva_pod/model.config b/fanuc_lrmate200id_gazebo_demo/models/kiva_pod/model.config
@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>Kiva Amazon Pod</name>
+  <version>1.0</version>
+  <sdf version="1.0">model.sdf</sdf>
+
+  <author>
+    <name>Joe Romano</name>
+    <email>[email protected]</email>
+  </author>
+
+  <description>
+    A model of a Kiva Amazon Shelf Pod
+  </description>
+
+  <depend>
+  </depend>
+</model>