joint_torque_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_torque_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 
00045 PLUGINLIB_DECLARE_CLASS(dynamixel_hardware_interface,
00046                         JointTorqueController,
00047                         controller::JointTorqueController,
00048                         controller::SingleJointController)
00049 
00050 namespace controller
00051 {
00052 
00053 bool JointTorqueController::initialize(std::string name,
00054                                        std::string port_namespace,
00055                                        dynamixel_hardware_interface::DynamixelIO* dxl_io)
00056 {
00057     if (!SingleJointController::initialize(name, port_namespace, dxl_io)) { return false; }
00058 
00059     for (size_t i = 0; i < motor_ids_.size(); ++i)
00060     {
00061         setVelocity(0.0);
00062         
00063         if (motor_data_[i]->cw_angle_limit != 0 || motor_data_[i]->ccw_angle_limit != 0)
00064         {
00065             ROS_WARN("%s: motor %d is not set to torque control mode, setting motor to torque control mode", name_.c_str(), motor_ids_[i]);
00066             
00067             if (!dxl_io_->setAngleLimits(motor_ids_[i], 0, 0))
00068             {
00069                 ROS_ERROR("%s: unable to set motor to torque control mode", name_.c_str());
00070                 return false;
00071             }
00072         }
00073     }
00074     
00075     return true;
00076 }
00077 
00078 bool JointTorqueController::processTorqueEnable(dynamixel_hardware_interface::TorqueEnable::Request& req,
00079                                                   dynamixel_hardware_interface::TorqueEnable::Request& res)
00080 {
00081     setVelocity(0.0);
00082     return SingleJointController::processTorqueEnable(req, res);
00083 }
00084 
00085 std::vector<std::vector<int> > JointTorqueController::getRawMotorCommands(double position, double velocity)
00086 {
00087     int16_t vel_enc = velRad2Enc(velocity);
00088     std::vector<std::vector<int> > mcv;
00089     
00090     for (size_t i = 0; i < motor_ids_.size(); ++i)
00091     {
00092         int motor_id = motor_ids_[i];
00093         
00094         std::vector<int> pair;
00095         pair.push_back(motor_id);
00096         
00097         if (i == 0)
00098         {
00099             pair.push_back(vel_enc);
00100         }
00101         else
00102         {
00103             if (!drive_mode_reversed_[motor_id])
00104             {
00105                 pair.push_back(vel_enc);
00106             }
00107             else
00108             {
00109                 pair.push_back(-vel_enc);
00110             }
00111         }
00112         
00113         ROS_DEBUG("%s, setting velocity for motor %d to %d", name_.c_str(), motor_id, vel_enc);
00114         mcv.push_back(pair);
00115     }
00116 
00117     return mcv;
00118 }
00119 
00120 void JointTorqueController::processMotorStates(const dynamixel_hardware_interface::MotorStateListConstPtr& msg)
00121 {
00122     dynamixel_hardware_interface::MotorState state;
00123     int master_id = motor_ids_[0];
00124     
00125     for (size_t i = 0; i < msg->motor_states.size(); ++i)
00126     {
00127         if (master_id == msg->motor_states[i].id)
00128         {
00129             state = msg->motor_states[i];
00130             break;
00131         }
00132     }
00133     
00134     if (state.id != master_id)
00135     {
00136         ROS_ERROR("%s: motor id %d not found", name_.c_str(), master_id);
00137         return;
00138     }
00139     
00140     joint_state_.header.stamp = ros::Time(state.timestamp);
00141     joint_state_.target_position = convertToRadians(state.target_position);
00142     joint_state_.target_velocity = ((double)state.target_velocity / dynamixel_hardware_interface::DXL_MAX_VELOCITY_ENCODER) * motor_max_velocity_;
00143     joint_state_.position = convertToRadians(state.position);
00144     joint_state_.velocity = ((double)state.velocity / dynamixel_hardware_interface::DXL_MAX_VELOCITY_ENCODER) * motor_max_velocity_;
00145     joint_state_.load = (double)state.load / dynamixel_hardware_interface::DXL_MAX_LOAD_ENCODER;
00146     joint_state_.moving = state.moving;
00147     
00148     joint_state_pub_.publish(joint_state_);
00149 }
00150 
00151 void JointTorqueController::processCommand(const std_msgs::Float64ConstPtr& msg)
00152 {
00153     setVelocity(msg->data);
00154 }
00155 
00156 bool JointTorqueController::setVelocity(double velocity)
00157 {
00158     return dxl_io_->setMultiVelocity(getRawMotorCommands(0.0, velocity));
00159 }
00160 
00161 bool JointTorqueController::processSetVelocity(dynamixel_hardware_interface::SetVelocity::Request& req,
00162                                                dynamixel_hardware_interface::SetVelocity::Request& res)
00163 {
00164     return setVelocity(req.velocity);
00165 }
00166 
00167 int16_t JointTorqueController::velRad2Enc(double vel_rad)
00168 {
00169     if (vel_rad < -max_velocity_) { vel_rad = -max_velocity_; }
00170     if (vel_rad > max_velocity_) { vel_rad = max_velocity_; }
00171     
00172     return (int16_t) round(vel_rad / velocity_per_encoder_tick_);
00173 }
00174 
00175 }


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