Merge pull request #43 from Danfoa/kinetic-devel

Add CSDA10F robot packages for simulation and robot operation
ros-industrial · Jul 30, 2020 · ab6be97 · ab6be97
2 parents 18ed5ed + 68c37c8
commit ab6be97
Show file tree

Hide file tree

Showing 11 changed files with 145 additions and 22 deletions.
diff --git a/motoman_csda10f_moveit_config/config/controllers.yaml b/motoman_csda10f_moveit_config/config/controllers.yaml
@@ -0,0 +1,48 @@
+controller_list:
+  - name: ''
+    action_ns: joint_trajectory_action
+    type: FollowJointTrajectory
+    joints: [arm_left_joint_1_s,
+            arm_left_joint_2_l,
+            arm_left_joint_3_e,
+            arm_left_joint_4_u,
+            arm_left_joint_5_r,
+            arm_left_joint_6_b,
+            arm_left_joint_7_t,
+            arm_right_joint_1_s,
+            arm_right_joint_2_l,
+            arm_right_joint_3_e,
+            arm_right_joint_4_u,
+            arm_right_joint_5_r,
+            arm_right_joint_6_b,
+            arm_right_joint_7_t,
+            torso_joint_b1,
+            torso_joint_b2]
+  - name: csda10f/csda10f_r1_controller
+    action_ns: joint_trajectory_action
+    type: FollowJointTrajectory
+    joints: [arm_left_joint_1_s,
+            arm_left_joint_2_l,
+            arm_left_joint_3_e,
+            arm_left_joint_4_u,
+            arm_left_joint_5_r,
+            arm_left_joint_6_b,
+            arm_left_joint_7_t]
+  - name: csda10f/csda10f_b1_controller
+    action_ns: joint_trajectory_action
+    type: FollowJointTrajectory
+    joints: [torso_joint_b1]
+  - name: csda10f/csda10f_b2_controller
+    action_ns: joint_trajectory_action
+    type: FollowJointTrajectory
+    joints: [torso_joint_b2]
+  - name: csda10f/csda10f_r2_controller
+    action_ns: joint_trajectory_action
+    type: FollowJointTrajectory
+    joints: [arm_right_joint_1_s,
+            arm_right_joint_2_l,
+            arm_right_joint_3_e,
+            arm_right_joint_4_u,
+            arm_right_joint_5_r,
+            arm_right_joint_6_b,
+            arm_right_joint_7_t]
diff --git a/motoman_csda10f_moveit_config/launch/motoman_csda10f_moveit_controller_manager.launch.xml b/motoman_csda10f_moveit_config/launch/motoman_csda10f_moveit_controller_manager.launch.xml
@@ -1,3 +1,9 @@
 <launch>
 
+  <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
+  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager"/>
+  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
+  <!-- load controller_list -->
+  <rosparam file="$(find motoman_csda10f_moveit_config)/config/controllers.yaml"/>
+
 </launch>
diff --git a/motoman_csda10f_moveit_config/launch/moveit_planning_execution.launch b/motoman_csda10f_moveit_config/launch/moveit_planning_execution.launch
@@ -0,0 +1,56 @@
+<launch>
+  <!--<env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}]: ${message}"/> -->
+
+
+  <!-- The planning and execution components of MoveIt! configured to run -->
+  <!-- using the ROS-Industrial interface. -->
+  <rosparam command="load" file="$(find motoman_csda10f_support)/config/joint_names_csda10f.yaml"/>
+
+  <!-- the "sim" argument controls whether we connect to a Simulated or Real robot -->
+  <!--  - if sim=false, a robot_ip and controller(fs100|dx100) arguments is required -->
+  <arg name="sim" default="false" />
+  <arg name="robot_enable" default="false" doc="Set to true if you want to enable robot at start"/>
+  <arg name="robot_ip" unless="$(arg sim)" doc="IP of controller" />
+  <arg name="controller" unless="$(arg sim)" default="fs100" doc="Series of the controller (dx100, fs100)" />
+
+  <!-- load the robot_description parameter before launching ROS-I nodes -->
+  <include file="$(find motoman_csda10f_moveit_config)/launch/planning_context.launch" >
+    <arg name="load_robot_description" value="true" />
+  </include>
+
+  <!-- run the robot simulator and action interface nodes -->
+  <group if="$(arg sim)">
+    <include file="$(find industrial_robot_simulator)/launch/robot_interface_simulator.launch" />
+  </group>
+
+  <!-- run the "real robot" interface nodes -->
+  <!--   - this typically includes: robot_state, motion_interface, and joint_7_trajectory_action nodes -->
+  <!--   - replace these calls with appropriate robot-specific calls or launch files -->
+  <group unless="$(arg sim)">
+    <include file="$(find motoman_csda10f_support)/launch/robot_interface_streaming_csda10f.launch" >
+      <arg name="robot_ip" value="$(arg robot_ip)"/>
+      <arg name="controller" value="$(arg controller)"/>
+    </include>
+  </group>
+
+  <!-- publish the robot state (tf transforms) -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+
+  <include file="$(find motoman_csda10f_moveit_config)/launch/move_group.launch">
+    <arg name="publish_monitored_planning_scene" value="true"/>
+    <arg name="fake_execution" if="$(arg sim)" value="true"/>
+  </include>
+
+  <include file="$(find motoman_csda10f_moveit_config)/launch/moveit_rviz.launch">
+    <arg name="config" value="true"/>
+  </include>
+
+  <!--<include file="$(find motoman_csda10f_moveit_config)/launch/default_warehouse_db.launch" />-->
+
+  <!-- Enable robot if requested -->
+  <group if="$(arg robot_enable)">
+    <include file="$(find motoman_csda10f_support)/launch/robot_enable.launch" />
+  </group>
+
+</launch>
+
diff --git a/motoman_csda10f_moveit_config/package.xml b/motoman_csda10f_moveit_config/package.xml
@@ -2,18 +2,19 @@
 <package format="2">
 
   <name>motoman_csda10f_moveit_config</name>
