m1n6s200 moveit config and two fingers support

kinova_control/launch/kinova_control.launch

@@ -3,6 +3,7 @@
   <arg name="kinova_robotName" default="$(arg kinova_robotType)"/>  
   <arg name="use_trajectory_controller" default="true"/>
   <arg name="is7dof" default="false"/>
+  <arg name="has2finger" default="false"/><!-- if false: 3 finger, true: 2 finger -->
 
   <!-- Load joint controller configurations from YAML file to parameter server -->
   <rosparam file="$(find kinova_control)/config/$(arg kinova_robotName)_control.yaml" command="load"/>
@@ -11,39 +12,63 @@
     <group unless="$(arg is7dof)">
       <!-- load the joint by joint position controllers -->  
       <node name="$(arg kinova_robotName)_joints_controller" pkg="controller_manager" type="spawner" respawn="false"
-        output="screen" ns="$(arg kinova_robotName)" 
+        output="screen" ns="$(arg kinova_robotName)" unless="$(arg has2finger)"
        args="joint_1_position_controller joint_2_position_controller  
                joint_3_position_controller joint_4_position_controller
                joint_5_position_controller joint_6_position_controller 
                finger_2_position_controller finger_1_position_controller 
                finger_3_position_controller finger_tip_1_position_controller
                finger_tip_2_position_controller finger_tip_3_position_controller
                joint_state_controller"/> 
+      <node name="$(arg kinova_robotName)_joints_controller" pkg="controller_manager" type="spawner" respawn="false"
+        output="screen" ns="$(arg kinova_robotName)" if="$(arg has2finger)"
+       args="joint_1_position_controller joint_2_position_controller
+               joint_3_position_controller joint_4_position_controller
+               joint_5_position_controller joint_6_position_controller
+               finger_2_position_controller finger_1_position_controller
+               finger_tip_1_position_controller finger_tip_2_position_controller
+               joint_state_controller"/>
     </group>
     <group if="$(arg is7dof)">
       <!-- load the joint by joint position controllers -->  
       <node name="$(arg kinova_robotName)_joints_controller" pkg="controller_manager" type="spawner" respawn="false"
-        output="screen" ns="$(arg kinova_robotName)" 
+        output="screen" ns="$(arg kinova_robotName)" unless="$(arg has2finger)"
        args="joint_1_position_controller joint_2_position_controller  
                joint_3_position_controller joint_4_position_controller
                joint_5_position_controller joint_6_position_controller joint_7_position_controller
                finger_2_position_controller finger_1_position_controller 
                finger_3_position_controller finger_tip_1_position_controller
                finger_tip_2_position_controller finger_tip_3_position_controller
                joint_state_controller"/> 
+      <node name="$(arg kinova_robotName)_joints_controller" pkg="controller_manager" type="spawner" respawn="false"
+        output="screen" ns="$(arg kinova_robotName)" if="$(arg has2finger)"
+       args="joint_1_position_controller joint_2_position_controller
+               joint_3_position_controller joint_4_position_controller
+               joint_5_position_controller joint_6_position_controller joint_7_position_controller
+               finger_2_position_controller finger_1_position_controller
+               finger_tip_1_position_controller
+               finger_tip_2_position_controller
+               joint_state_controller"/>
     </group>
   </group>
 
   <group if="$(arg use_trajectory_controller)">
     <!-- Effort Joint trajectory controller-->
     <node name="$(arg kinova_robotName)_trajectory_controller" pkg="controller_manager" type="spawner" 
-      output="screen" ns="$(arg kinova_robotName)" 
+      output="screen" ns="$(arg kinova_robotName)" unless="$(arg has2finger)"
       args="effort_joint_trajectory_controller
       effort_finger_trajectory_controller    
       finger_tip_1_position_controller
       finger_tip_2_position_controller 
       finger_tip_3_position_controller
       joint_state_controller"/>    
+    <node name="$(arg kinova_robotName)_trajectory_controller" pkg="controller_manager" type="spawner"
+      output="screen" ns="$(arg kinova_robotName)" if="$(arg has2finger)"
+      args="effort_joint_trajectory_controller
+      effort_finger_trajectory_controller
+      finger_tip_1_position_controller
+      finger_tip_2_position_controller
+      joint_state_controller"/>
   </group>
 
   <!-- convert joint states to TF transforms for rviz, etc -->

kinova_gazebo/launch/robot_launch.launch

@@ -10,6 +10,7 @@
   <arg name="debug" default="false"/>
   <arg name="use_trajectory_controller" default="true"/>
   <arg name="is7dof" default="false"/>
+  <arg name="has2finger" default="false"/>
 
   <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
@@ -26,39 +27,71 @@
     command="$(find xacro)/xacro --inorder '$(find kinova_description)/urdf/$(arg kinova_robotType)_standalone.xacro'" />
 
   <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
