From 11bfe13e8f71fd3bd3e7efbec6a36d0afca7f4fc Mon Sep 17 00:00:00 2001
From: Ethan Gordon <ekgordon@cs.washington.edu>
Date: Tue, 24 Aug 2021 17:11:44 -0700
Subject: [PATCH 1/2] Modified default.launch to be more compatible with gen3
 and 7dof arms

---
 config/{controllers.yaml => gen2_6dof.yaml} | 105 ++++++++++++++------
 include/libada/Ada.hpp                      |   2 +-
 launch/default.launch                       |  43 +++++---
 src/AdaHand.cpp                             |   2 +-
 4 files changed, 106 insertions(+), 46 deletions(-)
 rename config/{controllers.yaml => gen2_6dof.yaml} (71%)

diff --git a/config/controllers.yaml b/config/gen2_6dof.yaml
similarity index 71%
rename from config/controllers.yaml
rename to config/gen2_6dof.yaml
index bda1209..bd3b549 100644
--- a/config/controllers.yaml
+++ b/config/gen2_6dof.yaml
@@ -1,13 +1,36 @@
+# 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
+  type: pr_ros_controller/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
+  type: pr_ros_controller/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
+  type: pr_ros_controller/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 +62,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 +103,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 +139,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_controller/JointGroupVelocityController
+  joints: [j2n6s200_joint_finger_1, j2n6s200_joint_finger_2]
+
+# gripper base position controller
+hand_position_controller:
+  mode: POSITION
+  type: pr_ros_controller/JointGroupPositionController
+  joints: [j2n6s200_joint_finger_1, j2n6s200_joint_finger_2]
+
+# gripper base effort controller
+hand_effort_controller:
+  mode: EFFORT
+  type: pr_ros_controller/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}
 
 
diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp
index 5043bbc..7072588 100644
--- a/include/libada/Ada.hpp
+++ b/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;
diff --git a/launch/default.launch b/launch/default.launch
index fe90f77..8a405e0 100644
--- a/launch/default.launch
+++ b/launch/default.launch
@@ -1,32 +1,36 @@
 <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
+         stopped
+         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,13 +38,13 @@
       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"/>
@@ -48,11 +52,20 @@
     <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>
diff --git a/src/AdaHand.cpp b/src/AdaHand.cpp
index f885011..5d3d014 100644
--- a/src/AdaHand.cpp
+++ b/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,

From d060e06ffb0c72feda5ae90eb5cc8bb6cabc3482 Mon Sep 17 00:00:00 2001
From: Ethan Gordon <ekgordon@cs.washington.edu>
Date: Tue, 24 Aug 2021 18:26:18 -0700
Subject: [PATCH 2/2] More intuitive arguments in default.launch, complete
 JACO2 6DOF controller list

---
 config/gen2_6dof.yaml | 15 +++++++++------
 launch/default.launch |  8 ++++----
 2 files changed, 13 insertions(+), 10 deletions(-)

diff --git a/config/gen2_6dof.yaml b/config/gen2_6dof.yaml
index bd3b549..734e2ca 100644
--- a/config/gen2_6dof.yaml
+++ b/config/gen2_6dof.yaml
@@ -10,21 +10,24 @@ joint_state_controller:
 # whole-arm base velocity controller
 velocity_controller:
   mode: VELOCITY
-  type: pr_ros_controller/JointGroupVelocityController
+  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
-  type: pr_ros_controller/JointGroupPositionController
+  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
-  type: pr_ros_controller/JointGroupEffortController
+  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]
 
@@ -146,19 +149,19 @@ rewd_trajectory_controller:
 # gripper base velocity controller
 hand_velocity_controller:
   mode: VELOCITY
-  type: pr_ros_controller/JointGroupVelocityController
+  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_controller/JointGroupPositionController
+  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_controller/JointGroupEffortController
+  type: pr_ros_controllers/JointGroupEffortController
   joints: [j2n6s200_joint_finger_1, j2n6s200_joint_finger_2]
 
 hand_controller:
diff --git a/launch/default.launch b/launch/default.launch
index 8a405e0..87dbc34 100644
--- a/launch/default.launch
+++ b/launch/default.launch
@@ -13,7 +13,7 @@
           name="ros_control_kinova_jaco"
           type="jaco_hardware"
           output="screen"
-          if="$(eval arg('version') == '2')" />
+          if="$(eval arg('version') == 2)" />
 
    <!-- load controllers -->
    <rosparam file="$(find libada)/config/gen$(arg version)_$(arg dof)dof.yaml" command="load" />
@@ -21,16 +21,16 @@
    <node name="controller_spawner_started" pkg="controller_manager" type="spawner" respawn="false"
       output="screen"
       args="joint_state_controller" />
-   <!-- <node name="base_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
+         --stopped
          position_controller
          velocity_controller
          effort_controller
          trajectory_controller
          hand_controller
-         " /> -->
+         " />
 
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" />