Add j2s6s200 arm gazebo

kinova_control/config/j2s6s200_control.yaml

@@ -0,0 +1,160 @@
+j2s6s200:
+  effort_finger_trajectory_controller:
+    constraints:
+      goal_time: 1.0
+      j2s6s200_joint_finger_1:
+        goal: 0.02
+        trajectory: 0.05
+      j2s6s200_joint_finger_2:
+        goal: 0.02
+        trajectory: 0.05
+      stopped_velocity_tolerance: 0.02
+    gains:
+      j2s6s200_joint_finger_1:
+        d: 0
+        i: 0
+        i_clamp: 1
+        p: 10
+      j2s6s200_joint_finger_2:
+        d: 0
+        i: 0
+        i_clamp: 1
+        p: 10
+    joints:
+    - j2s6s200_joint_finger_1
+    - j2s6s200_joint_finger_2
+    type: effort_controllers/JointTrajectoryController
+  effort_joint_trajectory_controller:
+    constraints:
+      goal_time: 1.0
+      j2s6s200_joint_1:
+        goal: 0.02
+        trajectory: 0.05
+      j2s6s200_joint_2:
+        goal: 0.02
+        trajectory: 0.05
+      j2s6s200_joint_3:
+        goal: 0.02
+        trajectory: 0.05
+      j2s6s200_joint_4:
+        goal: 0.02
+        trajectory: 0.05
+      j2s6s200_joint_5:
+        goal: 0.02
+        trajectory: 0.05
+      j2s6s200_joint_6:
+        goal: 0.02
+        trajectory: 0.05
+      stopped_velocity_tolerance: 0.02
+    gains:
+      j2s6s200_joint_1:
+        d: 0
+        i: 0
+        i_clamp: 10
+        p: 5000
+      j2s6s200_joint_2:
+        d: 0
+        i: 0
+        i_clamp: 10
+        p: 5000
+      j2s6s200_joint_3:
+        d: 0
+        i: 0
+        i_clamp: 10
+        p: 5000
+      j2s6s200_joint_4:
+        d: 0
+        i: 0
+        i_clamp: 10
+        p: 500
+      j2s6s200_joint_5:
+        d: 0
+        i: 0
+        i_clamp: 10
+        p: 200
+      j2s6s200_joint_6:
+        d: 0
+        i: 0
+        i_clamp: 10
+        p: 500
+    joints:
+    - j2s6s200_joint_1
+    - j2s6s200_joint_2
+    - j2s6s200_joint_3
+    - j2s6s200_joint_4
+    - j2s6s200_joint_5
+    - j2s6s200_joint_6
+    type: effort_controllers/JointTrajectoryController
+  finger_1_position_controller:
+    joint: j2s6s200_joint_finger_1
+    pid:
+      d: 0
+      i: 0
+      p: 10
+    type: effort_controllers/JointPositionController
+  finger_2_position_controller:
+    joint: j2s6s200_joint_finger_2
+    pid:
+      d: 0
+      i: 0
+      p: 10
+    type: effort_controllers/JointPositionController
+  finger_tip_1_position_controller:
+    joint: j2s6s200_joint_finger_tip_1
+    pid:
+      d: 0
+      i: 0
+      p: 0.5
+    type: effort_controllers/JointPositionController
+  finger_tip_2_position_controller:
+    joint: j2s6s200_joint_finger_tip_2
+    pid:
+      d: 0
+      i: 0
+      p: 0.5
+    type: effort_controllers/JointPositionController
+  joint_1_position_controller:
+    joint: j2s6s200_joint_1
+    pid:
+      d: 0
+      i: 0
+      p: 5000
+    type: effort_controllers/JointPositionController
+  joint_2_position_controller:
+    joint: j2s6s200_joint_2
+    pid:
+      d: 0
+      i: 0
+      p: 5000
+    type: effort_controllers/JointPositionController
+  joint_3_position_controller:
+    joint: j2s6s200_joint_3
+    pid:
+      d: 0
+      i: 0
+      p: 5000
+    type: effort_controllers/JointPositionController
+  joint_4_position_controller:
+    joint: j2s6s200_joint_4
+    pid:
+      d: 0
+      i: 0
+      p: 500
+    type: effort_controllers/JointPositionController
+  joint_5_position_controller:
+    joint: j2s6s200_joint_5
+    pid:
+      d: 0
+      i: 0
+      p: 200
+    type: effort_controllers/JointPositionController
+  joint_6_position_controller:
+    joint: j2s6s200_joint_6
+    pid:
+      d: 0
+      i: 0
+      p: 500
+    type: effort_controllers/JointPositionController
+  joint_state_controller:
+    publish_rate: 50
+    type: joint_state_controller/JointStateController

