[talos_roscontrol_sot/talos_roscontrol_sot_talos] First successfull test of...

[talos_roscontrol_sot/talos_roscontrol_sot_talos] First successfull test of current control on the robot.

.gitmodules

 [submodule "talos_roscontrol_sot/cmake"]
 	path = talos_roscontrol_sot/cmake
-	url = git@github.com:jrl-umi3218/jrl-cmakemodules.git
+	url = https://github.com/jrl-umi3218/jrl-cmakemodules.git

talos_roscontrol_sot/CMakeLists.txt

@@ -16,6 +16,9 @@
 # along with this program.  If not, see <http://www.gnu.org/licenses/>.
 cmake_minimum_required(VERSION 2.8.3)
 
+# Authorize warning error.
+SET(CXX_DISABLE_WERROR)
+
 include(cmake/base.cmake)
 include(cmake/ros.cmake)
 include(cmake/GNUInstallDirs.cmake)
@@ -41,7 +44,7 @@ find_package(catkin REQUIRED COMPONENTS
   control_msgs
   sensor_msgs
   realtime_tools
-  talos_controller_interface
+  controller_interface
 )
 
 ## LAAS cmake submodule part

talos_roscontrol_sot/config/roscontrol_sot_controller_plugins.xml

 <library path="lib/libtalos_rcsot_controller">
 
-  <class name="talos_sot_controller/RCSotController" type="talos_sot_controller::RCSotController" base_class_type="talos_controller_interface::ControllerBase">
+  <class name="talos_sot_controller/RCSotController" type="talos_sot_controller::RCSotController" base_class_type="controller_interface::ControllerBase">
     <description>
        The roscontrol SotController generates whole body motions for your robot (following the talos interface).
     </description>

talos_roscontrol_sot/package.xml

@@ -30,7 +30,7 @@
   <build_depend>control_msgs</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>realtime_tools</build_depend>
-  <build_depend>talos_controller_interface</build_depend>
+  <build_depend>controller_interface</build_depend>
   <build_depend>cmake_modules</build_depend>
   <build_depend>dynamic_graph_bridge</build_depend>
 
@@ -40,12 +40,12 @@
   <run_depend>realtime_tools</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>std_msgs</run_depend>
-  <run_depend>talos_controller_interface</run_depend>
+  <run_depend>controller_interface</run_depend>
   <run_depend>cmake_modules</run_depend>
   <run_depend>message_runtime</run_depend>
   <run_depend>dynamic_graph_bridge</run_depend>
   <!-- The export tag contains other, unspecified, tags -->
   <export>
-    <talos_controller_interface plugin="${prefix}/config/roscontrol_sot_controller_plugins.xml"/>
+    <controller_interface plugin="${prefix}/config/roscontrol_sot_controller_plugins.xml"/>
   </export>
 </package>

talos_roscontrol_sot/src/roscontrol-sot-controller.cpp

@@ -35,7 +35,7 @@
 #define ODEBUG4FULL(x)
 #define ODEBUG4(x)
 
-using namespace talos_hardware_interface;
+using namespace hardware_interface;
 using namespace rc_sot_system;
 
 namespace talos_sot_controller  
@@ -68,7 +68,7 @@ namespace talos_sot_controller
   }
 
   bool RCSotController::
-  initRequest (talos_hardware_interface::RobotHW * robot_hw, 
+  initRequest (hardware_interface::RobotHW * robot_hw, 
 	       ros::NodeHandle &robot_nh,
 	       ros::NodeHandle &controller_nh,
 	       std::set<std::string> & claimed_resources)
@@ -91,7 +91,7 @@ namespace talos_sot_controller
   }
   
   bool RCSotController::
