joint_position_controller.cpp
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 #include <sstream>
00029 #include <XmlRpcValue.h>
00030 
00031 #include <dynamixel_hardware_interface/dynamixel_const.h>
00032 #include <dynamixel_hardware_interface/dynamixel_io.h>
00033 #include <dynamixel_hardware_interface/single_joint_controller.h>
00034 #include <dynamixel_hardware_interface/joint_position_controller.h>
00035 #include <dynamixel_hardware_interface/MotorState.h>
00036 #include <dynamixel_hardware_interface/SetVelocity.h>
00037 #include <dynamixel_hardware_interface/TorqueEnable.h>
00038 #include <dynamixel_hardware_interface/SetTorqueLimit.h>
00039 
00040 #include <ros/ros.h>
00041 #include <pluginlib/class_list_macros.h>
00042 
00043 #include <std_msgs/Float64.h>
00044 #include <std_srvs/Empty.h>
00045 
00046 PLUGINLIB_DECLARE_CLASS(dynamixel_hardware_interface,
00047                         JointPositionController,
00048                         controller::JointPositionController,
00049                         controller::SingleJointController)
00050 
00051 namespace controller
00052 {
00053 
00054 bool JointPositionController::initialize(std::string name,
00055                                          std::string port_namespace,
00056                                          dynamixel_hardware_interface::DynamixelIO* dxl_io)
00057 {
00058     if (!SingleJointController::initialize(name, port_namespace, dxl_io)) { return false; }
00059 
00060     for (size_t i = 0; i < motor_ids_.size(); ++i)
00061     {
00062         int motor_id = motor_ids_[i];
00063         
00064         if (motor_data_[i]->cw_angle_limit == 0 && motor_data_[i]->ccw_angle_limit == 0)
00065         {
00066             ROS_WARN("%s: motor %d is not set to position control mode, setting motor to position control mode", name_.c_str(), motor_id);
00067             
00068             if (!dxl_io_->setAngleLimits(motor_id, 0, motor_model_max_encoder_))
00069             {
00070                 ROS_ERROR("%s: unable to set motor %d to position control mode", name_.c_str(), motor_id);
00071                 return false;
00072             }
00073         }
00074         
00075     }
00076 
00077     setVelocity(INIT_VELOCITY);
00078     return true;
00079 }
00080 
00081 bool JointPositionController::processTorqueEnable(dynamixel_hardware_interface::TorqueEnable::Request& req,
00082                                                   dynamixel_hardware_interface::TorqueEnable::Request& res)
00083 {
00084     // set target position to current joint position
00085     // so the motor won't go crazy once torque is enabled again
00086     std_msgs::Float64 position;
00087     position.data = joint_state_.position;
00088     processCommand(boost::make_shared<const std_msgs::Float64>(position));
00089     
00090     return SingleJointController::processTorqueEnable(req, res);
00091 }
00092 
00093 std::vector<std::vector<int> > JointPositionController::getRawMotorCommands(double position, double velocity)
00094 {
00095     uint16_t pos_enc = posRad2Enc(position);
00096     uint16_t vel_enc = velRad2Enc(velocity);
00097     
00098     std::vector<std::vector<int> > value_pairs;
00099 
00100     for (size_t i = 0; i < motor_ids_.size(); ++i)
00101     {
00102         int motor_id = motor_ids_[i];
00103         
00104         std::vector<int> pair;
00105         pair.push_back(motor_id);
00106         
00107         // master motor
00108         if (i == 0)
00109         {
00110             pair.push_back(pos_enc);
00111         }
00112         // slave motors calculate their positions based on
00113         // selected drive mode and master position
00114         else
00115         {
00116             if (!drive_mode_reversed_[motor_id])
00117             {
00118                 pair.push_back(pos_enc);
00119             }
00120             else // reverse, e.g. smart arm double joints
00121             {
00122                 pair.push_back(motor_model_max_encoder_ - pos_enc);
00123             }
00124         }
00125         
00126         pair.push_back(vel_enc);
00127         
00128         ROS_DEBUG("%s, setting position and velocity for motor %d to %d and %d", name_.c_str(), motor_id, pair[1], pair[2]);
00129         value_pairs.push_back(pair);
00130     }
00131 
00132     return value_pairs;
00133 }
00134 
00135 void JointPositionController::processMotorStates(const dynamixel_hardware_interface::MotorStateListConstPtr& msg)
00136 {
00137     dynamixel_hardware_interface::MotorState state;
00138     int master_id = motor_ids_[0];
00139     
00140     for (size_t i = 0; i < msg->motor_states.size(); ++i)
00141     {
00142         if (master_id == msg->motor_states[i].id)
00143         {
00144             state = msg->motor_states[i];
00145             break;
00146         }
00147     }
00148     
00149     if (state.id != master_id)
00150     {
00151         ROS_ERROR("%s: motor %d not found in motor states message", name_.c_str(), master_id);
00152         return;
00153     }
00154     
00155     joint_state_.header.stamp = ros::Time(state.timestamp);
00156     joint_state_.target_position = convertToRadians(state.target_position);
00157     joint_state_.target_velocity = ((double)state.target_velocity / dynamixel_hardware_interface::DXL_MAX_VELOCITY_ENCODER) * motor_max_velocity_;
00158     joint_state_.position = convertToRadians(state.position);
00159     joint_state_.velocity = ((double)state.velocity / dynamixel_hardware_interface::DXL_MAX_VELOCITY_ENCODER) * motor_max_velocity_;
00160     joint_state_.load = (double)state.load / dynamixel_hardware_interface::DXL_MAX_LOAD_ENCODER;
00161     joint_state_.moving = state.moving;
00162     
00163     joint_state_pub_.publish(joint_state_);
00164 }
00165 
00166 void JointPositionController::processCommand(const std_msgs::Float64ConstPtr& msg)
00167 {
00168     uint16_t pos_enc = posRad2Enc(msg->data);
00169     std::vector<std::vector<int> > mcv;
00170     
00171     for (size_t i = 0; i < motor_ids_.size(); ++i)
00172     {
00173         int motor_id = motor_ids_[i];
00174         
00175         std::vector<int> pair;
00176         pair.push_back(motor_id);
00177         
00178         // master motor
00179         if (i == 0)
00180         {
00181             pair.push_back(pos_enc);
00182         }
00183         // slave motors calculate their positions based on
00184         // selected drive mode and master position
00185         else
00186         {
00187             if (!drive_mode_reversed_[motor_id])
00188             {
00189                 pair.push_back(pos_enc);
00190             }
00191             else // reverse, e.g. smart arm double joints
00192             {
00193                 pair.push_back(motor_model_max_encoder_ - pos_enc);
00194             }
00195         }
00196         
00197         ROS_DEBUG("%s: setting position for motor %d to %d", name_.c_str(), motor_id, pair[1]);
00198         mcv.push_back(pair);
00199     }
00200     
00201     dxl_io_->setMultiPosition(mcv);
00202 }
00203 
00204 bool JointPositionController::setVelocity(double velocity)
00205 {
00206     uint16_t vel_enc = velRad2Enc(velocity);
00207     std::vector<std::vector<int> > mcv;
00208     
00209     for (size_t i = 0; i < motor_ids_.size(); ++i)
00210     {
00211         std::vector<int> pair;
00212         
00213         pair.push_back(motor_ids_[i]);
00214         pair.push_back(vel_enc);
00215         
00216         ROS_DEBUG("%s, setting velocity for motor %d to %d", name_.c_str(), motor_ids_[i], vel_enc);
00217         mcv.push_back(pair);
00218     }
00219 
00220     return dxl_io_->setMultiVelocity(mcv);
00221 }
00222 
00223 bool JointPositionController::processSetVelocity(dynamixel_hardware_interface::SetVelocity::Request& req,
00224                                                  dynamixel_hardware_interface::SetVelocity::Request& res)
00225 {
00226     return setVelocity(req.velocity);
00227 }
00228 
00229 uint16_t JointPositionController::posRad2Enc(double pos_rad)
00230 {
00231     if (pos_rad < min_angle_radians_) { pos_rad = min_angle_radians_; }
00232     if (pos_rad > max_angle_radians_) { pos_rad = max_angle_radians_; }
00233     return convertToEncoder(pos_rad);
00234 }
00235 
00236 uint16_t JointPositionController::velRad2Enc(double vel_rad)
00237 {
00238     if (vel_rad < min_velocity_) { vel_rad = min_velocity_; }
00239     if (vel_rad > max_velocity_) { vel_rad = max_velocity_; }
00240     
00241     return std::max<uint16_t>(1, (uint16_t) round(vel_rad / velocity_per_encoder_tick_));
00242 }
00243 
00244 }


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