Make sure that the robot is not falling when switching to effort control mode.

talos_roscontrol_sot/CMakeLists.txt

@@ -24,7 +24,7 @@ include(cmake/ros.cmake)
 include(cmake/GNUInstallDirs.cmake)
 include(cmake/python.cmake)
 
-project(talos_roscontrol_sot)
+#project(talos_roscontrol_sot)
 
 find_package(PkgConfig REQUIRED)
 

talos_roscontrol_sot/src/roscontrol-sot-controller.cpp

@@ -41,6 +41,18 @@ using namespace rc_sot_system;
 namespace talos_sot_controller  
 {
   typedef std::map<std::string,std::string>::iterator it_map_rt_to_sot;
+
+  EffortControlPDMotorControlData::EffortControlPDMotorControlData()
+  {
+    prev = 0.0; vel_prev = 0.0; des_pos=0.0;
+    integ_err=0.0;
+  }
+  
+  void EffortControlPDMotorControlData::read_from_xmlrpc_value
+  (const std::string &prefix)
+  {
+    pid_controller.initParam(prefix);
+  }
   
   RCSotController::
   RCSotController():
@@ -234,19 +246,61 @@ namespace talos_sot_controller
     return true;
   }
 
-
+  bool RCSotController::
+  readParamsEffortControlPDMotorControlData(ros::NodeHandle &robot_nh)
+  {
+    // Read libname
+    if (robot_nh.hasParam("/sot_controller/effort_control_pd_motor_init/gains"))
+      {
+	XmlRpc::XmlRpcValue xml_rpc_ecpd_init;
+	robot_nh.getParamCached("/sot_controller/effort_control_pd_motor_init/gains",
+				xml_rpc_ecpd_init);
+	
+	ROS_INFO("/sot_controller/effort_control_pd_motor_init/gains: %d %d %d\n",
+		 xml_rpc_ecpd_init.getType(),XmlRpc::XmlRpcValue::TypeArray,XmlRpc::XmlRpcValue::TypeStruct);
+	effort_mode_pd_motors_.clear();
+	
+	for (int i=0;i<joints_name_.size();i++)
+	  {
+	    if (xml_rpc_ecpd_init.hasMember(joints_name_[i]))
+	      {
+		/*
+		XmlRpc::XmlRpcValue &aDataSet= xml_rpc_ecpd_init[joints_name_[i]];
+		ROS_INFO("/sot_controller/effort_control_pd_motor_init %s type: %d\n",joints_name_[i],aDataSet.getType());
+		if (aDataSet.getType()!=XmlRpc::XmlRpcValue::TypeStruct)
+		  {
+		    ROS_ERROR("In /sot_controller/effort_control_pd_motor_init/gains/%s  not a struct"
+			      ,joints_name_[i]);
+		    throw XmlrpcHelperException("Pb in readParamsEffortControlPDMotorControlData");
+		  }
+		*/
+		std::string prefix= "/sot_controller/effort_control_pd_motor_init/gains/" + joints_name_[i];
+		effort_mode_pd_motors_[joints_name_[i]].read_from_xmlrpc_value(prefix);
+	      }
+	    else
+	      ROS_INFO("joint %s not in /sot_controller/effort_control_pd_motor_init/gains\n",
+		       joints_name_[i]);
+	  }
+	return true;
+      }
+    
+    ROS_ERROR("No parameter /sot_controller/effort_controler_pd_motor_init");
+    return false;
+  }
+  
   bool RCSotController::
   readParamsFromRCToSotDevice(ros::NodeHandle &robot_nh)
   {
     // Read libname
     if (robot_nh.hasParam("/sot_controller/map_rc_to_sot_device")) 
       {
-	if (robot_nh.getParam("/sot_controller/map_rc_to_sot_device",mapFromRCToSotDevice))
+	if (robot_nh.getParam("/sot_controller/map_rc_to_sot_device",
+			      mapFromRCToSotDevice_))
 	  {
 	    /// TODO: Check if the mapping is complete wrt to the interface and the mapping.
 	    ROS_INFO_STREAM("Loading map rc to sot device: ");
-	    for (it_map_rt_to_sot it = mapFromRCToSotDevice.begin(); 
-		 it != mapFromRCToSotDevice.end(); ++it) 
+	    for (it_map_rt_to_sot it = mapFromRCToSotDevice_.begin(); 
+		 it != mapFromRCToSotDevice_.end(); ++it) 
 	      ROS_INFO_STREAM( it->first << ", " << it->second);
 	  }
 	else
@@ -347,6 +401,9 @@ namespace talos_sot_controller
     /// Calls readParamsFromRCToSotDevice
     // Mapping from ros-controll to sot device
     readParamsFromRCToSotDevice(robot_nh);
+
+    if (control_mode_==EFFORT)
+      readParamsEffortControlPDMotorControlData(robot_nh);
     return true;
   }
 
@@ -455,9 +512,9 @@ namespace talos_sot_controller
   {
     /// Tries to find the mapping from the local validation
     /// to the SoT device.
-    it_map_rt_to_sot it_mapRC2Sot= mapFromRCToSotDevice.find(title);
+    it_map_rt_to_sot it_mapRC2Sot= mapFromRCToSotDevice_.find(title);
     /// If the mapping is found
-    if (it_mapRC2Sot!=mapFromRCToSotDevice.end())
+    if (it_mapRC2Sot!=mapFromRCToSotDevice_.end())
       {
 	/// Expose the data to the SoT device.
 	std::string lmapRC2Sot = it_mapRC2Sot->second;
@@ -608,8 +665,8 @@ namespace talos_sot_controller
     else if (control_mode_==EFFORT)
       cmdTitle="cmd-effort";
 
-    it_map_rt_to_sot it_mapRC2Sot= mapFromRCToSotDevice.find(cmdTitle);
-    if (it_mapRC2Sot!=mapFromRCToSotDevice.end())
+    it_map_rt_to_sot it_mapRC2Sot= mapFromRCToSotDevice_.find(cmdTitle);
+    if (it_mapRC2Sot!=mapFromRCToSotDevice_.end())
       {
 	std::string lmapRC2Sot = it_mapRC2Sot->second;
 	command_ = controlValues[lmapRC2Sot].getValues();
@@ -643,10 +700,46 @@ namespace talos_sot_controller
   }
 
   void RCSotController::
-  update(const ros::Time&, const ros::Duration& )
+  update(const ros::Time&, const ros::Duration& period)
   {
+    // Do not send any control if the dynamic graph is not started
     if (!isDynamicGraphStopped())
       one_iteration();
+    else
+      // But in effort mode it means that we are sending 0
+      // Therefore implements a default PD controller on the system.
+      if (control_mode_==EFFORT)
+	{
+	  // ROS_INFO("Compute command for effort mode: %d %d",joints_.size(),effort_mode_pd_motors_.size());
+	  for(unsigned int idJoint=0;idJoint<joints_.size();idJoint++)
+	    {
+	      std::string joint_name = joints_name_[idJoint];
+	      std::map<std::string,EffortControlPDMotorControlData>::iterator
+		search_ecpd = effort_mode_pd_motors_.find(joint_name);
+	      
+	      if (search_ecpd!=effort_mode_pd_motors_.end())
+		{
+		  EffortControlPDMotorControlData & ecpdcdata =
+		    search_ecpd->second;
+		  
+		  double vel_err = 0 - joints_[idJoint].getVelocity();
+		  double err = ecpdcdata.des_pos - joints_[idJoint].getPosition();
+		  
+		  ecpdcdata.integ_err +=err;
+
+		  double local_command = ecpdcdata.pid_controller.computeCommand(err,vel_err,period);
+		  // Apply command
+		  control_toolbox::Pid::Gains gains = ecpdcdata.pid_controller.getGains();
+		  //ROS_INFO("command: %d %s %f %f (%f %f %f)",idJoint, joints_name_[idJoint].c_str(),
+		  //local_command, DataOneIter_.motor_angle[idJoint],
+		  //gains.p_gain_,gains.d_gain_, gains.i_gain_);
+		  joints_[idJoint].setCommand(local_command);
+		  
+		  // Update previous value.
+		  ecpdcdata.prev = DataOneIter_.motor_angle[idJoint];
+		}
+	    }
+	}
   }
   
   void RCSotController::

talos_roscontrol_sot/src/roscontrol-sot-controller.hh

@@ -44,11 +44,35 @@
 #include <dynamic_graph_bridge/sot_loader_basic.hh>
 
 #include "log.hh"
+#include <ros/ros.h>
+#include <control_toolbox/pid.h>
 
 namespace talos_sot_controller 
 {
   enum SotControlMode { POSITION, EFFORT};
 
+  class XmlrpcHelperException : public ros::Exception
+  {
+  public:
+    XmlrpcHelperException(const std::string& what)
+      : ros::Exception(what) {}
+  };
+
+  
+  struct EffortControlPDMotorControlData
+  {
+    control_toolbox::Pid pid_controller;
+
+    //double p_gain,d_gain,i_gain;
+    double prev;
+    double vel_prev;
+    double des_pos;
+    double integ_err;
+
+    EffortControlPDMotorControlData();
+    //    void read_from_xmlrpc_value(XmlRpc::XmlRpcValue &aXRV);
+    void read_from_xmlrpc_value(const std::string &prefix);
+  };
   /**
      This class encapsulates the Stack of Tasks inside the ros-control infra-structure.
      
@@ -109,7 +133,11 @@ namespace talos_sot_controller
 
     /// \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_;
+
     /// \brief Map from ros-control quantities to robot device
     /// ros-control quantities are for the sensors:
     /// * motor-angles
@@ -119,7 +147,7 @@ namespace talos_sot_controller
     /// ros-control quantities for control are:
     /// * joints
     /// * torques
-    std::map<std::string,std::string> mapFromRCToSotDevice;
+    std::map<std::string,std::string> mapFromRCToSotDevice_;
 
   public :
 
@@ -182,6 +210,9 @@ namespace talos_sot_controller
     /// \brief Read the control mode.
     bool readParamsControlMode(ros::NodeHandle & robot_nh);
 
+    /// \brief Read the PID information of the robot in effort mode.
+    bool readParamsEffortControlPDMotorControlData(ros::NodeHandle &robot_nh);
+
     ///@}
 
     /// \brief Fill the SoT map structures

talos_roscontrol_sot_talos/config/sot_talos_params_effort.yaml

@@ -9,4 +9,117 @@ sot_controller:
   map_rc_to_sot_device: { motor-angles: motor-angles ,
   joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
   torques: torques, cmd-joints: control, cmd-effort: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
+    effort_control_pd_motor_init:
+	- name: leg_left_1_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: leg_left_2_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: leg_left_3_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: leg_left_4_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: leg_left_5_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: leg_left_6_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0	  
+	- name: leg_right_1_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: leg_right_2_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: leg_right_3_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: leg_right_4_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: leg_right_5_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: leg_right_6_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0	  
+	- name: torso_1_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: torso_2_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: arm_left_1_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: arm_left_2_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: arm_left_3_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: arm_left_4_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: arm_left_5_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: arm_left_6_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: arm_left_7_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0	  
+	- name: arm_right_1_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: arm_right_2_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: arm_right_3_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: arm_right_4_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: arm_right_5_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0
+	- name: arm_right_6_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0	  
+	- name: arm_right_7_joint
+	  p_gain: 1000.0
+	  d_gain: 1000.0
+	  des_pos: 0.0	  
   control_mode: EFFORT 

talos_roscontrol_sot_talos/launch/sot_talos_controller_gazebo_effort.launch

@@ -2,10 +2,12 @@
 
   <!-- Sot Controller configuration -->
   <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_params_gazebo_effort.yaml"/>
+  <rosparam command="load" ns="/sot_controller/effort_control_pd_motor_init" file="$(find talos_hardware_gazebo)/config/pids.yaml"/>
   <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_controller.yaml" />
   
   <!-- Spawn walking controller -->
-  <node name="sot_controller_spawner"
+  <node launch-prefix="xterm -e python -m pdb"
+  	name="sot_controller_spawner"
         pkg="talos_controller_manager" type="spawner" output="screen"
         args="sot_controller" />