From d9c0f506a8f22c651cba8f0d694b5ca6c56681bb Mon Sep 17 00:00:00 2001
From: Borrego <joaohenriqueborrego@gmail.com>
Date: Tue, 17 Apr 2018 20:17:13 +0100
Subject: [PATCH] fix hand controller and missing parameter.

Removed unimplemented controller instantiation in launcher.
These will have to be reimplemented and fine-tuned.
Added missing use_full_hand_model parameter for Vizzy simulation launcher.
---
 vizzy_control/config/vizzy_control.yaml     | 51 +++++++++++----------
 vizzy_control/launch/vizzy_control.launch   |  8 +++-
 vizzy_description/robots/vizzy.urdf.xacro   |  2 +
 vizzy_gazebo/launch/gazebo.launch           |  3 ++
 vizzy_gazebo/launch/vizzy_spawn.launch      |  2 +
 vizzy_launch/launch/vizzy_simulation.launch |  3 ++
 6 files changed, 44 insertions(+), 25 deletions(-)

diff --git a/vizzy_control/config/vizzy_control.yaml b/vizzy_control/config/vizzy_control.yaml
index 6dc675cb..12fd74ac 100644
--- a/vizzy_control/config/vizzy_control.yaml
+++ b/vizzy_control/config/vizzy_control.yaml
@@ -15,30 +15,33 @@ vizzy:
     pid: {p: 100.0, i: 0.0, d: 0.0}
 
   # Right Hand Position Controllers
-  r_min_phal_1_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: r_min_phal_1_joint
-    pid: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0.0}
-
-  r_med_phal_1_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: r_med_phal_1_joint
-    pid: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0.0}
-
-  r_ind_phal_1_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: r_ind_phal_1_joint
-    pid: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0.0}
-
-  r_thumb_phal_1_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: r_thumb_phal_1_joint
-    pid: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0.0}
-
-  r_thumb_phal_2_position_controller:
-    type: velocity_controllers/JointPositionController
-    joint: r_thumb_phal_2_joint
-    pid: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0.0}
+  #
+  # TODO: Implement controller after hand update
+  #
+  # r_min_phal_1_position_controller:
+  #   type: velocity_controllers/JointPositionController
+  #   joint: r_min_phal_1_joint
+  #   pid: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0.0}
+
+  # r_med_phal_1_position_controller:
+  #   type: velocity_controllers/JointPositionController
+  #   joint: r_med_phal_1_joint
+  #   pid: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0.0}
+
+  # r_ind_phal_1_position_controller:
+  #   type: velocity_controllers/JointPositionController
+  #   joint: r_ind_phal_1_joint
+  #   pid: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0.0}
+
+  # r_thumb_phal_1_position_controller:
+  #   type: velocity_controllers/JointPositionController
+  #   joint: r_thumb_phal_1_joint
+  #   pid: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0.0}
+
+  # r_thumb_phal_2_position_controller:
+  #   type: velocity_controllers/JointPositionController
+  #   joint: r_thumb_phal_2_joint
+  #   pid: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0.0}
 
   # Head Position Controllers
   neck_pan_position_controller:
diff --git a/vizzy_control/launch/vizzy_control.launch b/vizzy_control/launch/vizzy_control.launch
index 01116415..f201d4df 100644
--- a/vizzy_control/launch/vizzy_control.launch
+++ b/vizzy_control/launch/vizzy_control.launch
@@ -53,12 +53,18 @@
         l_eye_position_controller
         r_eye_position_controller
         left_arm_trajectory_controller
-        right_arm_trajectory_controller
+        right_arm_trajectory_controller"/>
+
+    <!--
+        TODO: Implement controller after hand update
+    -->
+    <!--    
         r_min_phal_1_position_controller
         r_med_phal_1_position_controller
         r_ind_phal_1_position_controller
         r_thumb_phal_1_position_controller
         r_thumb_phal_2_position_controller"/>
+    -->
 
   </group>
 
diff --git a/vizzy_description/robots/vizzy.urdf.xacro b/vizzy_description/robots/vizzy.urdf.xacro
index ce014239..86a61388 100644
--- a/vizzy_description/robots/vizzy.urdf.xacro
+++ b/vizzy_description/robots/vizzy.urdf.xacro
@@ -4,6 +4,8 @@
   <!-- Load Yarp plugins for real robot execution -->
   <xacro:arg name="use_yarp" default="false"/>
 