-  <!-- For the 6DOF -->
-  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" unless="$(arg is7dof)"
-    args="-urdf -model $(arg kinova_robotName) -param robot_description
-        -J $(arg kinova_robotType)_joint_1 0.0
-        -J $(arg kinova_robotType)_joint_2 2.9
-        -J $(arg kinova_robotType)_joint_3 1.3
-        -J $(arg kinova_robotType)_joint_4 -2.07
-        -J $(arg kinova_robotType)_joint_5 1.4
-        -J $(arg kinova_robotType)_joint_6 0.0
-        -J $(arg kinova_robotType)_joint_finger_1 1.0
-        -J $(arg kinova_robotType)_joint_finger_2 1.0
-        -J $(arg kinova_robotType)_joint_finger_3 1.0" />
+  <!-- For 6-DOF -->
+  <group unless="$(arg is7dof)">
+    <!-- For 3 finger -->
+    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" unless="$(arg has2finger)"
+      args="-urdf -model $(arg kinova_robotName) -param robot_description
+          -J $(arg kinova_robotType)_joint_1 0.0
+          -J $(arg kinova_robotType)_joint_2 2.9
+          -J $(arg kinova_robotType)_joint_3 1.3
+          -J $(arg kinova_robotType)_joint_4 -2.07
+          -J $(arg kinova_robotType)_joint_5 1.4
+          -J $(arg kinova_robotType)_joint_6 0.0
+          -J $(arg kinova_robotType)_joint_finger_1 1.0
+          -J $(arg kinova_robotType)_joint_finger_2 1.0
+          -J $(arg kinova_robotType)_joint_finger_3 1.0" />
 
-  <!-- For the 7DOF -->
-    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" if="$(arg is7dof)"
-    args="-urdf -model $(arg kinova_robotName) -param robot_description
-        -J $(arg kinova_robotType)_joint_1 0.0
-        -J $(arg kinova_robotType)_joint_2 2.9
-        -J $(arg kinova_robotType)_joint_3 0.0
-        -J $(arg kinova_robotType)_joint_4 1.3
-        -J $(arg kinova_robotType)_joint_5 -2.07
-        -J $(arg kinova_robotType)_joint_6 1.4
-        -J $(arg kinova_robotType)_joint_7 0.0
-        -J $(arg kinova_robotType)_joint_finger_1 1.0
-        -J $(arg kinova_robotType)_joint_finger_2 1.0
-        -J $(arg kinova_robotType)_joint_finger_3 1.0" />
+    <!-- For 2 finger -->
+    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" if="$(arg has2finger)"
+      args="-urdf -model $(arg kinova_robotName) -param robot_description
+          -J $(arg kinova_robotType)_joint_1 0.0
+          -J $(arg kinova_robotType)_joint_2 2.9
+          -J $(arg kinova_robotType)_joint_3 1.3
+          -J $(arg kinova_robotType)_joint_4 -2.07
+          -J $(arg kinova_robotType)_joint_5 1.4
+          -J $(arg kinova_robotType)_joint_6 0.0
+          -J $(arg kinova_robotType)_joint_finger_1 1.0
+          -J $(arg kinova_robotType)_joint_finger_2 1.0" />
+  </group>
+
+  <!-- For 7-DOF -->
+  <group if="$(arg is7dof)">
+    <!-- For 3 finger -->
+    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" unless="$(arg has2finger)"
+      args="-urdf -model $(arg kinova_robotName) -param robot_description
+          -J $(arg kinova_robotType)_joint_1 0.0
+          -J $(arg kinova_robotType)_joint_2 2.9
+          -J $(arg kinova_robotType)_joint_3 0.0
+          -J $(arg kinova_robotType)_joint_4 1.3
+          -J $(arg kinova_robotType)_joint_5 -2.07
+          -J $(arg kinova_robotType)_joint_6 1.4
+          -J $(arg kinova_robotType)_joint_7 0.0
+          -J $(arg kinova_robotType)_joint_finger_1 1.0
+          -J $(arg kinova_robotType)_joint_finger_2 1.0
+          -J $(arg kinova_robotType)_joint_finger_3 1.0" />
+
+    <!-- For 2 finger -->
+    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" if="$(arg has2finger)"
+      args="-urdf -model $(arg kinova_robotName) -param robot_description
+          -J $(arg kinova_robotType)_joint_1 0.0
+          -J $(arg kinova_robotType)_joint_2 2.9
+          -J $(arg kinova_robotType)_joint_3 0.0
+          -J $(arg kinova_robotType)_joint_4 1.3
+          -J $(arg kinova_robotType)_joint_5 -2.07
+          -J $(arg kinova_robotType)_joint_6 1.4
+          -J $(arg kinova_robotType)_joint_7 0.0
+          -J $(arg kinova_robotType)_joint_finger_1 1.0
+          -J $(arg kinova_robotType)_joint_finger_2 1.0" />
+  </group>
 
   <!-- ros_control launch file -->
   <include file="$(find kinova_control)/launch/kinova_control.launch">
     <arg name="kinova_robotName" value="$(arg kinova_robotName)"/>
     <arg name="kinova_robotType" value="$(arg kinova_robotType)"/>
     <arg name="use_trajectory_controller" value="$(arg use_trajectory_controller)"/>
     <arg name="is7dof" value="$(arg is7dof)"/>
