single_joint_controller.h
Go to the documentation of this file.
00001 /*
00002     Copyright (c) 2011, Antons Rebguns <email>
00003     All rights reserved.
00004 
00005     Redistribution and use in source and binary forms, with or without
00006     modification, are permitted provided that the following conditions are met:
00007         * Redistributions of source code must retain the above copyright
00008         notice, this list of conditions and the following disclaimer.
00009         * Redistributions in binary form must reproduce the above copyright
00010         notice, this list of conditions and the following disclaimer in the
00011         documentation and/or other materials provided with the distribution.
00012         * Neither the name of the <organization> nor the
00013         names of its contributors may be used to endorse or promote products
00014         derived from this software without specific prior written permission.
00015 
00016     THIS SOFTWARE IS PROVIDED BY Antons Rebguns <email> ''AS IS'' AND ANY
00017     EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018     WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019     DISCLAIMED. IN NO EVENT SHALL Antons Rebguns <email> BE LIABLE FOR ANY
00020     DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021     (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022     LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023     ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024     (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025     SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 
00029 #ifndef DYNAMIXEL_HARDWARE_INTERFACE_SINGLE_JOINT_CONTROLLER_H
00030 #define DYNAMIXEL_HARDWARE_INTERFACE_SINGLE_JOINT_CONTROLLER_H
00031 
00032 #include <map>
00033 #include <string>
00034 #include <cmath>
00035 
00036 #include <dynamixel_hardware_interface/dynamixel_const.h>
00037 #include <dynamixel_hardware_interface/dynamixel_io.h>
00038 #include <dynamixel_hardware_interface/JointState.h>
00039 #include <dynamixel_hardware_interface/MotorStateList.h>
00040 #include <dynamixel_hardware_interface/SetVelocity.h>
00041 #include <dynamixel_hardware_interface/TorqueEnable.h>
00042 #include <dynamixel_hardware_interface/SetTorqueLimit.h>
00043 #include <dynamixel_hardware_interface/SetComplianceMargin.h>
00044 #include <dynamixel_hardware_interface/SetComplianceSlope.h>
00045 
00046 #include <ros/ros.h>
00047 #include <std_msgs/Float64.h>
00048 #include <std_srvs/Empty.h>
00049 
00050 namespace controller
00051 {
00052 
00053 class SingleJointController
00054 {
00055 public:
00056     SingleJointController() {};
00057     virtual ~SingleJointController() {};
00058     
00059     virtual bool initialize(std::string name,
00060                             std::string port_namespace,
00061                             dynamixel_hardware_interface::DynamixelIO* dxl_io)
00062     {
00063         name_ = name;
00064         port_namespace_ = port_namespace;
00065         dxl_io_ = dxl_io;
00066         
00067         try
00068         {
00069             c_nh_ = ros::NodeHandle(nh_, name_);
00070         }
00071         catch(std::exception &e)
00072         {
00073             ROS_ERROR("Exception thrown while constructing nodehandle for controller with name '%s':\n%s",
00074                       name_.c_str(), e.what());
00075             return false;
00076         }
00077         catch(...)
00078         {
00079             ROS_ERROR("Exception thrown while constructing nodehandle for controller with name '%s'",
00080                       name_.c_str());
00081             return false;
00082         }
00083 
00084         if (!c_nh_.getParam("joint", joint_))
00085         {
00086             ROS_ERROR("%s: joint name is not specified", name_.c_str());
00087             return false;
00088         }
00089         
00090         joint_state_.name = joint_;
00091         XmlRpc::XmlRpcValue raw_motor_list;
00092         
00093         if (!c_nh_.getParam("motors", raw_motor_list))
00094         {
00095             ROS_ERROR("%s: no motors configuration present", name.c_str());
00096             return false;
00097         }
00098         
00099         if (raw_motor_list.getType() != XmlRpc::XmlRpcValue::TypeArray)
00100         {
00101             ROS_ERROR("%s: motors paramater is not a list", name_.c_str());
00102             return false;
00103         }
00104         
00105         int num_motors = raw_motor_list.size();
00106         motor_ids_.resize(num_motors);
00107         motor_data_.resize(num_motors);
00108         
00109         XmlRpc::XmlRpcValue available_ids;
00110         nh_.getParam("dynamixel/" + port_namespace_ + "/connected_ids", available_ids);
00111 
00112         if (available_ids.getType() != XmlRpc::XmlRpcValue::TypeArray)
00113         {
00114             ROS_ERROR("%s: connected_ids paramater is not an array", name_.c_str());
00115             return false;
00116         }
00117         
00118         for (int i = 0; i < num_motors; ++i)
00119         {
00120             XmlRpc::XmlRpcValue v = raw_motor_list[i];
00121             
00122             if (v.getType() != XmlRpc::XmlRpcValue::TypeStruct)
00123             {
00124                 ROS_ERROR("%s: motor configuration is not a map", name_.c_str());
00125                 return false;
00126             }
00127             
00128             if (!v.hasMember("id"))
00129             {
00130                 ROS_ERROR("%s: no motor id specified for motor %d", name_.c_str(), i);
00131                 return false;
00132             }
00133             
00134             int motor_id = static_cast<int>(v["id"]);
00135             bool found_motor_id = false;
00136             
00137             for (int id = 0; id < available_ids.size(); ++id)
00138             {
00139                 XmlRpc::XmlRpcValue val = available_ids[id];
00140                 
00141                 if (motor_id == static_cast<int>(val))
00142                 {
00143                     found_motor_id = true;
00144                     break;
00145                 }
00146             }
00147             
00148             if (!found_motor_id)
00149             {
00150                 ROS_ERROR("%s: motor id %d is not connected to port %s or it is connected and not responding",
00151                           name_.c_str(), motor_id, port_namespace_.c_str());
00152                 return false;
00153             }
00154             
00155             motor_ids_[i] = motor_id;
00156             motor_data_[i] = dxl_io_->getCachedParameters(motor_id);
00157             if (motor_data_[i] == NULL) { return false; }
00158             
00159             // first motor in the list is the master motor from which we take
00160             // all compliance parameters, initial, minimum and maximum positions
00161             if (i == 0)
00162             {
00163                 if (!v.hasMember("init"))
00164                 {
00165                     ROS_ERROR("%s: no initial position specified for motor %d", name_.c_str(), motor_id);
00166                     return false;
00167                 }
00168                 
00169                 init_position_encoder_ = static_cast<int>(v["init"]);
00170 
00171                 if (!v.hasMember("min"))
00172                 {
00173                     ROS_ERROR("%s: no minimum angle specified for motor %d", name_.c_str(), motor_id);
00174                     return false;
00175                 }
00176                 
00177                 min_angle_encoder_ = static_cast<int>(v["min"]);
00178 
00179                 if (!v.hasMember("max"))
00180                 {
00181                     ROS_ERROR("%s: no maximum angle specified for motor %d", name_.c_str(), motor_id);
00182                     return false;
00183                 }
00184                 
00185                 max_angle_encoder_ = static_cast<int>(v["max"]);
00186 
00187                 if (v.hasMember("compliance_margin")) { compliance_margin_ = static_cast<int>(v["compliance_margin"]); }
00188                 else { compliance_margin_ = motor_data_[i]->cw_compliance_margin; }
00189                 
00190                 if (v.hasMember("compliance_slope")) { compliance_slope_ = static_cast<int>(v["compliance_slope"]); }
00191                 else { compliance_slope_ = motor_data_[i]->cw_compliance_slope; }
00192                 
00193                 std::string prefix = "dynamixel/" + port_namespace_ + "/" +
00194                                      boost::lexical_cast<std::string>(motor_id) + "/";
00195                 
00196                 /***************** Joint velocity related parameters **********************/
00197                 nh_.getParam(prefix + "max_velocity", motor_max_velocity_);
00198                 c_nh_.param("max_velocity", max_velocity_, motor_max_velocity_);
00199         
00200                 nh_.getParam(prefix + "radians_second_per_encoder_tick", velocity_per_encoder_tick_);
00201                 min_velocity_ = velocity_per_encoder_tick_;
00202                 
00203                 if (max_velocity_ > motor_max_velocity_)
00204                 {
00205                     ROS_WARN("%s: requested maximum joint velocity exceeds motor capabilities (%f > %f)",
00206                              name_.c_str(), max_velocity_, motor_max_velocity_);
00207                     max_velocity_ = motor_max_velocity_;
00208                 }
00209                 else if (max_velocity_ < min_velocity_)
00210                 {
00211                     ROS_WARN("%s: requested maximum joint velocity is too small (%f < %f)",
00212                              name_.c_str(), max_velocity_, min_velocity_);
00213                     max_velocity_ = min_velocity_;
00214                 }
00215                 
00216                 flipped_ = min_angle_encoder_ > max_angle_encoder_;
00217                 
00218                 nh_.getParam(prefix + "radians_per_encoder_tick", radians_per_encoder_tick_);
00219                 nh_.getParam(prefix + "encoder_ticks_per_radian", encoder_ticks_per_radian_);
00220                 
00221                 if (flipped_)
00222                 {
00223                     min_angle_radians_ = (init_position_encoder_ - min_angle_encoder_) * radians_per_encoder_tick_;
00224                     max_angle_radians_ = (init_position_encoder_ - max_angle_encoder_) * radians_per_encoder_tick_;
00225                 }
00226                 else
00227                 {
00228                     min_angle_radians_ = (min_angle_encoder_ - init_position_encoder_) * radians_per_encoder_tick_;
00229                     max_angle_radians_ = (max_angle_encoder_ - init_position_encoder_) * radians_per_encoder_tick_;
00230                 }
00231                 
00232                 nh_.getParam(prefix + "encoder_resolution", motor_model_max_encoder_);
00233                 motor_model_max_encoder_ -= 1;
00234             }
00235             // slave motors, will mimic all master motor paramaters
00236             else
00237             {
00238                 drive_mode_reversed_[motor_id] = false;
00239                 if (v.hasMember("reversed")) { drive_mode_reversed_[motor_id] = static_cast<bool>(v["reversed"]); }
00240             }
00241             
00242             // set compliance margins and slopes for all motors controlling this joint to values
00243             // provided in the configuration or values from the master motor
00244             if (!dxl_io_->setComplianceMargins(motor_id, compliance_margin_, compliance_margin_))
00245             {
00246                 ROS_ERROR("%s: unable to set complaince margins for motor %d", name_.c_str(), motor_id);
00247                 return false;
00248             }
00249             
00250             if (!dxl_io_->setComplianceSlopes(motor_id, compliance_slope_, compliance_slope_))
00251             {
00252                 ROS_ERROR("%s: unable to set complaince slopes for motor %d", name_.c_str(), motor_id);
00253                 return false;
00254             }
00255         }
00256         
00257         return true;
00258     }
00259     
00260     const dynamixel_hardware_interface::JointState& getJointState() { return joint_state_; }
00261     dynamixel_hardware_interface::DynamixelIO* getPort() { return dxl_io_; }
00262     
00263     std::string getName() { return name_; }
00264     std::string getJointName() { return joint_; }
00265     std::string getPortNamespace() { return port_namespace_; }
00266     std::vector<int> getMotorIDs() { return motor_ids_; }
00267     double getMaxVelocity() { return max_velocity_; }
00268     
00269     virtual void start()
00270     {
00271         motor_states_sub_ = nh_.subscribe("motor_states/" + port_namespace_, 50, &SingleJointController::processMotorStates, this);
00272         joint_command_sub_ = c_nh_.subscribe("command", 50, &SingleJointController::processCommand, this);
00273         joint_state_pub_ = c_nh_.advertise<dynamixel_hardware_interface::JointState>("state", 50);
00274         joint_velocity_srv_ = c_nh_.advertiseService("set_velocity", &SingleJointController::processSetVelocity, this);
00275         torque_enable_srv_ = c_nh_.advertiseService("torque_enable", &SingleJointController::processTorqueEnable, this);
00276         reset_overload_error_srv_ = c_nh_.advertiseService("reset_overload_error", &SingleJointController::processResetOverloadError, this);
00277         set_torque_limit_srv_ = c_nh_.advertiseService("set_torque_limit", &SingleJointController::processSetTorqueLimit, this);
00278         set_compliance_margin_srv_ = c_nh_.advertiseService("set_compliance_margin", &SingleJointController::processSetComplianceMargin, this);
00279         set_compliance_slope_srv_ = c_nh_.advertiseService("set_compliance_slope", &SingleJointController::processSetComplianceSlope, this);
00280     }
00281     
00282     virtual void stop()
00283     {
00284         motor_states_sub_.shutdown();
00285         joint_command_sub_.shutdown();
00286         joint_state_pub_.shutdown();
00287         joint_velocity_srv_.shutdown();
00288         torque_enable_srv_.shutdown();
00289         reset_overload_error_srv_.shutdown();
00290         set_torque_limit_srv_.shutdown();
00291         set_compliance_margin_srv_.shutdown();
00292         set_compliance_slope_srv_.shutdown();
00293     }
00294     
00295     virtual bool processTorqueEnable(dynamixel_hardware_interface::TorqueEnable::Request& req,
00296                                      dynamixel_hardware_interface::TorqueEnable::Request& res)
00297     {
00298         std::vector<std::vector<int> > mcv;
00299         
00300         for (size_t i = 0; i < motor_ids_.size(); ++i)
00301         {
00302             int motor_id = motor_ids_[i];
00303             
00304             std::vector<int> pair;
00305             pair.push_back(motor_id);
00306             pair.push_back(req.torque_enable);
00307             
00308             mcv.push_back(pair);
00309         }
00310         
00311         return dxl_io_->setMultiTorqueEnabled(mcv);
00312     }
00313 
00314     bool processResetOverloadError(std_srvs::Empty::Request& req,
00315                                    std_srvs::Empty::Request& res)
00316     {
00317         bool result = true;
00318         
00319         for (size_t i = 0; i < motor_ids_.size(); ++i)
00320         {
00321             result &= dxl_io_->resetOverloadError(motor_ids_[i]);
00322         }
00323         
00324         return result;
00325     }
00326         
00327     bool processSetTorqueLimit(dynamixel_hardware_interface::SetTorqueLimit::Request& req,
00328                                dynamixel_hardware_interface::SetTorqueLimit::Request& res)
00329     {
00330         double torque_limit = req.torque_limit;
00331         
00332         if (torque_limit < 0)
00333         {
00334             ROS_WARN("%s: Torque limit is below minimum (%f < %f)", name_.c_str(), torque_limit, 0.0);
00335             torque_limit = 0.0;
00336         }
00337         else if (torque_limit > 1.0)
00338         {
00339             ROS_WARN("%s: Torque limit is above maximum (%f > %f)", name_.c_str(), torque_limit, 1.0);
00340             torque_limit = 1.0;
00341         }
00342         
00343         std::vector<std::vector<int> > mcv;
00344         
00345         for (size_t i = 0; i < motor_ids_.size(); ++i)
00346         {
00347             int motor_id = motor_ids_[i];
00348             
00349             std::vector<int> pair;
00350             pair.push_back(motor_id);
00351             pair.push_back(torque_limit * dynamixel_hardware_interface::DXL_MAX_TORQUE_ENCODER);
00352             
00353             mcv.push_back(pair);
00354         }
00355         
00356         return dxl_io_->setMultiTorqueLimit(mcv);
00357     }
00358 
00359     bool processSetComplianceMargin(dynamixel_hardware_interface::SetComplianceMargin::Request& req,
00360                                     dynamixel_hardware_interface::SetComplianceMargin::Request& res)
00361     {
00362         std::vector<std::vector<int> > mcv;
00363         
00364         for (size_t i = 0; i < motor_ids_.size(); ++i)
00365         {
00366             int motor_id = motor_ids_[i];
00367             
00368             std::vector<int> pair;
00369             pair.push_back(motor_id);
00370             pair.push_back(req.margin);
00371             pair.push_back(req.margin);
00372             
00373             mcv.push_back(pair);
00374         }
00375         
00376         return dxl_io_->setMultiComplianceMargins(mcv);
00377     }
00378 
00379     bool processSetComplianceSlope(dynamixel_hardware_interface::SetComplianceSlope::Request& req,
00380                                    dynamixel_hardware_interface::SetComplianceSlope::Request& res)
00381     {
00382         std::vector<std::vector<int> > mcv;
00383         
00384         for (size_t i = 0; i < motor_ids_.size(); ++i)
00385         {
00386             int motor_id = motor_ids_[i];
00387             
00388             std::vector<int> pair;
00389             pair.push_back(motor_id);
00390             pair.push_back(req.slope);
00391             pair.push_back(req.slope);
00392             
00393             mcv.push_back(pair);
00394         }
00395         
00396         return dxl_io_->setMultiComplianceSlopes(mcv);
00397     }
00398 
00399     virtual std::vector<std::vector<int> > getRawMotorCommands(double position, double velocity) = 0;
00400     
00401     virtual void processMotorStates(const dynamixel_hardware_interface::MotorStateListConstPtr& msg) = 0;
00402     virtual void processCommand(const std_msgs::Float64ConstPtr& msg) = 0;
00403     
00404     virtual bool setVelocity(double velocity) = 0;
00405     virtual bool processSetVelocity(dynamixel_hardware_interface::SetVelocity::Request& req,
00406                                     dynamixel_hardware_interface::SetVelocity::Request& res) = 0;
00407         
00408 protected:
00409     ros::NodeHandle nh_;
00410     ros::NodeHandle c_nh_;
00411     
00412     std::string name_;
00413     std::string port_namespace_;
00414     dynamixel_hardware_interface::DynamixelIO* dxl_io_;
00415     
00416     std::string joint_;
00417     dynamixel_hardware_interface::JointState joint_state_;
00418     
00419     std::vector<int> motor_ids_;
00420     std::map<int, bool> drive_mode_reversed_;
00421     std::vector<const dynamixel_hardware_interface::DynamixelData*> motor_data_;
00422     
00423     static const double INIT_VELOCITY = 0.5;
00424     double max_velocity_;
00425     double min_velocity_;
00426     
00427     double min_angle_radians_;
00428     double max_angle_radians_;
00429     
00430     int init_position_encoder_;
00431     int min_angle_encoder_;
00432     int max_angle_encoder_;
00433     bool flipped_;
00434     int compliance_margin_;
00435     int compliance_slope_;
00436     
00437     double encoder_ticks_per_radian_;
00438     double radians_per_encoder_tick_;
00439     double velocity_per_encoder_tick_;
00440     double motor_max_velocity_;
00441     int motor_model_max_encoder_;
00442         
00443     ros::Subscriber motor_states_sub_;
00444     ros::Subscriber joint_command_sub_;
00445     ros::Publisher joint_state_pub_;
00446     ros::ServiceServer joint_velocity_srv_;
00447     ros::ServiceServer torque_enable_srv_;
00448     ros::ServiceServer reset_overload_error_srv_;
00449     ros::ServiceServer set_torque_limit_srv_;
00450     ros::ServiceServer set_compliance_margin_srv_;
00451     ros::ServiceServer set_compliance_slope_srv_;
00452     
00453     uint16_t convertToEncoder(double angle_in_radians)
00454     {
00455         double angle_in_encoder = angle_in_radians * encoder_ticks_per_radian_;
00456         angle_in_encoder = flipped_ ? init_position_encoder_ - angle_in_encoder : init_position_encoder_ + angle_in_encoder;
00457         return (int) (round(angle_in_encoder));
00458     }
00459 
00460     double convertToRadians(int angle_in_encoder)
00461     {
00462         double angle_in_radians = flipped_ ? init_position_encoder_ - angle_in_encoder : angle_in_encoder - init_position_encoder_;
00463         return angle_in_radians * radians_per_encoder_tick_;
00464     }
00465 
00466 private:
00467     SingleJointController(const SingleJointController &c);
00468     SingleJointController& operator =(const SingleJointController &c);
00469 };
00470 
00471 }
00472 
00473 #endif  // DYNAMIXEL_HARDWARE_INTERFACE_SINGLE_JOINT_CONTROLLER_H


dynamixel_hardware_interface
Author(s): Antons Rebguns
autogenerated on Sun Oct 5 2014 23:33:10