7dof

hlpr_gazebo/controller/arm_controller_j2s7s300.yaml

@@ -0,0 +1,23 @@
+j2s7s300:
+    type: position_controllers/JointTrajectoryController
+    joints:
+        - j2s7s300_joint_1
+        - j2s7s300_joint_2
+        - j2s7s300_joint_3
+        - j2s7s300_joint_4
+        - j2s7s300_joint_5
+        - j2s7s300_joint_6
+        - j2s7s300_joint_7
+    constraints:
+        goal_time: 0.6
+        stopped_velocity_tolerance: 0.05
+        j2s7s300_joint_1: {trajectory: 0.1, goal: 0.1}
+        j2s7s300_joint_2: {trajectory: 0.1, goal: 0.1}
+        j2s7s300_joint_3: {trajectory: 0.1, goal: 0.1}
+        j2s7s300_joint_4: {trajectory: 0.1, goal: 0.1}
+        j2s7s300_joint_5: {trajectory: 0.1, goal: 0.1}
+        j2s7s300_joint_6: {trajectory: 0.1, goal: 0.1}
+        j2s7s300_joint_7: {trajectory: 0.1, goal: 0.1}
+    stop_trajectory_duration: 0.5
+    state_publish_rate:  100
+    action_monitor_rate: 10

hlpr_gazebo/controller/gripper_controller_robotiq.yaml

@@ -7,5 +7,5 @@ gripper_controller:
       stopped_velocity_tolerance: 0.05
       right_robotiq_85_left_knuckle_joint: {trajectory: 0.1, goal: 0.1}
   stop_trajectory_duration: 0.5
-  state_publish_rate:  25
+  state_publish_rate:  100
   action_monitor_rate: 10

hlpr_gazebo/controller/gripper_controller_robotiq_j2s7s300.yaml

@@ -0,0 +1,11 @@
+gripper_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+     - robotiq_85_left_knuckle_joint
+  constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      robotiq_85_left_knuckle_joint: {trajectory: 0.1, goal: 0.1}
+  stop_trajectory_duration: 0.5
+  state_publish_rate:  25
+  action_monitor_rate: 10

hlpr_gazebo/controller/joint_state_controller.yaml

@@ -1,3 +1,3 @@
 joint_state_controller:
     type: joint_state_controller/JointStateController
-    publish_rate: 10
+    publish_rate: 100

hlpr_gazebo/launch/kinova_control.launch

@@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="kinova_robotType" default="j2n6s300"/>
+  <arg name="joint_state_topic" default="/vector/right_arm/states"/>
+  <arg name="kinova_robotName" default="$(arg kinova_robotType)"/>  
+  <arg name="use_trajectory_controller" default="true"/>
+
+  <!-- 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)">
+    <!-- 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)" 
+      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
+            joint_state_controller"/>    
+  </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)" 
+      args="effort_joint_trajectory_controller
+      effort_finger_trajectory_controller    
+      joint_state_controller"/>    
+  </group>
+
+  <!-- convert joint states to TF transforms for rviz, etc -->
+  <!--node name="kinova_robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
+        respawn="false" output="screen">    
+    <remap from="/joint_states" to="/$(arg joint_state_topic)"/>    
+  </node-->
+
+
+    <!--node name="$(arg kinova_robotType)_tf_updater" pkg="kinova_driver" type="kinova_tf_updater" output="screen" cwd="node" args="$(arg kinova_robotType)">
+      <remap from="/$(arg kinova_robotType)_tf_updater/in/joint_angles" to="/$(arg kinova_robotType)_driver/out/joint_state"/>
+</node-->
+
+
+  <node name="command_robot_home_pose" pkg="kinova_control" type="move_robot.py"
+        respawn="false" output="screen" args="$(arg kinova_robotType)">       
+  </node>
+
+</launch>

hlpr_gazebo/launch/poli.launch

@@ -0,0 +1,6 @@
+<?xml version="1.0"?>
+<launch>
+    <include file="$(find hlpr_gazebo)/launch/vector.launch">
+        <arg name="robot_description" default="$(find poli_description)/launch/poli_upload.launch"/>
+    </include>
+</launch>

hlpr_gazebo/launch/vector.launch

@@ -6,20 +6,18 @@
     <arg name="world" default="worlds/empty.world" />
     <arg name="room" default="$(find hlpr_gazebo)/launch/simple_room.launch" />
     <arg name="robot_description" default="$(find vector_description)/launch/vector_upload.launch"/>
