Merge branch 'devel' into master

CMakeLists.txt

@@ -20,13 +20,13 @@ find_package(PkgConfig REQUIRED)
 add_required_dependency(bullet)
 add_required_dependency("urdfdom")
 
-#set(bullet_FOUND 0)
-#pkg_check_modules(bullet REQUIRED bullet)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
+  pal_hardware_interfaces
+  controller_interface  
   roscpp
   rospy
   std_msgs
@@ -34,8 +34,6 @@ find_package(catkin REQUIRED COMPONENTS
   control_msgs
   sensor_msgs
   realtime_tools
-  controller_interface
-  pal_hardware_interfaces
 )
 
 ## LAAS cmake submodule part
@@ -57,12 +55,12 @@ set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${LIBRARY_OUTPUT_PATH}")
 
 # Add dependency through jrl-cmakemodules to compile
 # this code without catkin_make
+add_required_dependency("pal_hardware_interfaces")
+add_optional_dependency("temperature_sensor_controller")
 add_required_dependency(roscpp)
 add_required_dependency("realtime_tools >= 1.8")
 add_required_dependency("dynamic_graph_bridge")
 add_required_dependency("controller_interface")
-add_required_dependency("pal_hardware_interfaces")
-add_optional_dependency("temperature_sensor_controller")
 add_required_dependency("pal_common_msgs")
 add_required_dependency("dynamic-graph")
 
@@ -113,8 +111,8 @@ target_link_libraries(rcsot_controller
   ${bullet_LIBRARIES}
   )
 
-pkg_config_use_dependency(rcsot_controller urdfdom)
-pkg_config_use_dependency(rcsot_controller dynamic-graph)
+pkg_config_use_dependency(rcsot_controller urdfdom optional NO_INCLUDE_SYSTEM)
+pkg_config_use_dependency(rcsot_controller dynamic-graph optional NO_INCLUDE_SYSTEM)
 
 ## Mark executables and/or libraries for installation
 install(TARGETS rcsot_controller DESTINATION lib )

cmake @ f34901f1

-Subproject commit 5c8c19f491f2c6f8488f5f37ff81d711d69dbb3f
+Subproject commit f34901f143d843b48dfdb8d9e904503ed96e2310

package.xml

 <?xml version="1.0"?>
 <package>
   <name>roscontrol_sot</name>
-  <version>0.0.4</version>
+  <version>0.0.8</version>
   <description>Wrapping the Stack-of-tasks framework in ros-control</description>
 
   <!-- Maintainer email --> 

src/log.cpp

@@ -9,7 +9,6 @@
 #include <fstream>
 #include <iomanip>
 
-#include<ros/console.h>
 
 using namespace std;
 using namespace rc_sot_system;
@@ -31,6 +30,7 @@ void DataToLog::init(ProfileLog &aProfileLog)
   gyrometer.resize(3*aProfileLog.length);
   force_sensors.resize(aProfileLog.nbForceSensors*6*aProfileLog.length);
   temperatures.resize(aProfileLog.nbDofs*aProfileLog.length);
+  controls.resize(aProfileLog.nbDofs*aProfileLog.length);
   timestamp.resize(aProfileLog.length);
   duration.resize(aProfileLog.length);
 
@@ -80,13 +80,15 @@ void Log::record(DataToLog &aDataToLog)
 	StoredData_.motor_currents[JointID+lref_]= aDataToLog.motor_currents[JointID];
       if (aDataToLog.temperatures.size()>JointID)
 	StoredData_.temperatures[JointID+lref_]= aDataToLog.temperatures[JointID];
+      if (aDataToLog.controls.size()>JointID)
+	StoredData_.controls[JointID+lref_]= aDataToLog.controls[JointID];
     }
   for(unsigned int axis=0;axis<3;axis++)
     {
       StoredData_.accelerometer[lrefts_*3+axis] = aDataToLog.accelerometer[axis];
       StoredData_.gyrometer[lrefts_*3+axis] = aDataToLog.gyrometer[axis];
     }
