srh_joint_muscle_valve_controller.cpp
Go to the documentation of this file.
00001 
00029 #include "sr_mechanism_controllers/srh_joint_muscle_valve_controller.hpp"
00030 #include "angles/angles.h"
00031 #include "pluginlib/class_list_macros.h"
00032 #include <sstream>
00033 #include <math.h>
00034 #include "sr_utilities/sr_math_utils.hpp"
00035 
00036 #include <std_msgs/Float64.h>
00037 
00038 PLUGINLIB_EXPORT_CLASS( controller::SrhJointMuscleValveController, pr2_controller_interface::Controller)
00039 
00040 using namespace std;
00041 
00042 namespace controller {
00043 
00044   SrhJointMuscleValveController::SrhJointMuscleValveController()
00045     : SrController(),
00046       cmd_valve_muscle_min_(-4),
00047       cmd_valve_muscle_max_(4)
00048   {
00049   }
00050 
00051   SrhJointMuscleValveController::~SrhJointMuscleValveController()
00052   {
00053     sub_command_.shutdown();
00054   }
00055 
00056   bool SrhJointMuscleValveController::init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name)
00057   {
00058     ROS_DEBUG(" --------- ");
00059     ROS_DEBUG_STREAM("Init: " << joint_name);
00060 
00061     assert(robot);
00062     robot_ = robot;
00063     last_time_ = robot->getTime();
00064 
00065     //joint 0s
00066     if( joint_name.substr(3,1).compare("0") == 0)
00067     {
00068       has_j2 = true;
00069       std::string j1 = joint_name.substr(0,3) + "1";
00070       std::string j2 = joint_name.substr(0,3) + "2";
00071       ROS_DEBUG_STREAM("Joint 0: " << j1 << " " << j2);
00072 
00073       joint_state_ = robot_->getJointState(j1);
00074       if (!joint_state_)
00075       {
00076         ROS_ERROR("SrhJointMuscleValveController could not find joint named \"%s\"\n",
00077                   j1.c_str());
00078         return false;
00079       }
00080 
00081       joint_state_2 = robot_->getJointState(j2);
00082       if (!joint_state_2)
00083       {
00084         ROS_ERROR("SrhJointMuscleValveController could not find joint named \"%s\"\n",
00085                   j2.c_str());
00086         return false;
00087       }
00088     }
00089     else
00090     {
00091       has_j2 = false;
00092       joint_state_ = robot_->getJointState(joint_name);
00093       if (!joint_state_)
00094       {
00095         ROS_ERROR("SrhJointPositionController could not find joint named \"%s\"\n",
00096                   joint_name.c_str());
00097         return false;
00098       }
00099     }
00100 
00101     friction_compensator = boost::shared_ptr<sr_friction_compensation::SrFrictionCompensator>(new sr_friction_compensation::SrFrictionCompensator(joint_name));
00102 
00103     //serve_set_gains_ = node_.advertiseService("set_gains", &SrhJointMuscleValveController::setGains, this);
00104     serve_reset_gains_ = node_.advertiseService("reset_gains", &SrhJointMuscleValveController::resetGains, this);
00105 
00106     ROS_DEBUG_STREAM(" joint_state name: " << joint_state_->joint_->name);
00107     ROS_DEBUG_STREAM(" In Init: " << getJointName() << " This: " << this
00108                      << " joint_state: "<<joint_state_ );
00109     //We do not call this function, as we want to listen to a different topic type
00110     //after_init();
00111     sub_command_ = node_.subscribe<sr_robot_msgs::JointMuscleValveControllerCommand>("command", 1, &SrhJointMuscleValveController::setCommandCB, this);
00112     return true;
00113   }
00114 
00115   bool SrhJointMuscleValveController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00116   {
00117     assert(robot);
00118     node_ = n;
00119 
00120     std::string joint_name;
00121     if (!node_.getParam("joint", joint_name)) {
00122       ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00123       return false;
00124     }
00125 
00126     controller_state_publisher_.reset(
00127       new realtime_tools::RealtimePublisher<sr_robot_msgs::JointMuscleValveControllerState>
00128       (node_, "state", 1));
00129 
00130     return init(robot, joint_name);
00131   }
00132 
00133 
00134   void SrhJointMuscleValveController::starting()
00135   {
00136     command_ = 0.0;
00137     read_parameters();
00138   }
00139 /*
00140   bool SrhJointMuscleValveController::setGains(sr_robot_msgs::SetEffortControllerGains::Request &req,
00141                                           sr_robot_msgs::SetEffortControllerGains::Response &resp)
00142   {
00143     max_force_demand = req.max_force;
00144     friction_deadband = req.friction_deadband;
00145 
00146     //Setting the new parameters in the parameter server
00147     node_.setParam("max_force", max_force_demand);
00148     node_.setParam("friction_deadband", friction_deadband);
00149 
00150     return true;
00151   }
00152 */
00153   bool SrhJointMuscleValveController::resetGains(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp)
00154   {
00155     command_ = 0.0;
00156 
00157     read_parameters();
00158 
00159     if( has_j2 )
00160       ROS_WARN_STREAM("Reseting controller gains: " << joint_state_->joint_->name << " and " << joint_state_2->joint_->name);
00161     else
00162       ROS_WARN_STREAM("Reseting controller gains: " << joint_state_->joint_->name);
00163 
00164     return true;
00165   }
00166 
00167   void SrhJointMuscleValveController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
00168   {
00169   }
00170 
00171   void SrhJointMuscleValveController::update()
00172   {
00173     //The valve commands can have values between -4 and 4
00174     int8_t valve[2];
00175     unsigned int i;
00176 
00177 //    if( !has_j2)
00178 //    {
00179 //      if (!joint_state_->calibrated_)
00180 //        return;
00181 //    }
00182 
00183     assert(robot_ != NULL);
00184     ros::Time time = robot_->getTime();
00185     assert(joint_state_->joint_);
00186     dt_= time - last_time_;
00187 
00188     if (!initialized_)
00189     {
00190       initialized_ = true;
00191       cmd_valve_muscle_[0] = 0.0;
00192       cmd_valve_muscle_[1] = 0.0;
00193       cmd_duration_ms_[0] = 0;
00194       cmd_duration_ms_[1] = 0;
00195       current_duration_ms_[0] = cmd_duration_ms_[0];
00196       current_duration_ms_[1] = cmd_duration_ms_[1];
00197     }
00198 
00199 
00200     //IGNORE the following  lines if we don't want to use the pressure sensors data
00201     //We don't want to define a modified version of JointState, as that would imply using a modified version of robot.h, controller manager,
00202     //ethercat_hardware and pr2_etherCAT main loop
00203     // So we heve encoded the two uint16 that contain the data from the muscle pressure sensors into the double measured_effort_. (We don't
00204     // have any measured effort in the muscle hand anyway).
00205     // Here we extract the pressure values from joint_state_->measured_effort_ and decode that back into uint16.
00206     double pressure_0_tmp = fmod(joint_state_->measured_effort_, 0x10000);
00207     double pressure_1_tmp = (fmod(joint_state_->measured_effort_, 0x100000000) - pressure_0_tmp) / 0x10000;
00208     uint16_t pressure_0 = static_cast<uint16_t>(pressure_0_tmp + 0.5);
00209     uint16_t pressure_1 = static_cast<uint16_t>(pressure_1_tmp + 0.5);
00210 
00211     //****************************************
00212 
00213 
00214 
00215 
00216 
00217     //************************************************
00218     // Here goes the control algorithm
00219 
00220     // This controller will allow the user to specify a separate command for each of the two muscles that control the joint.
00221     // The user will also specify a duration in ms for that command. During this duration the command will be sent to the hand
00222     // every ms (every cycle of this 1Khz control loop).
00223     //Once this duration period has elapsed, a command of 0 will be sent to the muscle (meaning both the filling and emptying valves for that
00224     // muscle remain closed)
00225     // A duration of 0 means that there is no timeout, so the valve command will be sent to the muscle until a different valve command is received
00226     // BE CAREFUL WHEN USING A DURATION OF 0 AS THIS COULD EVENTUALLY DAMAGE THE MUSCLE
00227 
00228     for(i=0;i<2;++i)
00229     {
00230       if (cmd_duration_ms_[i] == 0) // if the commanded duration is 0 it means that it will not timeout
00231       {
00232         // So we will use the last commanded valve command
00233         valve[i] = cmd_valve_muscle_[i];
00234       }
00235       else
00236       {
00237         if(current_duration_ms_[i] > 0) // If the command has not timed out yet
00238         {
00239           // we will use the last commanded valve command
00240           valve[i] = cmd_valve_muscle_[i];
00241           // and decrement the counter. This is a milliseconds counter, and this loop is running once every millisecond
00242           current_duration_ms_[i]--;
00243         }
00244         else // If the command has already timed out
00245         {
00246           // we will use 0 to close the valves
00247           valve[i] = 0;
00248         }
00249       }
00250     }
00251 
00252 
00253     //************************************************
00254 
00255 
00256 
00257 
00258 
00259 
00260 
00261     //************************************************
00262     // After doing any computation we consider, we encode the obtained valve commands into joint_state_->commanded_effort_
00263     //We don't want to define a modified version of JointState, as that would imply using a modified version of robot.h, controller manager,
00264     //ethercat_hardware and pr2_etherCAT main loop
00265     // So the controller encodes the two int8 (that are in fact int4) that contain the valve commands into the double commanded_effort_. (We don't
00266     // have any real commanded_effort_ in the muscle hand anyway).
00267 
00268     uint16_t valve_tmp[2];
00269     for(i=0;i<2;++i)
00270     {
00271       //Check that the limits of the valve command are not exceded
00272       if (valve[i] > 4)
00273         valve[i] = 4;
00274       if (valve[i] < -4)
00275         valve[i] = -4;
00276       //encode
00277       if (valve[i] < 0)
00278         valve_tmp[i] = -valve[i] + 8;
00279       else
00280         valve_tmp[i] = valve[i];
00281     }
00282 
00283     //We encode the valve 0 command in the lowest "half byte" i.e. the lowest 16 integer values in the double var (see decoding in simple_transmission_for_muscle.cpp)
00284     //the valve 1 command is envoded in the next 4 bits
00285     joint_state_->commanded_effort_ = static_cast<double>(valve_tmp[0]) + static_cast<double>(valve_tmp[1] << 4);
00286 
00287     //*******************************************************************************
00288 
00289 
00290 
00291 
00292     if(loop_count_ % 10 == 0)
00293     {
00294       if(controller_state_publisher_ && controller_state_publisher_->trylock())
00295       {
00296         controller_state_publisher_->msg_.header.stamp = time;
00297 
00298         controller_state_publisher_->msg_.set_valve_muscle_0 = cmd_valve_muscle_[0];
00299         controller_state_publisher_->msg_.set_valve_muscle_1 = cmd_valve_muscle_[1];
00300         controller_state_publisher_->msg_.set_duration_muscle_0 = cmd_duration_ms_[0];
00301         controller_state_publisher_->msg_.set_duration_muscle_1 = cmd_duration_ms_[1];
00302         controller_state_publisher_->msg_.current_valve_muscle_0 = valve[0];
00303         controller_state_publisher_->msg_.current_valve_muscle_1 = valve[1];
00304         controller_state_publisher_->msg_.current_duration_muscle_0 = current_duration_ms_[0];
00305         controller_state_publisher_->msg_.current_duration_muscle_1 = current_duration_ms_[1];
00306         controller_state_publisher_->msg_.packed_valve = joint_state_->commanded_effort_;
00307         controller_state_publisher_->msg_.muscle_pressure_0 = pressure_0;
00308         controller_state_publisher_->msg_.muscle_pressure_1 = pressure_1;
00309         controller_state_publisher_->msg_.time_step = dt_.toSec();
00310 
00311         controller_state_publisher_->unlockAndPublish();
00312       }
00313     }
00314     loop_count_++;
00315 
00316     last_time_ = time;
00317   }
00318 
00319   void SrhJointMuscleValveController::read_parameters()
00320   {
00321 
00322   }
00323 
00324   void SrhJointMuscleValveController::setCommandCB(const sr_robot_msgs::JointMuscleValveControllerCommandConstPtr& msg)
00325   {
00326     cmd_valve_muscle_[0] = clamp_command(msg->cmd_valve_muscle[0]);
00327     cmd_valve_muscle_[1] = clamp_command(msg->cmd_valve_muscle[1]);
00328     //These variables hold the commanded duration
00329     cmd_duration_ms_[0] = static_cast<unsigned int>(msg->cmd_duration_ms[0]);
00330     cmd_duration_ms_[1] = static_cast<unsigned int>(msg->cmd_duration_ms[1]);
00331     //These are the actual counters that we will decrement
00332     current_duration_ms_[0] = cmd_duration_ms_[0];
00333     current_duration_ms_[1] = cmd_duration_ms_[1];
00334   }
00335 
00337   int8_t SrhJointMuscleValveController::clamp_command( int8_t cmd )
00338   {
00339     if(cmd < cmd_valve_muscle_min_)
00340       return cmd_valve_muscle_min_;
00341 
00342     if(cmd > cmd_valve_muscle_max_)
00343       return cmd_valve_muscle_max_;
00344 
00345     return cmd;
00346   }
00347 }
00348 
00349 /* For the emacs weenies in the crowd.
00350 Local Variables:
00351    c-basic-offset: 2
00352 End:
00353 */


sr_mechanism_controllers
Author(s): Ugo Cupcic
autogenerated on Fri Aug 28 2015 13:09:56