Add more intuitive arguments to default.launch (#60)

* Modified default.launch to be more compatible with gen3 and 7dof arms

* More intuitive arguments in default.launch, complete JACO2 6DOF controller list

config/controllers.yaml → config/gen2_6dof.yaml

@@ -1,13 +1,39 @@
+# hardware parameters
 soft_limits:
   eff: [16,16,16,10,10,10,1.3,1.3]
 
-# whole-arm joint_state publisher
+# arm + gripper joint_state publisher
 joint_state_controller:
   type: joint_state_controller/JointStateController
   publish_rate: 50
 
-# trajectory controller
+# whole-arm base velocity controller
+velocity_controller:
+  mode: VELOCITY
+  mode_handle: joint_mode
+  type: pr_ros_controllers/JointGroupVelocityController
+  joints: [j2n6s200_joint_1, j2n6s200_joint_2, j2n6s200_joint_3,
+    j2n6s200_joint_4, j2n6s200_joint_5, j2n6s200_joint_6]
+
+# whole-arm base position controller
+position_controller:
+  mode: POSITION
+  mode_handle: joint_mode
+  type: pr_ros_controllers/JointGroupPositionController
+  joints: [j2n6s200_joint_1, j2n6s200_joint_2, j2n6s200_joint_3,
+    j2n6s200_joint_4, j2n6s200_joint_5, j2n6s200_joint_6]
+
+# whole-arm base effort controller
+effort_controller:
+  mode: EFFORT
+  mode_handle: joint_mode
+  type: pr_ros_controllers/JointGroupEffortController
+  joints: [j2n6s200_joint_1, j2n6s200_joint_2, j2n6s200_joint_3,
+    j2n6s200_joint_4, j2n6s200_joint_5, j2n6s200_joint_6]
+
+# whole-arm base trajectory controller
 trajectory_controller:
+  mode: TRAJECTORY
   type: velocity_controllers/JointTrajectoryController
   joints: [j2n6s200_joint_1, j2n6s200_joint_2, j2n6s200_joint_3,
     j2n6s200_joint_4, j2n6s200_joint_5, j2n6s200_joint_6]
@@ -39,15 +65,22 @@ trajectory_controller:
     j2n6s200_joint_5: {p: 3,  d: 0, i: 0, i_clamp: 1}
     j2n6s200_joint_6: {p: 3,  d: 0, i: 0, i_clamp: 1}
 
-# Optional controller that might be useful for testing.
-# Should do the same thing as trajectory_controller, but this is 
-# the lab's rewd_controllers implementation
-rewd_trajectory_controller:
-  type: rewd_controllers/JointTrajectoryController
+# whole-arm base trajectory controller
+move_until_touch_topic_controller:
+  mode: TRAJECTORY
+  type: rewd_controllers/MoveUntilTouchTopicController
+  # Forque Sensor
+  forcetorque_wrench_name: /forque/forqueSensor
+  forcetorque_tare_name: /forque/bias_controller/trigger
+  # FingerVision
+  # forcetorque_wrench_name: /fingervision/fv_3_l/wrench
+  # forcetorque_tare_name: /fingervision/fingerVisionTaring
+  sensor_force_limit: 50
+  sensor_torque_limit: 5
   joints: [j2n6s200_joint_1, j2n6s200_joint_2, j2n6s200_joint_3,
     j2n6s200_joint_4, j2n6s200_joint_5, j2n6s200_joint_6]
+  control_type: velocity
   constraints:
-    stopped_velocity_tolerance: 1.0
     j2n6s200_joint_1:
       goal: 0.02
       trajectory: 0.05
@@ -73,31 +106,17 @@ rewd_trajectory_controller:
     j2n6s200_joint_4: {p: 3,  d: 0, i: 0, i_clamp: 1}
     j2n6s200_joint_5: {p: 3,  d: 0, i: 0, i_clamp: 1}
     j2n6s200_joint_6: {p: 3,  d: 0, i: 0, i_clamp: 1}
-  control_type: velocity
-
-j2n6s200_hand_controller:
-  type: velocity_controllers/JointTrajectoryController
-  joints: [j2n6s200_joint_finger_1, j2n6s200_joint_finger_2]
-  constraints:
-    stopped_velocity_tolerance: 1.0
-  gains: # Required because we're controlling a velocity interface
-    j2n6s200_joint_finger_1: {p: 1,  d: 0, i: 0, i_clamp: 1}
-    j2n6s200_joint_finger_2: {p: 1,  d: 0, i: 0, i_clamp: 1}
 
-move_until_touch_topic_controller:
-  type: rewd_controllers/MoveUntilTouchTopicController
-  # Forque Sensor
-  forcetorque_wrench_name: /forque/forqueSensor
-  forcetorque_tare_name: /forque/bias_controller/trigger
-  # FingerVision
-  # forcetorque_wrench_name: /fingervision/fv_3_l/wrench
-  # forcetorque_tare_name: /fingervision/fingerVisionTaring
-  sensor_force_limit: 50
-  sensor_torque_limit: 5
+# Optional controller that might be useful for testing.
+# Should do the same thing as trajectory_controller, but this is 
+# the lab's rewd_controllers implementation
+rewd_trajectory_controller:
+  mode: TRAJECTORY
+  type: rewd_controllers/JointTrajectoryController
   joints: [j2n6s200_joint_1, j2n6s200_joint_2, j2n6s200_joint_3,
     j2n6s200_joint_4, j2n6s200_joint_5, j2n6s200_joint_6]
-  control_type: velocity
   constraints:
+    stopped_velocity_tolerance: 1.0
     j2n6s200_joint_1:
       goal: 0.02
       trajectory: 0.05
@@ -123,5 +142,36 @@ move_until_touch_topic_controller:
     j2n6s200_joint_4: {p: 3,  d: 0, i: 0, i_clamp: 1}
     j2n6s200_joint_5: {p: 3,  d: 0, i: 0, i_clamp: 1}
     j2n6s200_joint_6: {p: 3,  d: 0, i: 0, i_clamp: 1}
+  control_type: velocity
+
+##############################################
+
+# gripper base velocity controller
+hand_velocity_controller:
+  mode: VELOCITY
+  type: pr_ros_controllers/JointGroupVelocityController
+  joints: [j2n6s200_joint_finger_1, j2n6s200_joint_finger_2]
+
+# gripper base position controller
+hand_position_controller:
+  mode: POSITION
+  type: pr_ros_controllers/JointGroupPositionController
+  joints: [j2n6s200_joint_finger_1, j2n6s200_joint_finger_2]
+
+# gripper base effort controller
+hand_effort_controller:
+  mode: EFFORT
+  type: pr_ros_controllers/JointGroupEffortController
+  joints: [j2n6s200_joint_finger_1, j2n6s200_joint_finger_2]
+
+hand_controller:
+  mode: TRAJECTORY
+  type: velocity_controllers/JointTrajectoryController
+  joints: [j2n6s200_joint_finger_1, j2n6s200_joint_finger_2]
+  constraints:
+    stopped_velocity_tolerance: 1.0
+  gains: # Required because we're controlling a velocity interface
+    j2n6s200_joint_finger_1: {p: 1,  d: 0, i: 0, i_clamp: 1}
+    j2n6s200_joint_finger_2: {p: 1,  d: 0, i: 0, i_clamp: 1}
 
 

include/libada/Ada.hpp

@@ -404,7 +404,7 @@ class Ada final : public aikido::robot::Robot
 
   // Names of the trajectory executors
   std::string mArmTrajectoryExecutorName;
-  const std::string mHandTrajectoryExecutorName = "j2n6s200_hand_controller";
+  const std::string mHandTrajectoryExecutorName = "hand_controller";
 
   // Collision resolution.
   double mCollisionResolution;

launch/default.launch

@@ -1,31 +1,35 @@
 <launch>
 
-  <arg name="feeding" default="false" doc="Boolean flag of whether to set up the robot for the feeding demo"/>
-  <arg name="perception" default="true" doc="Boolean flag of whether to set up the real perception for the feeding demo"/>
+  <arg name="use_forque" default="false" doc="Add forque to URDF and spawned controllers."/>
+  <arg name="perception" default="false" doc="Boolean flag of whether to set up the perception on ADA" />
   <!-- Other options: retinanet, spnet, spanet -->
   <!-- See possible networks in https://github.com/personalrobotics/food_detector/ -->
-  <arg name="detector" default="retinanet" doc="Which network to use for food perception."/>
+  <arg name="detector" default="false" doc="Which network to use for food perception. Set to 'false' for simulation."/>
+  <arg name="version" default="2" doc="Kinova Arm Version (JACO = 2, Kortex = 3)" />
+  <arg name="dof" default="6" doc="Number of degrees of freedom in arm" />
 
    <!-- the controller node itself -->
    <node pkg="jaco_hardware"
           name="ros_control_kinova_jaco"
           type="jaco_hardware"
-          output="screen" />
+          output="screen"
+          if="$(eval arg('version') == 2)" />
 
    <!-- load controllers -->
-   <rosparam file="$(find libada)/config/controllers.yaml" command="load" />
+   <rosparam file="$(find libada)/config/gen$(arg version)_$(arg dof)dof.yaml" command="load" />
 
    <node name="controller_spawner_started" pkg="controller_manager" type="spawner" respawn="false"
       output="screen"
       args="joint_state_controller" />
-   <node name="controller_spawner_stopped" pkg="controller_manager" type="spawner" respawn="false"
+   <node name="base_controller_spawner_stopped" pkg="controller_manager" type="spawner" respawn="false"
       output="screen"
       args="
          --stopped
-         move_until_touch_topic_controller
+         position_controller
+         velocity_controller
+         effort_controller
          trajectory_controller
-         rewd_trajectory_controller
-         j2n6s200_hand_controller
+         hand_controller
          " />
 
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" />
@@ -34,25 +38,34 @@
       args="0 0 0 0 0 0 1 map world 10"/>
 
 
-  <group unless="$(arg feeding)">
+  <group unless="$(arg use_forque)">
     <!-- load ada urdf -->
     <param name="robot_description"
           command="cat $(find ada_description)/robots_urdf/ada.urdf"/>
   </group>
 
-  <group if="$(arg feeding)">
+  <group if="$(arg use_forque)">
     <!-- load ada urdf -->
     <param name="robot_description"
           command="cat $(find ada_description)/robots_urdf/ada_with_camera_forque.urdf"/>
 
     <node name="st_joule2camera" pkg="tf" type="static_transform_publisher" respawn="false" output="screen"
           args="0.09909 0.0181061 0.0178372 2.8905 3.13479 1.59224 j2n6s200_joule camera_link 10"/>
 
+    <node name="forque_controller_spawner_stopped" pkg="controller_manager" type="spawner" respawn="false"
+      output="screen"
+      args="
+         --stopped
+         move_until_touch_topic_controller
+         " />
+  </group>
+
+  <group if="$(arg perception)">
     <!-- start sim/real perception node-->
     <node pkg="food_detector" type="run_perception_module.py" name="food_detector"
-          args="--demo-type $(arg detector)" output="screen" cwd="node" if="$(arg perception)"/>
+          args="--demo-type $(arg detector)" output="screen" cwd="node" unless="$(eval arg('detector') == 'false' )"/>
     <node pkg="food_detector" type="run_sim_food_detector.py" name="food_detector"
-          output="screen" cwd="node" unless="$(arg perception)"/>
+          output="screen" cwd="node" if="$(eval arg('detector') == 'false' )"/>
   </group>
 
 </launch>

src/AdaHand.cpp

@@ -419,7 +419,7 @@ AdaHand::createTrajectoryExecutor(const dart::dynamics::SkeletonPtr& robot)
   else
   {
     // TODO (k):need to check trajectory_controller exists?
-    std::string serverName = "j2n6s200_hand_controller/follow_joint_trajectory";
+    std::string serverName = "hand_controller/follow_joint_trajectory";
     return std::make_shared<RosTrajectoryExecutor>(
         *mNode,
         serverName,