joint_torque_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 #ifndef JOINT_TORQUE_CONTROLLER_H
00029 #define JOINT_TORQUE_CONTROLLER_H
00030 
00031 #include <map>
00032 #include <string>
00033 
00034 #include <dynamixel_hardware_interface/dynamixel_io.h>
00035 #include <dynamixel_hardware_interface/single_joint_controller.h>
00036 #include <dynamixel_hardware_interface/MotorStateList.h>
00037 #include <dynamixel_hardware_interface/SetVelocity.h>
00038 #include <dynamixel_hardware_interface/TorqueEnable.h>
00039 
00040 #include <ros/ros.h>
00041 #include <std_msgs/Float64.h>
00042 #include <std_srvs/Empty.h>
00043 
00044 namespace controller
00045 {
00046 
00047 class JointTorqueController : public SingleJointController
00048 {
00049 public:
00050     bool initialize(std::string name,
00051                     std::string port_namespace,
00052                     dynamixel_hardware_interface::DynamixelIO* dxl_io);
00053     
00054     bool processTorqueEnable(dynamixel_hardware_interface::TorqueEnable::Request& req,
00055                              dynamixel_hardware_interface::TorqueEnable::Request& res);
00056     
00057     std::vector<std::vector<int> > getRawMotorCommands(double position, double velocity);
00058     
00059     void processMotorStates(const dynamixel_hardware_interface::MotorStateListConstPtr& msg);
00060     void processCommand(const std_msgs::Float64ConstPtr& msg);
00061     
00062     bool setVelocity(double velocity);
00063     bool processSetVelocity(dynamixel_hardware_interface::SetVelocity::Request& req,
00064                             dynamixel_hardware_interface::SetVelocity::Request& res);
00065     
00066 private:
00067     int16_t velRad2Enc(double vel_rad);    
00068 };
00069 
00070 }
00071 
00072 #endif  // JOINT_TORQUE_CONTROLLER_H


dynamixel_hardware_interface
Author(s): Antons Rebguns
autogenerated on Fri Aug 28 2015 10:32:45