kinova_control/launch/kinova_control.launch

@@ -3,6 +3,8 @@
   <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 -->
+  <arg name="description_name" default="robot_description"/><!-- allows remapping of robot_description into a namespace -->
 
   <!-- Load joint controller configurations from YAML file to parameter server -->
   <rosparam file="$(find kinova_control)/config/$(arg kinova_robotName)_control.yaml" command="load"/>
@@ -11,19 +13,36 @@
     <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
@@ -37,19 +56,27 @@
   <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"/>    
+      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 -->
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
-        respawn="false" output="screen">    
-    <remap from="/joint_states" to="/$(arg kinova_robotName)/joint_states"/>    
+  <node name="$(arg kinova_robotName)_robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
+        respawn="true" output="screen">
+    <remap from="/joint_states" to="/$(arg kinova_robotName)/joint_states"/>
+    <remap from="robot_description" to="$(arg description_name)"/>
   </node>
 
   <node name="command_robot_home_pose" pkg="kinova_control" type="move_robot.py"

kinova_description/urdf/j2s6s200_integration.xacro

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<!-- j2s6_2 refers to jaco v2 6DOF spherical 2fingers -->
+
+<robot xmlns:xi="http://www.w3.org/2001/XInclude" 
+    xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" 
+    xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" 
+    xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" 
+    xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" 
+    xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" 
+    xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" 
+    xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" 
+    xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" 
+    xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" 
+    xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable" 
+    xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" 
+    xmlns:xacro="http://www.ros.org/wiki/xacro" name="j2s6s300">
+
+
+    <xacro:include filename="$(find kinova_description)/urdf/j2s6s200.xacro"/>
+
+    <xacro:arg name="arm_root" default="base_link" />
+    <xacro:property name="arm_root" value="$(arg arm_root)" />
+    <link name="${arm_root}"/>
+    <xacro:j2s6s200 base_parent="${arm_root}"/>
+</robot>

kinova_description/urdf/j2s6s300_integration.xacro

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<!-- j2s6_3 refers to jaco v2 6DOF spherical 3fingers -->
+
+<robot xmlns:xi="http://www.w3.org/2001/XInclude" 
+    xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" 
+    xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" 
+    xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" 
+    xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" 
+    xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" 
+    xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" 
+    xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" 
+    xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" 
+    xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" 
+    xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable" 
+    xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" 
+    xmlns:xacro="http://www.ros.org/wiki/xacro" name="j2s6s300">
+
+
+    <xacro:include filename="$(find kinova_description)/urdf/j2s6s300.xacro"/>
+
+    <xacro:arg name="arm_root" default="base_link" />
+    <xacro:property name="arm_root" value="$(arg arm_root)" />
+    <link name="${arm_root}"/>
+    <xacro:j2s6s300 base_parent="${arm_root}"/>
+</robot>

kinova_gazebo/launch/robot_launch.launch

@@ -11,6 +11,8 @@
   <arg name="use_trajectory_controller" default="true"/>
   <arg name="is7dof" default="false"/>
   <arg name="rqt" default="false"/>
+  <arg name="has2finger" default="false"/>
+  <arg name="description_name" default="robot_description"/> <!-- allows remapping of robot_description into a namespace -->
 
   <!-- 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">
@@ -28,20 +30,49 @@
 
   <!-- 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)"
+  <group unless="$(arg is7dof)">
+    <!-- For 3 fingers -->
+    <node unless="$(arg has2finger)" name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
+      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 2 fingers -->
+    <node if="$(arg has2finger)" name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
+      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 the 7DOF -->
+  <group if="$(arg is7dof)">
+    <!-- For 3 fingers -->
+    <node unless="$(arg has2finger)" name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
     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_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 the 7DOF -->
-    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" if="$(arg is7dof)"
+    <!-- For 2 fingers -->
+    <node if="$(arg has2finger)" name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
     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
@@ -51,15 +82,17 @@
         -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" />
+        -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)"/>
+    <arg name="description_name" value="$(arg description_name)"/>
   </include>
 
   <!-- rqt launch file -->