Fix wrong type of interface provided if EFFORT mode is switch on.

Add proper launch files for effor in simulation.

talos_roscontrol_sot/src/roscontrol-sot-controller.cpp

@@ -31,7 +31,7 @@
     DebugFile << x << std::endl;		\
     DebugFile.close();}
 
-#define RESERDEBUG4()
+#define RESETDEBUG4()
 #define ODEBUG4FULL(x)
 #define ODEBUG4(x)
 
@@ -50,6 +50,7 @@ namespace talos_sot_controller
     simulation_mode_(false),
     control_mode_(POSITION)
   {
+    RESETDEBUG4();
   }
   
   void RCSotController::
@@ -662,7 +663,12 @@ namespace talos_sot_controller
   getHardwareInterfaceType() const
   {
     //return type_name_;
-    return talos_hardware_interface::internal::demangledTypeName<talos_hardware_interface::PositionJointInterface>();
+    if (control_mode_==POSITION)
+      return talos_hardware_interface::internal::
+	demangledTypeName<talos_hardware_interface::PositionJointInterface>();
+    else if (control_mode_==EFFORT)
+      return talos_hardware_interface::internal::
+	demangledTypeName<talos_hardware_interface::EffortJointInterface>();
   }
   
 

talos_roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml

+sot_controller:
+  libname: libsot-pyrene-controller.so
+  simulation_mode: gazebo
+  joint_names: [ leg_left_1_joint, leg_left_2_joint, leg_left_3_joint, leg_left_4_joint, leg_left_5_joint, leg_left_6_joint,
+    leg_right_1_joint, leg_right_2_joint, leg_right_3_joint, leg_right_4_joint, leg_right_5_joint, leg_right_6_joint,
+    torso_1_joint,torso_2_joint,
+    arm_left_1_joint, arm_left_2_joint, arm_left_3_joint, arm_left_4_joint, arm_left_5_joint, arm_left_6_joint, arm_left_7_joint, gripper_left_joint,
+    arm_right_1_joint, arm_right_2_joint, arm_right_3_joint, arm_right_4_joint, arm_right_5_joint, arm_right_6_joint, arm_right_7_joint, gripper_right_joint,
+    head_1_joint, head_2_joint ]
+  map_rc_to_sot_device: { motor-angles: motor-angles ,
+  joint-angles: joint-angles, velocities: velocities,
+  torques: torques, cmd-joints: control, cmd-effort: control }
+  control_mode: EFFORT 

talos_roscontrol_sot_talos/launch/sot_talos_controller_gazebo_effort.launch

+<launch>
+
+  <!-- Sot Controller configuration -->
+  <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_params_gazebo_effort.yaml"/>
+  <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_controller.yaml" />
+  
+  <!-- Spawn walking controller -->
+  <node name="sot_controller_spawner"
+        pkg="talos_controller_manager" type="spawner" output="screen"
+        args="sot_controller" />
+
+</launch>
+