+    <arg name="has2finger" value="$(arg has2finger)"/>
   </include>
 
   <!-- rqt launch file -->

kinova_moveit/robot_configs/m1n6s200_moveit_config/.setup_assistant

@@ -0,0 +1,8 @@
+moveit_setup_assistant_config:
+  URDF:
+    package: kinova_description
+    relative_path: urdf/m1n6s200_standalone.xacro
+  SRDF:
+    relative_path: config/m1n6s200.srdf
+  CONFIG:
+    generated_timestamp: 1484110350

kinova_moveit/robot_configs/m1n6s200_moveit_config/CMakeLists.txt

@@ -0,0 +1,9 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(m1n6s200_moveit_config)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

kinova_moveit/robot_configs/m1n6s200_moveit_config/config/controllers.yaml

@@ -0,0 +1,19 @@
+controller_list:
+  - name: m1n6s200
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints:
+      - m1n6s200_joint_1
+      - m1n6s200_joint_2
+      - m1n6s200_joint_3
+      - m1n6s200_joint_4
+      - m1n6s200_joint_5
+      - m1n6s200_joint_6      
+  - name: m1n6s200_gripper
+    action_ns: gripper_command
+    type: GripperCommand
+    default: true    
+    joints:
+      - m1n6s200_joint_finger_1
+      - m1n6s200_joint_finger_2

kinova_moveit/robot_configs/m1n6s200_moveit_config/config/controllers_ros_control.yaml

@@ -0,0 +1,19 @@
+controller_list:
+  - name: m1n6s200/effort_joint_trajectory_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints:
+      - m1n6s200_joint_1
+      - m1n6s200_joint_2
+      - m1n6s200_joint_3
+      - m1n6s200_joint_4
+      - m1n6s200_joint_5
+      - m1n6s200_joint_6
+  - name: m1n6s200/effort_finger_trajectory_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true    
+    joints:
+      - m1n6s200_joint_finger_1
+      - m1n6s200_joint_finger_2

kinova_moveit/robot_configs/m1n6s200_moveit_config/config/fake_controllers.yaml

@@ -0,0 +1,17 @@
+controller_list:
+  - name: fake_arm_controller
+    joints:
+      - m1n6s200_joint_1
+      - m1n6s200_joint_2
+      - m1n6s200_joint_3
+      - m1n6s200_joint_4
+      - m1n6s200_joint_5
+      - m1n6s200_joint_6
+  - name: fake_gripper_controller
+    joints:
+      - m1n6s200_joint_finger_1
+      - m1n6s200_joint_finger_2
+
+initial:
+  - group: arm
+    pose: Home

kinova_moveit/robot_configs/m1n6s200_moveit_config/config/joint_limits.yaml

@@ -0,0 +1,44 @@
+# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+joint_limits:
+  m1n6s200_joint_1:
+    has_velocity_limits: true
+    max_velocity: 0.35
+    has_acceleration_limits: false
+    max_acceleration: 0
+  m1n6s200_joint_2:
+    has_velocity_limits: true
+    max_velocity: 0.35
+    has_acceleration_limits: false
+    max_acceleration: 0
+  m1n6s200_joint_3:
+    has_velocity_limits: true
+    max_velocity: 0.35
+    has_acceleration_limits: false
+    max_acceleration: 0
+  m1n6s200_joint_4:
+    has_velocity_limits: true
+    max_velocity: 0.35
+    has_acceleration_limits: false
+    max_acceleration: 0
+  m1n6s200_joint_5:
+    has_velocity_limits: true
+    max_velocity: 0.35
+    has_acceleration_limits: false
+    max_acceleration: 0
+  m1n6s200_joint_6:
+    has_velocity_limits: true
+    max_velocity: 0.35
+    has_acceleration_limits: false
+    max_acceleration: 0
+  m1n6s200_joint_finger_1:
+    has_velocity_limits: true
+    max_velocity: 5
+    has_acceleration_limits: false
+    max_acceleration: 0
+  m1n6s200_joint_finger_2:
+    has_velocity_limits: true
+    max_velocity: 5
+    has_acceleration_limits: false
+    max_acceleration: 0

kinova_moveit/robot_configs/m1n6s200_moveit_config/config/joint_names.yaml

@@ -0,0 +1 @@
+controller_joint_names: [m1n6s200_joint_1 m1n6s200_joint_2 m1n6s200_joint_3 m1n6s200_joint_4 m1n6s200_joint_5 m1n6s200_joint_6]

kinova_moveit/robot_configs/m1n6s200_moveit_config/config/kinematics.yaml

@@ -0,0 +1,7 @@
+arm:
+  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin 
+  #kinematics_solver: m1n6s200_mico_arm_kinematics/IKFastKinematicsPlugin
+  #kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_attempts: 3
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.005