-    <!-- moveit and arm specific. See vector_controller.launch for details -->
-    <arg name="moveit_launch" default="false" />
-    <arg name="moveit_launch_file" default="$(find hlpr_wpi_jaco_moveit_config)/launch/hlpr_wpi_jaco_simple_moveit.launch" />
-    <arg name="wpi_jaco_launch" default="false" />
-    <arg name="use_wpi_jaco_exec" default="false" />
+    <!-- REMOVING ALL INSTANCES OF MOVEIT AND ARM DRIVER. NO LONGER USING THIS -->
+    <!-- trak_ik and arm specific. See vector_controllers.launch for details -->    
+    <arg name="trakik_launch" default="true" />
+    <arg name="trakik_launch_file" default="$(find hlpr_trac_ik)/launch/start_ik_service.launch"/>
     <arg name="robot_spawn_loc" default="" />
-    <arg name="use_octomap" default="false" />
-
 
     <!-- startup simulated world -->
     <include file="$(find gazebo_ros)/launch/empty_world.launch">
         <arg name="world_name" default="$(arg world)"/>
         <arg name="paused" value="$(arg paused)"/>
         <arg name="gui" value="$(arg gui)"/>
+        <arg name="debug" value="false"/>
     </include>
 
     <!-- simple world -->
@@ -28,15 +26,10 @@
     <!-- send robot urdf to param server -->
     <include file="$(arg robot_description)"/>
 
-    <!-- push robot_description to factory and spawn robot in gazebo -->
-    <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot $(arg robot_spawn_loc)" respawn="false" output="screen" />
-
     <!-- Bring up robot controllers -->   
     <include file="$(find hlpr_gazebo)/launch/vector_controllers.launch" >
-        <arg name="moveit_launch" value="$(arg moveit_launch)" />
-        <arg name="wpi_jaco_launch" value="$(arg wpi_jaco_launch)" />
-        <arg name="use_wpi_jaco_exec" value="$(arg use_wpi_jaco_exec)" />
-        <arg name="use_octomap" value="$(arg use_octomap)" />
+        <arg name="trakik_launch_file" value="$(arg trakik_launch_file)" />
+        <arg name="trakik_launch" value="$(arg trakik_launch)" />
     </include>
 
     <!-- Setup all of the TFs, states, etc. --> 
@@ -48,6 +41,9 @@
         <arg name="sim" value="true"/>
     </include>
 
+    <!-- push robot_description to factory and spawn robot in gazebo -->
+    <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot $(arg robot_spawn_loc)" respawn="false" output="screen" />
+
     <!-- Custom laser launch scripts -->
     <group if="$(optenv VECTOR_HAS_SECOND_2D_LASER false)">
         <node pkg="ira_laser_tools" name="laserscan_multi_merger" type="laserscan_multi_merger" output="screen">

hlpr_gazebo/launch/vector_controllers.launch

@@ -1,60 +1,68 @@
 <?xml version="1.0"?>
 <launch>
     <!-- Launch file that loads all of the controllers for gazebo -->
-
-    <!-- moveit_launch: do we launch moveit. wpi_jaco_launch: do we launch wpi_jaco_wrapper warm. use_wpi_jaco_exec - does moveit use wpi_jaco to send commands.
-     WARNING: if you set use_wpi_jaco_exec false and moveit launch true - you cannot execute a trajectory through moveit. If you set use_wpi_jaco_exec to true and wpi_jaco_launch to false, arm teleop will be disabled in simulation and moveit will hang when loading.--> 
-    <arg name="moveit_launch" default="false" />
-    <arg name="wpi_jaco_launch" default="false"/>
-    <arg name="use_wpi_jaco_exec" default="false"/>
-    <arg name="use_octomap" default="false"/>
-    <arg name="moveit_launch_file" default="$(find hlpr_wpi_jaco_moveit_config)/launch/hlpr_wpi_jaco_simple_moveit.launch"/>
-
-    <!-- Load moveit and wpi_jaco arm wrappers -->
-    <group if="$(arg moveit_launch)">
-        <include file="$(arg moveit_launch_file)">
-            <arg name="wpi_jaco_launch" value="$(arg wpi_jaco_launch)"/>
-            <arg name="use_wpi_jaco_exec" value="$(arg use_wpi_jaco_exec)"/>
-            <arg name="sim" value="true"/>
-            <arg name="use_octomap" value="$(arg use_octomap)"/>
-        </include>
+    <!-- GETTING RID OF ALL THINGS RELATED TO MOVEIT IN THIS LAUNCH. LAUNCH IT SEPARATELY USING start_robot_services.launch in hlpr_bringup-->
+    <!-- Trak IK Params -->
+    <arg name="trakik_launch" default="true" />
+    <arg name="trakik_launch_file" default="$(find hlpr_trac_ik)/launch/start_ik_service.launch"/>
+
+    <!-- Load Trak IK -->
+    <group if="$(arg trakik_launch)">
+        <include file="$(arg trakik_launch_file)"/>
     </group>
 
     <!-- Interface node for converting vector commands to gazebo commands -->
     <node name="vector2gazebo" pkg="hlpr_gazebo" type="vector_control_interface.py" output="screen" unless="$(optenv VECTOR_HAS_TWO_KINOVA_ARMS false)" >
         <param name="two_arms" value="$(optenv VECTOR_HAS_TWO_KINOVA_ARMS)" />
