Merge pull request #9 from tork-a/add_param_exclude_joints

Add param exclude joints

README.md

@@ -22,63 +22,131 @@ keyboards, and teaching pendants using these packages.
 
 # Quick start
 
-## Launch MoveIt!
-
 You can see the idea of jog_control package by demo with simulation
-and MoveIt!.
+and MoveIt!. Some robots are from
+[ROS-Industrial repositry](https://github.com/ros-industrial), which
+you need to build from source code.
+
+## UR5 
+
+![UR5 jog control](image/ur5_jog.png)
+
+Launch simulation and MoveIt!
+
+```
+$ roslaunch ur_gazebo ur5_joint_limited.launch
+$ roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true
+$ roslaunch ur5_moveit_config moveit_rviz.launch config:=true
+```
 
-You can use UR5:
+And launch jog nodes.
 
 ```
 $ roslaunch jog_controller ur5.launch
 ```
 
-![UR5 jog control](image/ur5_jog.png)
+## TRA1
+
+![TRA1 jog control](image/tra1_jog.png)
 
-TRA1:
+Launch simulation and MoveIt!
+
+```
+$ roslaunch tra1_bringup tra1_bringup.launch simulation:=true
+$ roslaunch tra1_bringup tra1_moveit.launch 
+```
+
+And launch jog nodes.
 
 ```
 $ roslaunch jog_controller tra1.launch
 ```
 
-![TRA1 jog control](image/tra1_jog.png)
+## Denso VS060
+
+![Denso VS060 jog control](image/vs060_jog.png)
 
-Denso VS060:
+Launch simulation and MoveIt!
+
+```
+$ roslaunch denso_launch denso_vs060_moveit_demo_simulation.launch 
+```
+
+And launch jog nodes.
 
 ```
 $ roslaunch jog_controller vs060.launch
 ```
 
-![Denso VS060 jog control](image/vs060_jog.png)
+## NEXTAGE Open
+
+![NEXTAGE Open jog control](image/nextage_jog.png)
 
-NEXTAGE Open:
+Launch simulation and MoveIt!
 
 ```
 $ rtmlaunch nextage_ros_bridge nextage_ros_bridge_simulation.launch
+$ roslaunch nextage_moveit_config moveit_planning_execution.launch 
+```
+
+And launch jog nodes.
+
+```
 $ roslaunch jog_controller nextage.launch
 ```
 
-![NEXTAGE Open jog control](image/nextage_jog.png)
+## ABB IRB2400
 
-You can use robots on ROS-I repositry.
+![ABB IRB2400 jog control](image/abb_irb2400_jog.png)
 
-ABB IRB2400:
-(CAUTION: The model of this robot is a bit strange in joint limits. 
+(CAUTION: The model of this robot is a bit strange in joint limits.
 The jog may not move because of joints flipping.)
 
+Launch simulation and MoveIt!
+
+```
+$ roslaunch abb_irb2400_moveit_config moveit_planning_execution.launch 
+```
+
+And launch jog nodes.
+
 ```
 $ roslaunch jog_controller abb_irb2400.launch 
 ```
 
-![ABB IRB2400 jog control](image/abb_irb2400_jog.png)
+## MOTOMAN SIA20D
 
-MOTOMAN SIA20D:
+![MOTOMAN SIA20D jog control](image/motoman_sia20d_jog.png)
+
+Launch simulation and MoveIt!
+
+```
+$ roslaunch motoman_sia20d_moveit_config moveit_planning_execution.launch sim:=true
+```
+
+And launch jog nodes.
 
 ```
 $ roslaunch jog_controller motoman_sia20d.launch
 ```
 
-![MOTOMAN SIA20D jog control](image/motoman_sia20d_jog.png)
+## MOTOMAN SDA10F
+
+![MOTOMAN SDA10F jog control](image/motoman_sda10f_jog.png)
+
+This is another dual arm robot by Yaskawa MOTOMAN.
+
+Launch simulation and MoveIt!
+
+```
+$ roslaunch motoman_sda10f_moveit_config moveit_planning_execution.launch sim:=true
+```
+
+And launch jog nodes.
+
+```
+$ roslaunch jog_controller motoman_sda10f.launch 
+```
 
 ## rviz JogFramePanel Pugin
 
@@ -106,7 +174,7 @@ TBA.
 
 # TODO
 
-- Orientation(pose) jogging (work in progress)
+- For precise jog and stability, joint position should updated only when the jog start
 - Wiser target picking (group name, target link, etc)
 - Marker visualization for target link and base link
 

jog_controller/config/baxter_jog.yaml

@@ -22,3 +22,5 @@ jog_frame_node:
   link_names:
     - left_hand
     - right_hand
+  exclude_joint_names:
+    - head_nod

jog_controller/config/motoman_sda10f_jog.yaml

@@ -0,0 +1,50 @@
+jog_joint_node:
+  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
+    - arm_right_joint_5_r
+    - arm_right_joint_6_b
+    - arm_right_joint_7_t
+    - torso_joint_b1
+    - torso_joint_b2
+
+jog_frame_node:
+  group_names:
+    - arm_left
+    - arm_right
+  link_names:
+    - arm_left_link_tool0
+    - arm_right_link_tool0
+
+# motoman sda10f controller list seems to be broken
+move_group:
+  controller_list:
+    - name: ''
+      action_ns: ''
+      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

jog_controller/config/ur5_jog.yaml

@@ -1,5 +1,4 @@
 jog_joint_node:
-  controller_name: arm_controller
   joint_names:
     - shoulder_pan_joint
     - shoulder_lift_joint
@@ -8,7 +7,13 @@ jog_joint_node:
     - wrist_2_joint
     - wrist_3_joint
 
-# UR5 moveit doesn't have controller name somehow    
+jog_frame_node:
+  group_names:
+    - manipulator
+  link_names:
+    - ee_link
+
+# Somehow, UR5 moveit doesn't have controller name, so specify here
 move_group:
   controller_list:
     - name: arm_controller
@@ -21,9 +26,3 @@ move_group:
         - wrist_1_joint
         - wrist_2_joint
         - wrist_3_joint
-
-jog_frame_node:
-  group_names:
-    - manipulator
-  link_names:
-    - ee_link

jog_controller/include/jog_controller/jog_frame_node.h

@@ -59,6 +59,7 @@ class JogFrameNode
 
   std::string target_link_;
   std::string group_name_;
+  std::vector<std::string> exclude_joints_;
 };
 
 } // namespace jog_frame