-  <version>0.3.0</version>
+  <version>0.2.0</version>
   <description>
-     An automatically generated package with all the configuration and launch files for using the motoman_csda10f with the MoveIt! Motion Planning Framework
+     Package with all the configuration and launch files for using the motoman_csda10f with the MoveIt! Motion Planning Framework, and ready to work with FS100 controller.
   </description>
   <author email="amrith1007@gmail.com">Amrith Ganesh</author>
   <maintainer email="amrith1007@gmail.com">Amrith Ganesh</maintainer>
+  <maintainer email="daniels.ordonez@gmail.com">Daniel Ordonez</maintainer>
 
   <license>BSD</license>
 
-  <url type="website">http://moveit.ros.org/</url>
-  <url type="bugtracker">https://github.com/ros-planning/moveit/issues</url>
-  <url type="repository">https://github.com/ros-planning/moveit</url>
+  <url type="website">http://wiki.ros.org/motoman_sda10f_moveit_config</url>
+  <url type="bugtracker">https://github.com/ros-industrial/motoman/issues</url>
+  <url type="repository">https://github.com/ros-industrial/motoman</url>
 
   <buildtool_depend>catkin</buildtool_depend>
 

diff --git a/motoman_csda10f_support/config/csda10f_motion_interface.yaml b/motoman_csda10f_support/config/csda10f_motion_interface.yaml
@@ -1,17 +1,18 @@
+# Ordering from 0 to 4 is required due to FS100 controller configuration, but it can be that ordering starting from 1 works for you.
 topic_list:
     - name: csda10f_r1_controller
       ns: csda10f
-      group: 1
+      group: 0
       joints: ['arm_left_joint_1_s','arm_left_joint_2_l','arm_left_joint_3_e','arm_left_joint_4_u','arm_left_joint_5_r','arm_left_joint_6_b','arm_left_joint_7_t']
     - name: csda10f_r2_controller
       ns: csda10f
-      group: 2
+      group: 1
       joints: ['arm_right_joint_1_s','arm_right_joint_2_l','arm_right_joint_3_e','arm_right_joint_4_u','arm_right_joint_5_r','arm_right_joint_6_b','arm_right_joint_7_t']
     - name: csda10f_b1_controller
       ns: csda10f
-      group: 3
+      group: 2
       joints: ['torso_joint_b1']
     - name: csda10f_b2_controller
       ns: csda10f
-      group: 4
+      group: 3
       joints: ['torso_joint_b2']