-  initInterfaces(talos_hardware_interface::RobotHW * robot_hw,
+  initInterfaces(hardware_interface::RobotHW * robot_hw,
 		 ros::NodeHandle &,
 		 ros::NodeHandle &,
 		 std::set<std::string> & claimed_resources)
@@ -107,7 +107,7 @@ namespace talos_sot_controller
     if (! pos_iface_)
       {
 	ROS_ERROR("This controller requires a hardware interface of type '%s'."
-		  " Make sure this is registered in the talos_hardware_interface::RobotHW class.",
+		  " Make sure this is registered in the hardware_interface::RobotHW class.",
 		  getHardwareInterfaceType().c_str());
 	return false ;
       }
@@ -117,7 +117,7 @@ namespace talos_sot_controller
     if (! effort_iface_)
       {
 	ROS_ERROR("This controller requires a hardware interface of type '%s'."
-		  " Make sure this is registered in the talos_hardware_interface::RobotHW class.",
+		  " Make sure this is registered in the hardware_interface::RobotHW class.",
 		  getHardwareInterfaceType().c_str());
 	    return false ;
       }
@@ -127,7 +127,7 @@ namespace talos_sot_controller
     if (! ft_iface_ )
       {
 	ROS_ERROR("This controller requires a hardware interface of type '%s '. " 
-		  " Make sure this is registered inthe talos_hardware_interface::RobotHW class.",
+		  " Make sure this is registered inthe hardware_interface::RobotHW class.",
 		  internal :: demangledTypeName<ForceTorqueSensorInterface>().c_str());
 	return false ;
       }
@@ -136,7 +136,7 @@ namespace talos_sot_controller
     if (! imu_iface_)
       {
 	ROS_ERROR("This controller requires a hardware interface of type '%s'."
-		  " Make sure this is registered in the talos_hardware_interface::RobotHW class.",
+		  " Make sure this is registered in the thardware_interface::RobotHW class.",
 		  internal :: demangledTypeName<ImuSensorInterface>().c_str());
 	return false ;
       }
@@ -149,7 +149,7 @@ namespace talos_sot_controller
 	if (!act_temp_iface_)
 	  {
 	    ROS_ERROR("This controller requires a hardware interface of type '%s'."
-		      " Make sure this is registered in the talos_hardware_interface::RobotHW class.",
+		      " Make sure this is registered in the hardware_interface::RobotHW class.",
 		      internal :: demangledTypeName<ActuatorTemperatureSensorInterface>().c_str());
 	    return false ;	  
 	  }
@@ -491,6 +491,8 @@ namespace talos_sot_controller
     fillSensorsIn(ltitle,DataOneIter_.velocities);
     ltitle = "torques";
     fillSensorsIn(ltitle,DataOneIter_.torques);
+    ltitle = "currents";
+    fillSensorsIn(ltitle,DataOneIter_.motor_currents);
     
   }
 
@@ -623,7 +625,6 @@ namespace talos_sot_controller
 
   void RCSotController::one_iteration()
   {
-    
     /// Update the sensors.
     fillSensors();
 
@@ -667,14 +668,16 @@ namespace talos_sot_controller
   {
     //return type_name_;
     if (control_mode_==POSITION)
-      return talos_hardware_interface::internal::
-	demangledTypeName<talos_hardware_interface::PositionJointInterface>();
+      return hardware_interface::internal::
+	demangledTypeName<hardware_interface::PositionJointInterface>();
     else if (control_mode_==EFFORT)
-      return talos_hardware_interface::internal::
-	demangledTypeName<talos_hardware_interface::EffortJointInterface>();
+      return hardware_interface::internal::
+	demangledTypeName<hardware_interface::EffortJointInterface>();
+    std::string voidstring("");
+    return voidstring;
   }
   
 
   PLUGINLIB_EXPORT_CLASS(talos_sot_controller::RCSotController, 
-			 talos_controller_interface::ControllerBase);
+			 controller_interface::ControllerBase);
 }

talos_roscontrol_sot/src/roscontrol-sot-controller.hh

@@ -35,11 +35,11 @@
 #include <string>
 #include <map>
 
-#include <talos_controller_interface/controller.h>
-#include <talos_hardware_interface/joint_command_interface.h>
-#include <talos_hardware_interface/imu_sensor_interface.h>
-#include <talos_hardware_interface/force_torque_sensor_interface.h>
-#include <talos_pal_hardware_interfaces/actuator_temperature_interface.h>
+#include <controller_interface/controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/imu_sensor_interface.h>
+#include <hardware_interface/force_torque_sensor_interface.h>
+#include <pal_hardware_interfaces/actuator_temperature_interface.h>
 
 #include <dynamic_graph_bridge/sot_loader_basic.hh>
 