jog_controller/launch/abb_irb2400.launch

@@ -1,9 +1,6 @@
 <launch>
-  <param name="pub_rate" value="100.0"/>
+  <arg name="use_joy" default="false"/>
 
-  <!-- Launch MoveIt! -->
-  <include file="$(find abb_irb2400_moveit_config)/launch/moveit_planning_execution.launch"/>
-
   <!-- Launch jog controllers -->
   <rosparam command="load"
 	    file="$(find jog_controller)/config/abb_irb2400_jog.yaml"/>
@@ -16,4 +13,11 @@
     <param name="time_from_start" value="1.0"/>
   </node>
 
+  <!-- Launch joypad -->
+  <include if="$(arg use_joy)" file="$(find jog_controller)/launch/joypad.launch">
+    <arg name="group_name" value="manipulator"/>
+    <arg name="frame_id" value="base_link"/>
+    <arg name="link_name" value="tool0"/>
+  </include>
+
 </launch>

jog_controller/launch/baxter_fake.launch

@@ -0,0 +1,27 @@
+<launch>
+  <!-- Load model file -->
+  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find baxter_description)/urdf/baxter.urdf.xacro'" />
+
+  <node name="tfpub" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 world base 100"/>
+
+  <!-- Launch MoveIt! -->
+  <include file="$(find baxter_moveit_config)/launch/baxter_grippers.launch"/>
+
+  <!-- fake_joint_driver_node -->
+  <node name="fake_joint_driver" pkg="fake_joint_driver" type="fake_joint_driver_node">
+    <remap from="joint_states" to="/robot/joint_states"/>
+  </node>
+
+  <!-- robot_state_publisher -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
+    <remap from="joint_states" to="/robot/joint_states"/>
+  </node>
+
+  <!-- Load joint trajecotory controller -->
+  <rosparam file="$(find jog_controller)/config/baxter_controllers.yaml" />
+
+  <node name="controller_spawner"
+	pkg="controller_manager" type="spawner" respawn="false"
+	args="joint_state_controller robot/limb/right robot/limb/left"/> 
+
+</launch>

