Merge lp:~shadowrobot/sr-ros-interface-ethercat/Feature_update_controller_params into lp:sr-ros-interface-ethercat

[Toni Oliver]

Update params in param server when changing controller parameters.

Update params in param server when changing controller parameters.

[markpitchless (markpitchless)]

http://reviewboard.shadow.local/r/107/

=== modified file 'sr_robot_lib/include/sr_robot_lib/sr_hand_lib.hpp'
--- sr_robot_lib/include/sr_robot_lib/sr_hand_lib.hpp	2012-08-20 09:38:26 +0000
+++ sr_robot_lib/include/sr_robot_lib/sr_hand_lib.hpp	2012-11-08 11:55:23 +0000
@@ -100,6 +100,31 @@
                             std::vector<shadow_joints::JointToSensor> joint_to_sensors,
                             std::vector<sr_actuator::SrActuator*> actuators);
 
+    /**
+     * Updates the parameter values for the force control in the Parameter Server
+     *
+     * @param joint_name The name of the joint.
+     * @param max_pwm The max pwm the motor will apply
+     * @param sg_left Strain gauge left
+     * @param sg_right Strain gauge right
+     * @param f The feedforward term (directly adds f*error to the output of the PID)
+     * @param p The p value.
+     * @param i the i value.
+     * @param d the d value.
+     * @param imax the imax value.
+     * @param deadband the deadband on the force.
+     * @param sign can be 0 or 1 depending on the way the motor is plugged in.
+     */
+    void update_force_control_in_param_server(std::string joint_name, int max_pwm, int sg_left, int sg_right, int f, int p,
+                                                       int i, int d, int imax, int deadband, int sign);
+
+    /**
+     * Finds the joint name for a certain motor index
+     *
+     * @param motor_index The integer motor index
+     */
+    std::string find_joint_name(int motor_index);
+
   private:
     ros::NodeHandle nodehandle_;
 
 
=== modified file 'sr_robot_lib/src/sr_hand_lib.cpp'
--- sr_robot_lib/src/sr_hand_lib.cpp	2012-08-20 09:38:26 +0000
+++ sr_robot_lib/src/sr_hand_lib.cpp	2012-11-08 11:55:23 +0000
@@ -283,10 +283,10 @@
     full_param << "/" << act_name << "/pid/max_pwm";
     nodehandle_.param<int>(full_param.str(), max_pwm, 0);
     full_param.str("");
-    full_param << "/" << act_name << "/pid/sg_left";
+    full_param << "/" << act_name << "/pid/sgleftref";
     nodehandle_.param<int>(full_param.str(), sg_left, 0);
     full_param.str("");
-    full_param << "/" << act_name << "/pid/sg_right";
+    full_param << "/" << act_name << "/pid/sgrightref";
     nodehandle_.param<int>(full_param.str(), sg_right, 0);
     full_param.str("");
     full_param << "/" << act_name << "/pid/deadband";
@@ -432,6 +432,10 @@
     generate_force_control_config( motor_index, request.maxpwm, request.sgleftref,
                                    request.sgrightref, request.f, request.p, request.i,
                                    request.d, request.imax, request.deadband, request.sign );
+
+    update_force_control_in_param_server( find_joint_name(motor_index), request.maxpwm, request.sgleftref,
+                                     request.sgrightref, request.f, request.p, request.i,
+                                     request.d, request.imax, request.deadband, request.sign);
     response.configured = true;
 
     //Reinitialize motors information
@@ -440,6 +444,61 @@
     return true;
   }
 
+  std::string SrHandLib::find_joint_name(int motor_index)
+  {
+    for( boost::ptr_vector<shadow_joints::Joint>::iterator joint = joints_vector.begin();
+        joint != joints_vector.end(); ++joint )
+    {
+      if( !boost::is_null(joint) ) // check for validity
+      {
+        if(joint->motor->motor_id == motor_index)
+          return joint->joint_name;
+      }
+    }
+    ROS_ERROR("Could not find joint name for motor index: %d", motor_index);
+    return "";
+  }
+
+  void SrHandLib::update_force_control_in_param_server(std::string joint_name, int max_pwm, int sg_left, int sg_right, int f, int p,
+                                                     int i, int d, int imax, int deadband, int sign)
+  {
+    if(joint_name != "")
+    {
+      std::stringstream full_param;
+      std::string act_name = boost::to_lower_copy(joint_name);
+
+      full_param << "/" << act_name << "/pid/f";
+      nodehandle_.setParam(full_param.str(), f);
+      full_param.str("");
+      full_param << "/" << act_name << "/pid/p";
+      nodehandle_.setParam(full_param.str(), p);
+      full_param.str("");
+      full_param << "/" << act_name << "/pid/i";
+      nodehandle_.setParam(full_param.str(), i);
+      full_param.str("");
+      full_param << "/" << act_name << "/pid/d";
+      nodehandle_.setParam(full_param.str(), d);
+      full_param.str("");
+      full_param << "/" << act_name << "/pid/imax";
+      nodehandle_.setParam(full_param.str(), imax);
+      full_param.str("");
+      full_param << "/" << act_name << "/pid/max_pwm";
+      nodehandle_.setParam(full_param.str(), max_pwm);
+      full_param.str("");
+      full_param << "/" << act_name << "/pid/sgleftref";
+      nodehandle_.setParam(full_param.str(), sg_left);
+      full_param.str("");
+      full_param << "/" << act_name << "/pid/sgrightref";
+      nodehandle_.setParam(full_param.str(), sg_right);
+      full_param.str("");
+      full_param << "/" << act_name << "/pid/deadband";
+      nodehandle_.setParam(full_param.str(), deadband);
+      full_param.str("");
+      full_param << "/" << act_name << "/pid/sign";
+      nodehandle_.setParam(full_param.str(), sign);
+    }
+  }
+
 
   std::vector<shadow_joints::JointToSensor> SrHandLib::read_joint_to_sensor_mapping()
   {
 
=== modified file 'sr_robot_lib/src/sr_robot_lib.cpp'
--- sr_robot_lib/src/sr_robot_lib.cpp	2012-10-29 14:18:42 +0000
+++ sr_robot_lib/src/sr_robot_lib.cpp	2012-11-08 11:55:23 +0000
@@ -934,7 +934,7 @@
                                                  int i, int d, int imax, int deadband, int sign)
   {
     ROS_INFO_STREAM(
-      "Setting new pid values for motor" << motor_index << ": max_pwm="<< max_pwm <<" sg_left=" << sg_left << " sg_right=" << sg_right << " f=" << f << " p=" << p << " i=" << i << " d="<< d << " imax=" << imax << " deadband="<< deadband << " sign=" << sign);
+      "Setting new pid values for motor" << motor_index << ": max_pwm="<< max_pwm <<" sgleftref=" << sg_left << " sgrightref=" << sg_right << " f=" << f << " p=" << p << " i=" << i << " d="<< d << " imax=" << imax << " deadband="<< deadband << " sign=" << sign);
 
     //the vector is of the size of the TO_MOTOR_DATA_TYPE enum.
     //the value of the element at a given index is the value