-  unsigned width_pad= 6 * profileLog_.nbForceSensors;
+  std::size_t width_pad= 6 * profileLog_.nbForceSensors;
   
   for(unsigned int fsID=0;fsID<profileLog_.nbForceSensors;fsID++)
     {
@@ -158,31 +160,34 @@ void Log::save(std::string &fileName)
   suffix = "-temperatures.log";
   saveVector(fileName,suffix,StoredData_.temperatures, profileLog_.nbDofs);
 
+  suffix = "-controls.log";
+  saveVector(fileName,suffix,StoredData_.controls, profileLog_.nbDofs);
+
   suffix = "-duration.log";
   saveVector(fileName,suffix,StoredData_.duration, 1);
 
 }
 
 inline void writeHeaderToBinaryBuffer (ofstream& of,
-    const unsigned int& nVector,
-    const unsigned int& vectorSize)
+				       const std::size_t& nVector,
+				       const std::size_t& vectorSize)
 {
-  of.write ((char*)&nVector   , sizeof(unsigned int));
-  of.write ((char*)&vectorSize, sizeof(unsigned int));
+  of.write ((const char*)(&nVector)   , sizeof(std::size_t));
+  of.write ((const char*)(&vectorSize), sizeof(std::size_t));
 }
 
 inline void writeToBinaryFile (ofstream& of,
     const double& t, const double& dt,
     const std::vector<double>& data, const std::size_t& idx, const std::size_t& size)
 {
-  of.write ((char*)&t          ,       sizeof(double));
-  of.write ((char*)&dt         ,       sizeof(double));
-  of.write ((char*)(&data[idx]), size*(sizeof(double)));
+  of.write ((const char*)&t          ,       sizeof(double));
+  of.write ((const char*)&dt         ,       sizeof(double));
+  of.write ((const char*)(&data[idx]), size*(sizeof(double)));
 }
 
 void Log::saveVector(std::string &fileName,std::string &suffix,
 		     const std::vector<double> &avector,
-		     unsigned int size)
+		     std::size_t size)
 {
   ostringstream oss;
   oss << fileName;

src/log.hh

@@ -14,9 +14,9 @@ namespace rc_sot_system {
 
   struct ProfileLog
   {
-    unsigned int nbDofs;
-    unsigned int nbForceSensors;
-    unsigned int length;
+    std::size_t nbDofs;
+    std::size_t nbForceSensors;
+    std::size_t length;
   };
   
   struct DataToLog
@@ -41,6 +41,8 @@ namespace rc_sot_system {
     std::vector<double> motor_currents;
     // Measured temperatures
     std::vector<double> temperatures;
+    // Control
+    std::vector<double> controls;
 
     // Timestamp
     std::vector<double> timestamp;
@@ -51,9 +53,9 @@ namespace rc_sot_system {
     
     DataToLog();
     void init(ProfileLog &aProfileLog);
-    unsigned int nbDofs() { return profileLog_.nbDofs;}
-    unsigned int nbForceSensors() { return profileLog_.nbForceSensors;}
-    unsigned int length() { return profileLog_.length;}
+    std::size_t nbDofs() { return profileLog_.nbDofs;}
+    std::size_t nbForceSensors() { return profileLog_.nbForceSensors;}
+    std::size_t length() { return profileLog_.length;}
 
   };
 
@@ -80,7 +82,7 @@ namespace rc_sot_system {
     void saveVector(std::string &filename, 
 		    std::string &suffix,
 		    const std::vector<double> &avector,
-		    unsigned int);
+		    std::size_t);
 
   public:
   
@@ -95,4 +97,10 @@ namespace rc_sot_system {
 
   };
 }
+
+#pragma GCC diagnostic push
+#pragma GCC system_header
+#include<ros/console.h>
+#pragma GCC diagnostic pop
+
 #endif /* _RC_SOT_SYSTEM_LOG_H_ */

src/roscontrol-sot-controller.hh

@@ -8,11 +8,15 @@
 #include <string>
 #include <map>
 
+#pragma GCC diagnostic push
+#pragma GCC system_header
 #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 <pluginlib/class_list_macros.h>
+#pragma GCC diagnostic pop
 
 #include <dynamic_graph_bridge/sot_loader_basic.hh>
 #include <ros/ros.h>
@@ -26,7 +30,9 @@
 
 namespace sot_controller
 {
-  enum SotControlMode { POSITION, EFFORT};
+  enum ControlMode { POSITION, VELOCITY, EFFORT};
+  namespace lhi = hardware_interface;
+  namespace lci = controller_interface;
 
   class XmlrpcHelperException : public ros::Exception
   {
@@ -36,21 +42,27 @@ namespace sot_controller
   };
 
 
-  struct EffortControlPDMotorControlData
+  struct ControlPDMotorControlData
   {
     control_toolbox::Pid pid_controller;
 
-    //double p_gain,d_gain,i_gain;
-    double prev;
-    double vel_prev;
-    double des_pos;
-    double integ_err;
-
-    EffortControlPDMotorControlData();
+    ControlPDMotorControlData();
     //    void read_from_xmlrpc_value(XmlRpc::XmlRpcValue &aXRV);
     void read_from_xmlrpc_value(const std::string &prefix);
   };
 
+  struct JointSotHandle
+  {
+    lhi::JointHandle joint;
+    double desired_init_pose;
+    // This should not be handled in roscontrol_sot package. The control type
+    // should be handled in SoT directly, by externalizing the integration from
+    // the Device.
+    //ControlMode sot_control_mode;
+    ControlMode ros_control_mode;
+  };
+
+  typedef std::map<std::string,JointSotHandle>::iterator it_joint_sot_h;
 #ifndef CONTROLLER_INTERFACE_KINETIC
   typedef std::set<std::string> ClaimedResources;
 #endif
@@ -58,9 +70,6 @@ namespace sot_controller
      This class encapsulates the Stack of Tasks inside the ros-control infra-structure.
 
    */
-  namespace lhi = hardware_interface;
-  namespace lci = controller_interface;
-
   class RCSotController : public lci::ControllerBase,
 			       SotLoaderBasic
   {
@@ -76,7 +85,7 @@ namespace sot_controller
     /// @{ \name Ros-control related fields
 
     /// \brief Vector of joint handles.
-    std::vector<lhi::JointHandle> joints_;
+    std::map<std::string,JointSotHandle> joints_;
     std::vector<std::string> joints_name_;
 
     /// \brief Vector towards the IMU.
@@ -94,6 +103,9 @@ namespace sot_controller
     /// \brief Interface to the joints controlled in position.
     lhi::PositionJointInterface * pos_iface_;
 
+    /// \brief Interface to the joints controlled in position.
+    lhi::VelocityJointInterface * vel_iface_;
+
     /// \brief Interface to the joints controlled in force.
     lhi::EffortJointInterface * effort_iface_;
 
@@ -118,15 +130,13 @@ namespace sot_controller
     /// \brief Adapt the interface to Gazebo simulation
     bool simulation_mode_;
 
-    /// \brief The robot can controlled in effort or position mode (default).
-    SotControlMode control_mode_;
-
     /// \brief Implement a PD controller for the robot when the dynamic graph
     /// is not on.
-    std::map<std::string, EffortControlPDMotorControlData> effort_mode_pd_motors_;
+    std::map<std::string, ControlPDMotorControlData> effort_mode_pd_motors_;
 
-    /// \brief Give the desired position when the dynamic graph is not on.
-    std::vector<double> desired_init_pose_;
+    /// \brief Implement a PD controller for the robot when the dynamic graph
+    /// is not on.
+    std::map<std::string, ControlPDMotorControlData> velocity_mode_pd_motors_;
 
     /// \brief Map from ros-control quantities to robot device
     /// ros-control quantities are for the sensors:
@@ -179,8 +189,6 @@ namespace sot_controller
     void starting(const ros::Time&);
     /// \brief Stopping the control
     void stopping(const ros::Time&);
-    /// \brief Display the kind of hardware interface that this controller is using.
-    virtual std::string getHardwareInterfaceType() const;
 
   protected:
     /// Initialize the roscontrol interfaces
@@ -220,6 +228,9 @@ namespace sot_controller
     /// \brief Read the control mode.
     bool readParamsControlMode(ros::NodeHandle & robot_nh);
 
+    /// \brief Read the PID information of the robot in velocity mode.
+    bool readParamsVelocityControlPDMotorControlData(ros::NodeHandle &robot_nh);
+
     /// \brief Read the PID information of the robot in effort mode.
     bool readParamsEffortControlPDMotorControlData(ros::NodeHandle &robot_nh);
 
@@ -259,6 +270,8 @@ namespace sot_controller
     ///@{ Control the robot while waiting for the SoT
     /// Default control in effort.
     void localStandbyEffortControlMode(const ros::Duration& period);
+    /// Default control in velocity.
+    void localStandbyVelocityControlMode(const ros::Duration& period);
     /// Default control in position.
     void localStandbyPositionControlMode();
 
@@ -285,6 +298,13 @@ namespace sot_controller
 
     /// Read URDF model from /robot_description parameter.
     bool readUrdf(ros::NodeHandle &robot_nh);
+
+    /// Returns control mode by reading rosparam.
+    /// It reads /sot_controller/control_mode/joint_name
+    /// and check 
+    bool
+    getJointControlMode(std::string &joint_name,
+			JointSotHandle &aJointSotHandle);
   };
 }
 

src/roscontrol-sot-parse-log.cc

 #include <fstream>
 #include <iostream>
 #include <iomanip>
+#include <cassert>
+#include <cstring>
+
+void usage (char* bin)
+{
+  std::cerr << "Usage: " << bin << " [--separator sep] binary_file_name\n";
+}
 
 int main (int argc, char* argv[])
 {
-  if (argc != 2) {
-    std::cerr << "Usage: " << argv[0] << " binary_file_name\n";
+  if (argc < 2) {
+    usage(argv[0]);
     return 1;
   }
+  int iarg = 1;
+  std::string sep (" ");
+  while (iarg + 1 < argc - 1) {
+    if (strcmp(argv[iarg], "--separator") == 0) {
+      sep = argv[iarg+1];
+      iarg += 2;
+    } else {
+      usage(argv[0]);
+      return 1;
+    }
+  }
+  assert (iarg == argc-1);
 
-  std::ifstream in (argv[1], std::ios::binary);
+  std::ifstream in (argv[iarg], std::ios::binary);
   if (!in.is_open() || !in.good()) {
-    std::cerr << "Couldn't open file " << argv[1] << '\n';
+    std::cerr << "Couldn't open file " << argv[iarg] << '\n';
     return 2;
   }
 
   // Read headers
-  unsigned int nVector = 0, vectorSize = 0;
-  in.read ((char*)&nVector   , sizeof(unsigned int));
-  in.read ((char*)&vectorSize, sizeof(unsigned int));
+  std::size_t nVector = 0, vectorSize = 0;
+  in.read ((char*)&nVector   , sizeof(std::size_t));
+  in.read ((char*)&vectorSize, sizeof(std::size_t));
   if (!in.good()) {
-    std::cerr << "Couldn't parse file: " << argv[1] << '\n';
+    std::cerr << "Couldn't parse file: " << argv[iarg] << '\n';
     return 3;
   }
 
@@ -33,10 +52,10 @@ int main (int argc, char* argv[])
       in.read ((char*)&v, sizeof(double));
       if (!in.good()) {
         std::cerr << "Stopped to parse at (" << i << ',' << j
-          << ") of file: " << argv[1] << '\n';
+          << ") of file: " << argv[iarg] << '\n';
         return 4;
       }
-      std::cout << v << ' ';
+      std::cout << v << sep;
     }
     std::cout << '\n';
   }