$search
00001 00026 #include "sr_mechanism_controllers/sr_controller.hpp" 00027 #include "angles/angles.h" 00028 #include "pluginlib/class_list_macros.h" 00029 #include <sstream> 00030 #include <math.h> 00031 #include "sr_utilities/sr_math_utils.hpp" 00032 00033 #include <std_msgs/Float64.h> 00034 00035 using namespace std; 00036 00037 namespace controller { 00038 00039 SrController::SrController() 00040 : joint_state_(NULL), command_(0), 00041 min_(0.0), max_(sr_math_utils::pi), 00042 loop_count_(0), initialized_(false), robot_(NULL), last_time_(0), 00043 n_tilde_("~"), 00044 max_force_demand(1023.), friction_deadband(5) 00045 { 00046 } 00047 00048 SrController::~SrController() 00049 { 00050 sub_command_.shutdown(); 00051 } 00052 00053 void SrController::after_init() 00054 { 00055 sub_command_ = node_.subscribe<std_msgs::Float64>("command", 1, &SrController::setCommandCB, this); 00056 } 00057 00058 00059 std::string SrController::getJointName() 00060 { 00061 return joint_state_->joint_->name; 00062 } 00063 00064 // Set the joint position command 00065 void SrController::setCommand(double cmd) 00066 { 00067 command_ = cmd; 00068 } 00069 00070 // Return the current position command 00071 void SrController::getCommand(double & cmd) 00072 { 00073 cmd = command_; 00074 } 00075 00076 void SrController::setCommandCB(const std_msgs::Float64ConstPtr& msg) 00077 { 00078 command_ = msg->data; 00079 } 00080 00081 bool SrController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) 00082 { 00083 return true; 00084 } 00085 00086 void SrController::update() 00087 { 00088 } 00089 00090 void SrController::starting() 00091 {} 00092 00093 bool SrController::resetGains(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp) 00094 { 00095 return true; 00096 } 00097 00098 void SrController::getGains(double &p, double &i, double &d, double &i_max, double &i_min) 00099 {} 00100 00101 void SrController::get_min_max( urdf::Model model, std::string joint_name ) 00102 { 00103 if( joint_name.substr(3,1).compare("0") == 0) 00104 { 00105 std::string j1 = joint_name.substr(0,3) + "1"; 00106 std::string j2 = joint_name.substr(0,3) + "2"; 00107 00108 boost::shared_ptr<const urdf::Joint> joint1 = model.getJoint( j1 ); 00109 boost::shared_ptr<const urdf::Joint> joint2 = model.getJoint( j2 ); 00110 00111 min_ = joint1->limits->lower + joint2->limits->lower; 00112 max_ = joint1->limits->upper + joint2->limits->upper; 00113 } 00114 else 00115 { 00116 boost::shared_ptr<const urdf::Joint> joint = model.getJoint( joint_name ); 00117 00118 min_ = joint->limits->lower; 00119 max_ = joint->limits->upper; 00120 } 00121 } 00122 00123 double SrController::clamp_command( double cmd ) 00124 { 00125 if(cmd < min_) 00126 return min_; 00127 00128 if(cmd > max_) 00129 return max_; 00130 00131 return cmd; 00132 } 00133 } 00134 00135 /* For the emacs weenies in the crowd. 00136 Local Variables: 00137 c-basic-offset: 2 00138 End: 00139 */ 00140 00141