@@ -53,7 +53,7 @@ namespace talos_sot_controller
      This class encapsulates the Stack of Tasks inside the ros-control infra-structure.
      
    */
-  class RCSotController : public talos_controller_interface::ControllerBase,
+  class RCSotController : public controller_interface::ControllerBase,
 			       SotLoaderBasic
   {
     
@@ -68,33 +68,33 @@ namespace talos_sot_controller
     /// @{ \name Ros-control related fields
     
     /// \brief Vector of joint handles.
-    std::vector<talos_hardware_interface::JointHandle> joints_;
+    std::vector<hardware_interface::JointHandle> joints_;
     std::vector<std::string> joints_name_;
 
     /// \brief Vector towards the IMU.
-    std::vector<talos_hardware_interface::ImuSensorHandle> imu_sensor_;
+    std::vector<hardware_interface::ImuSensorHandle> imu_sensor_;
 
     /// \brief Vector of 6D force sensor.
-    std::vector<talos_hardware_interface::ForceTorqueSensorHandle> ft_sensors_;
+    std::vector<hardware_interface::ForceTorqueSensorHandle> ft_sensors_;
     
     /// \brief Vector of temperature sensors for the actuators.
-    std::vector<talos_hardware_interface::ActuatorTemperatureSensorHandle> 
+    std::vector<hardware_interface::ActuatorTemperatureSensorHandle> 
     act_temp_sensors_;
     
     /// \brief Interface to the joints controlled in position.
-    talos_hardware_interface::PositionJointInterface * pos_iface_;
+    hardware_interface::PositionJointInterface * pos_iface_;
 
     /// \brief Interface to the joints controlled in force.
-    talos_hardware_interface::EffortJointInterface * effort_iface_;
+    hardware_interface::EffortJointInterface * effort_iface_;
     
     /// \brief Interface to the sensors (IMU).
-    talos_hardware_interface::ImuSensorInterface* imu_iface_;
+    hardware_interface::ImuSensorInterface* imu_iface_;
 
     /// \brief Interface to the sensors (Force).
-    talos_hardware_interface::ForceTorqueSensorInterface* ft_iface_;
+    hardware_interface::ForceTorqueSensorInterface* ft_iface_;
     
     /// \brief Interface to the actuator temperature sensor.
-    talos_hardware_interface::ActuatorTemperatureSensorInterface  * act_temp_iface_;
+    hardware_interface::ActuatorTemperatureSensorInterface  * act_temp_iface_;
 
     /// @}
 
@@ -127,7 +127,7 @@ namespace talos_sot_controller
 
     /// \brief Read the configuration files, 
     /// claims the request to the robot and initialize the Stack-Of-Tasks.
-    bool initRequest (talos_hardware_interface::RobotHW * robot_hw, 
+    bool initRequest (hardware_interface::RobotHW * robot_hw, 
 		      ros::NodeHandle &robot_nh,
 		      ros::NodeHandle &controller_nh,
 		      std::set<std::string> & claimed_resources);
@@ -150,7 +150,7 @@ namespace talos_sot_controller
 
   protected:
     /// Initialize the roscontrol interfaces
-    bool initInterfaces(talos_hardware_interface::RobotHW * robot_hw,
+    bool initInterfaces(hardware_interface::RobotHW * robot_hw,
 			ros::NodeHandle &,
 			ros::NodeHandle &,
 			std::set<std::string> & claimed_resources);

talos_roscontrol_sot_talos/config/sot_talos_params_effort.yaml

+sot_controller:
+  libname: libsot-pyrene-controller.so
+  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.launch

@@ -6,7 +6,7 @@
   
   <!-- Spawn walking controller -->
   <node name="sot_controller_spawner"
-        pkg="talos_controller_manager" type="spawner" output="screen"
+        pkg="controller_manager" type="spawner" output="screen"
         args="sot_controller" />
 
 </launch>

talos_roscontrol_sot_talos/launch/sot_talos_controller_effort.launch

+<launch>
+
+  <!-- Sot Controller configuration -->
+  <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_params_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="controller_manager" type="spawner" output="screen"
+        args="sot_controller" />
+
+</launch>
+