+        <param name="use_7dof_jaco" value="$(optenv VECTOR_HAS_KINOVA_7DOF_ARM)" />
     </node>
     <node name="vector2gazebo" pkg="hlpr_gazebo" type="vector_control_interface_two_arms.py" output="screen" if="$(optenv VECTOR_HAS_TWO_KINOVA_ARMS false)" />
 
+    <group unless="$(optenv VECTOR_HAS_KINOVA_7DOF_ARM false)">
+        <!-- Arms: Currently only supports position control  -->
+        <rosparam file="$(find hlpr_gazebo)/controller/arm_controller_jaco.yaml" command="load" if="$(optenv VECTOR_HAS_KINOVA_ARM false)"/>
+        <node name="arm_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="vector/right_arm --shutdown-timeout 0.5" if="$(optenv VECTOR_HAS_KINOVA_ARM false)"/>
 
-    <!-- Arms: Currently only supports position control  -->
-    <rosparam file="$(find hlpr_gazebo)/controller/arm_controller_jaco.yaml" command="load" if="$(optenv VECTOR_HAS_KINOVA_ARM false)"/>
-    <node name="arm_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="vector/right_arm --shutdown-timeout 0.5" if="$(optenv VECTOR_HAS_KINOVA_ARM false)"/>
+         <rosparam file="$(find hlpr_gazebo)/controller/two_arms_controller_jaco.yaml" command="load" if="$(optenv VECTOR_HAS_TWO_KINOVA_ARMS false)"/>
+        <node name="two_arm_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="vector/left_arm --shutdown-timeout 0.5" if="$(optenv VECTOR_HAS_TWO_KINOVA_ARMS false)"/>
 
-     <rosparam file="$(find hlpr_gazebo)/controller/two_arms_controller_jaco.yaml" command="load" if="$(optenv VECTOR_HAS_TWO_KINOVA_ARMS false)"/>
-    <node name="two_arm_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="vector/left_arm --shutdown-timeout 0.5" if="$(optenv VECTOR_HAS_TWO_KINOVA_ARMS false)"/>
+        <!-- Setup initial simulation positions -->
+        <node name="r_sim_setup" pkg="hlpr_gazebo" type="body_part_setup.py" output="screen"
+              args="vector/right_arm">
+            <!--param name="init_position" value="0, 3.14, 3.14, 0, 0, 0"/ ZERO CONFIG (straight out)-->
+            <param name="init_position" value="-1.92, 1.50, 0.53, -2.50, -2.91, 0.72"/>
+        </node>
 
-    <!-- Setup initial simulation positions -->
-    <node name="r_sim_setup" pkg="hlpr_gazebo" type="body_part_setup.py" output="screen"
-            args="vector/right_arm">
-        <!--param name="init_position" value="0, 3.14, 3.14, 0, 0, 0"/ ZERO CONFIG (straight out)-->
-        <param name="init_position" value="-1.92, 1.50, 0.53, -2.50, -2.91, 0.72"/>
-    </node>
+        <node name="l_sim_setup" pkg="hlpr_gazebo" type="body_part_setup.py" output="screen"
+              args="vector/left_arm" if="$(optenv VECTOR_HAS_TWO_KINOVA_ARMS false)">
+             <param name="init_position" value="0, 3.14, 3.14, 0, 0, 0"/> <!-- / ZERO CONFIG (straight out) -->
+            <!-- <param name="init_position" value="-1.92, 1.50, 0.53, -2.50, -2.91, 0.72"/> -->
+        </node>
 