diff --git a/motoman_csda10f_support/config/joint_names_csda10f.yaml b/motoman_csda10f_support/config/joint_names_csda10f.yaml
@@ -1,3 +1,4 @@
+# The order of this list should match that expected by your robot's socket interface. Typically Base -> Tip.
 controller_joint_names: ['arm_left_joint_1_s','arm_left_joint_2_l','arm_left_joint_3_e'
 ,'arm_left_joint_4_u','arm_left_joint_5_r','arm_left_joint_6_b','arm_left_joint_7_t',
 'arm_right_joint_1_s','arm_right_joint_2_l','arm_right_joint_3_e','arm_right_joint_4_u',

diff --git a/motoman_csda10f_support/launch/robot_interface_streaming_csda10f.launch b/motoman_csda10f_support/launch/robot_interface_streaming_csda10f.launch
@@ -1,15 +1,14 @@
-
 <!--
   Manipulator specific version of 'robot_interface_streaming.launch'.
 
   Defaults provided for csda10f:
-   - 7 joints
+   - 15 joints
 
   Usage:
-    robot_interface_streaming_csda10f.launch robot_ip:=<value> controller:=<fs100|dx100>
+    robot_interface_streaming_cda10f.launch robot_ip:=<value> controller:=<fs100|dx100>
 -->
 <launch>
-  <arg name="robot_ip" />
+  <arg name="robot_ip" doc="IP of controller"/>
 
   <!-- controller: Controller name (fs100 or dx100) -->
   <arg name="controller"/>

diff --git a/motoman_csda10f_support/launch/robot_state_visualize_csda10f.launch b/motoman_csda10f_support/launch/robot_state_visualize_csda10f.launch
@@ -3,20 +3,23 @@
   Manipulator specific version of the state visualizer.
 
   Defaults provided for csda10f:
-   - 7 joints
+   - 15 joints
 
   Usage:
-    robot_state_visualize_sia20d.launch robot_ip:=<value> controller:=<fs100|dx100>
+    robot_state_visualize_csda10f.launch robot_ip:=<value> controller:=<fs100|dx100>
 -->
 <launch>
-	<arg name="robot_ip" />
+	<arg name="robot_ip" doc="IP of controller" />
 
   <!-- controller: Controller name (fs100 or dx100) -->
-  <arg name="controller" />
+  <arg name="controller" default="fs100" doc="Series of the controller (dx100, fs100)"/>
 
+  <!-- Load the custom naming of robot joints (param name = controller_joint_names)-->
   <rosparam command="load" file="$(find motoman_csda10f_support)/config/joint_names_csda10f.yaml" />
+  <!-- Load the topic_list parapemeter containing the definition of the motion groups and joints mapping to the real controller ros application-->
   <rosparam command="load" file="$(find motoman_csda10f_support)/config/csda10f_motion_interface.yaml" />
 
+  <!-- Obtain the robot state from the robot controller-->
 	<include file="$(find motoman_driver)/launch/robot_state_$(arg controller).launch">
 		<arg name="robot_ip"   value="$(arg robot_ip)" />
 	</include>

diff --git a/motoman_csda10f_support/launch/test_csda10f.launch b/motoman_csda10f_support/launch/test_csda10f.launch
@@ -1,8 +1,12 @@
 
 <launch>
+	<!-- Load robot description -->
 	<include file="$(find motoman_csda10f_support)/launch/load_csda10f.launch" />
+
 	<param name="use_gui" value="true" />
 	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
 	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+
+	<!-- Start Rviz-->
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find industrial_robot_client)/config/robot_state_visualize.rviz" required="true" />
 </launch>
diff --git a/motoman_csda10f_support/package.xml b/motoman_csda10f_support/package.xml
@@ -18,7 +18,7 @@
   </ul>
   <p>
       Joint limits and maximum joint velocities are based on the information 
-      found in the online 
+      found in the online.
       All urdfs are based on the default motion and joint velocity limits, 
       unless noted otherwise.
     </p>
@@ -29,11 +29,15 @@
     </p>
  </description>
  <author email="amrith1007@gmail.com">Amrith Ganesh</author>
+ <maintainer email="daniels.ordonez@gmail.com">Amrith Ganesh</maintainer>
  <maintainer email="amrith1007@gmail.com">Amrith Ganesh</maintainer>
+
  <license>BSD</license>
- <url type="website">http://wiki.ros.org/motoman_sda10f_support</url>
+
+ <url type="website">http://ros.org/wiki/motoman_sda10f_support</url>
  <url type="bugtracker">https://github.com/ros-industrial/motoman/issues</url>
  <url type="repository">https://github.com/ros-industrial/motoman</url>
+
  <buildtool_depend>catkin</buildtool_depend>
  <test_depend>roslaunch</test_depend>
  <exec_depend>joint_state_publisher</exec_depend>

diff --git a/motoman_csda10f_support/urdf/arm_macro.xacro b/motoman_csda10f_support/urdf/arm_macro.xacro
@@ -121,7 +121,7 @@
 			<child link="${prefix}link_1_s"/>
 			<origin xyz="0.1 ${reflect*0.023} 0.3381" rpy="1.57 0 ${(reflect-1)*1.57}"/>
 			<axis xyz="0 0 ${-reflect}" />
-			<limit lower="-3.13" upper="3.13" effort="0" velocity="2.95" />
+			<limit lower="-2.95" upper="2.95" effort="0" velocity="2.95" />
 		</joint>
 		<joint name="${prefix}joint_2_l" type="revolute">
 			<parent link="${prefix}link_1_s"/>
@@ -142,7 +142,7 @@
 			<child link="${prefix}link_4_u"/>
 			<origin xyz="0.00001 -0.0155 -0.185" rpy="-1.57 0 0"/>
 			<axis xyz="0 0 ${reflect}" />
-			<limit lower="-2.36" upper="2.36" effort="0" velocity="2.95" />
+			<limit lower="-2.35" upper="2.35" effort="0" velocity="2.95" />
 		</joint>
 		<joint name="${prefix}joint_5_r" type="revolute">
 			<parent link="${prefix}link_4_u"/>
@@ -162,7 +162,7 @@
 			<parent link="${prefix}link_6_b"/>
 			<child link="${prefix}link_7_t"/>
 			<origin xyz="0 0.118 -0.015" rpy="1.57 0 0"/>
-			<axis xyz="0 0 ${-reflect}" />
+			<axis xyz="0 0 ${reflect}" />
 			<limit lower="-3.13" upper="3.13" effort="0" velocity="6.97" />
 		</joint>