jog_controller/launch/motoman_sda10f.launch

@@ -0,0 +1,20 @@
+<launch>
+  <arg name="use_joy" default="false"/>
+
+  <!-- Launch jog controllers -->
+  <rosparam command="load"
+	    file="$(find jog_controller)/config/motoman_sda10f_jog.yaml"/>
+  <node name="jog_joint_node" pkg="jog_controller" type="jog_joint_node">
+    <remap from="command" to="joint_path_command"/>
+  </node>
+  <node name="jog_frame_node" pkg="jog_controller" type="jog_frame_node">
+    <remap from="command" to="joint_path_command"/>
+  </node>
+  <!-- Launch joypad -->
+  <include if="$(arg use_joy)" file="$(find jog_controller)/launch/joypad.launch">
+    <arg name="group_name" value="manipulator"/>
+    <arg name="frame_id" value="base_link"/>
+    <arg name="link_name" value="tool0"/>
+  </include>
+
+</launch>

jog_controller/launch/motoman_sia20d.launch

@@ -1,10 +1,5 @@
 <launch>
-  <param name="pub_rate" value="100.0"/>
-
-  <!-- Launch MoveIt! -->
-  <include file="$(find motoman_sia20d_moveit_config)/launch/moveit_planning_execution.launch">
-    <arg name="sim" value="true"/>
-  </include>
+  <arg name="use_joy" default="false"/>
 
   <!-- Launch jog controllers -->
   <rosparam command="load"
@@ -18,4 +13,11 @@
     <param name="time_from_start" value="0.01"/>
   </node>
 
+  <!-- Launch joypad -->
+  <include if="$(arg use_joy)" file="$(find jog_controller)/launch/joypad.launch">
+    <arg name="group_name" value="manipulator"/>
+    <arg name="frame_id" value="base_link"/>
+    <arg name="link_name" value="tool0"/>
+  </include>
+
 </launch>

jog_controller/launch/nextage.launch

@@ -6,6 +6,7 @@
   <node name="jog_joint_node" pkg="jog_controller" type="jog_joint_node"/>
   <node name="jog_frame_node" pkg="jog_controller" type="jog_frame_node"/>
 
+  <!-- Launch joypad -->
   <include if="$(arg use_joy)" file="$(find jog_controller)/launch/joypad.launch">
     <arg name="group_name" value="left_arm"/>
     <arg name="frame_id" value="WAIST"/>

jog_controller/launch/tra1.launch

@@ -7,6 +7,7 @@
   <node name="jog_joint_node" pkg="jog_controller" type="jog_joint_node"/>
   <node name="jog_frame_node" pkg="jog_controller" type="jog_frame_node"/>
 
+  <!-- Launch joypad -->
   <include if="$(arg use_joy)" file="$(find jog_controller)/launch/joypad.launch">
     <arg name="group_name" value="manipulator"/>
     <arg name="frame_id" value="base_link"/>

jog_controller/launch/ur5.launch

@@ -1,18 +1,16 @@
 <launch>
-  <include file="$(find ur_gazebo)/launch/ur5_joint_limited.launch"/>
-
-  <include file="$(find ur5_moveit_config)/launch/ur5_moveit_planning_execution.launch">
-    <arg name="sim" value="true"/>
-    <arg name="limited" value="false"/>
-  </include>
-
-  <include file="$(find ur5_moveit_config)/launch/moveit_rviz.launch">
-    <arg name="config" value="true"/>
-  </include>
+  <arg name="use_joy" default="false"/>
 
   <rosparam command="load"
 	    file="$(find jog_controller)/config/ur5_jog.yaml"/>
   <node name="jog_joint_node" pkg="jog_controller" type="jog_joint_node"/>
   <node name="jog_frame_node" pkg="jog_controller" type="jog_frame_node"/>
 
+  <!-- Launch joypad -->
+  <include if="$(arg use_joy)" file="$(find jog_controller)/launch/joypad.launch">
+    <arg name="group_name" value="manipulator"/>
+    <arg name="frame_id" value="base_link"/>
+    <arg name="link_name" value="link_ee"/>
+  </include>
+
 </launch>