Merge pull request #292 from wxmerkt/wxm-j2s6s200-integration

j2s6s200 integration

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,53 +3,80 @@
   <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"/>
 
   <group unless="$(arg use_trajectory_controller)">
     <group unless="$(arg is7dof)">
-      <!-- load the joint by joint position controllers -->  
+      <!-- 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"/> 
+               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_3_position_controller finger_tip_1_position_controller
                finger_tip_2_position_controller finger_tip_3_position_controller
-               joint_state_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"/>    
+      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>