+  <!-- Load full inertial and collision model or a simplified model -->
+  <xacro:arg name="use_full_gazebo_model" default="false"/>
   <!-- Load a fully detailed hand model with properly modeled links and joints -->
   <xacro:arg name="use_full_hand_model" default="false"/>
   <!-- Disable laser sensors -->
diff --git a/vizzy_gazebo/launch/gazebo.launch b/vizzy_gazebo/launch/gazebo.launch
index 42099065..20709665 100644
--- a/vizzy_gazebo/launch/gazebo.launch
+++ b/vizzy_gazebo/launch/gazebo.launch
@@ -19,6 +19,8 @@
   <arg name="use_sim_time" default="true"/>
   <!-- Load full inertial and collision model or a simplified model -->
   <arg name="use_full_gazebo_model" default="false"/>
+  <!-- Load a fully detailed hand model with properly modeled links and joints -->
+  <arg name="use_full_hand_model" default="false"/>
 
   <!-- Map -->
   <arg name="world" default="empty.world"/>
@@ -46,6 +48,7 @@
     <arg name="use_yarp" value="$(arg use_yarp)"/>
     <arg name="urdf_file" value="$(arg urdf_file)"/>
     <arg name="use_full_gazebo_model" value="$(arg use_full_gazebo_model)"/>
+    <arg name="use_full_hand_model" value="$(arg use_full_hand_model)"/>
     <arg name="disable_laser" value="$(arg disable_laser)"/>
     <arg name="disable_3d_sensor" value="$(arg disable_3d_sensor)"/>
   </include>
diff --git a/vizzy_gazebo/launch/vizzy_spawn.launch b/vizzy_gazebo/launch/vizzy_spawn.launch
index fd427b54..0cf5bf01 100644
--- a/vizzy_gazebo/launch/vizzy_spawn.launch
+++ b/vizzy_gazebo/launch/vizzy_spawn.launch
@@ -6,6 +6,7 @@
   <arg name="urdf_file" default="vizzy.urdf.xacro"/>
   <arg name="use_yarp" default="false"/>
   <arg name="use_full_gazebo_model" default="false"/>
+  <arg name="use_full_hand_model" default="false"/>
 
   <arg name="disable_laser" default="false"/>
   <arg name="disable_3d_sensor" default="false"/>
@@ -14,6 +15,7 @@
   <param name="robot_description" command="$(find xacro)/xacro --inorder $(find vizzy_description)/robots/$(arg urdf_file)
     use_yarp:=$(arg use_yarp)
     use_full_gazebo_model:=$(arg use_full_gazebo_model)
+    use_full_hand_model:=$(arg use_full_hand_model)
     disable_laser:=$(arg disable_laser)
     disable_3d_sensor:=$(arg disable_3d_sensor)"/>
 
diff --git a/vizzy_launch/launch/vizzy_simulation.launch b/vizzy_launch/launch/vizzy_simulation.launch
index f68f8207..995ac6a4 100644
--- a/vizzy_launch/launch/vizzy_simulation.launch
+++ b/vizzy_launch/launch/vizzy_simulation.launch
@@ -31,6 +31,8 @@
   <arg name="use_sim_time" default="true"/>
   <!-- Load full inertial and collision model or a simplified model -->
   <arg name="use_full_gazebo_model" default="false"/>
+  <!-- Load a fully detailed hand model with properly modeled links and joints -->
+  <arg name="use_full_hand_model" default="false"/>
 
   <!-- rviz -->
   <arg name="rviz" default="true"/>
@@ -88,6 +90,7 @@
     <arg name="use_sim_time" value="$(arg use_sim_time)"/>
     <arg name="world" value="$(arg world)"/>
     <arg name="use_full_gazebo_model" value="$(arg use_full_gazebo_model)"/>
+    <arg name="use_full_hand_model" value="$(arg use_full_hand_model)"/>
     <arg name="disable_laser" value="$(arg disable_laser)"/>
     <arg name="disable_3d_sensor" value="$(arg disable_3d_sensor)"/>
   </include>