-    <node name="l_sim_setup" pkg="hlpr_gazebo" type="body_part_setup.py" output="screen"
-            args="vector/left_arm" if="$(optenv VECTOR_HAS_TWO_KINOVA_ARMS false)">
-         <param name="init_position" value="0, 3.14, 3.14, 0, 0, 0"/> <!-- / ZERO CONFIG (straight out) -->
-        <!-- <param name="init_position" value="-1.92, 1.50, 0.53, -2.50, -2.91, 0.72"/> -->
-    </node>
 
+        <!-- Gripper -->
+        <rosparam file="$(find hlpr_gazebo)/controller/gripper_controller_robotiq.yaml" command="load" if="$(optenv VECTOR_HAS_ROBOTIQ_GRIPPER false)" />
+        <node name="gripper_controller_spawner" pkg="controller_manager" type="spawner" args="gripper_controller --shutdown-timeout 0.5" if="$(optenv VECTOR_HAS_ROBOTIQ_GRIPPER false)"/>
 
-    <!-- Gripper -->
-    <rosparam file="$(find hlpr_gazebo)/controller/gripper_controller_robotiq.yaml" command="load" if="$(optenv VECTOR_HAS_ROBOTIQ_GRIPPER false)" />
-    <node name="gripper_controller_spawner" pkg="controller_manager" type="spawner" args="gripper_controller --shutdown-timeout 0.5" if="$(optenv VECTOR_HAS_ROBOTIQ_GRIPPER false)"/>
+        <rosparam file="$(find hlpr_gazebo)/controller/two_arms_gripper_controller_robotiq.yaml" command="load" if="$(optenv VECTOR_HAS_TWO_ROBOTIQ_GRIPPERS false)" />
+        <node name="two_arms_gripper_controller_spawner" pkg="controller_manager" type="spawner" args="left_gripper_controller --shutdown-timeout 0.5" if="$(optenv VECTOR_HAS_TWO_ROBOTIQ_GRIPPERS false)"/>
+    </group>
+
+    <group if="$(optenv VECTOR_HAS_KINOVA_7DOF_ARM false)">
+        <!-- Arms: Currently only supports position control  -->
+        <rosparam file="$(find hlpr_gazebo)/controller/arm_controller_j2s7s300.yaml" command="load" if="$(optenv VECTOR_HAS_KINOVA_ARM false)"/>
+        <node name="arm_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="j2s7s300 --shutdown-timeout 0.5" if="$(optenv VECTOR_HAS_KINOVA_ARM false)"/>
 
-    <rosparam file="$(find hlpr_gazebo)/controller/two_arms_gripper_controller_robotiq.yaml" command="load" if="$(optenv VECTOR_HAS_TWO_ROBOTIQ_GRIPPERS false)" />
-    <node name="two_arms_gripper_controller_spawner" pkg="controller_manager" type="spawner" args="left_gripper_controller --shutdown-timeout 0.5" if="$(optenv VECTOR_HAS_TWO_ROBOTIQ_GRIPPERS false)"/>
+        <!-- Setup initial simulation positions -->
+        <node name="r_sim_setup" pkg="hlpr_gazebo" type="body_part_setup.py" output="screen"
+              args="j2s7s300">
+            <param name="init_position" value="0, 1.3, 0, 1.3, 4.2, 1.4, 0"/>
+        </node>
 
+        <!-- Gripper -->
+        <rosparam file="$(find hlpr_gazebo)/controller/gripper_controller_robotiq_j2s7s300.yaml" command="load" if="$(optenv VECTOR_HAS_ROBOTIQ_GRIPPER false)" />
+        <node name="gripper_controller_spawner" pkg="controller_manager" type="spawner" args="gripper_controller --shutdown-timeout 0.5" if="$(optenv VECTOR_HAS_ROBOTIQ_GRIPPER false)"/>
+    </group>
 
     <!-- Pan Tilt -->
     <rosparam file="$(find hlpr_gazebo)/controller/pan_tilt_controller.yaml" command="load" if="$(optenv VECTOR_HAS_KINECT_PAN_TILT false)"/>  
@@ -67,6 +75,6 @@
 
     <!-- Setup initial simulation positions -->
     <node name="lin_sim_setup" pkg="hlpr_gazebo" type="body_part_setup.py" output="screen"
-            args="linear_actuator_controller 0.55"/>
+            args="linear_actuator_controller 0.65"/>
 
 </launch>