00001 00028 #ifndef _SRH_MUSCLE_JOINT_POSITION_CONTROLLER_HPP_ 00029 #define _SRH_MUSCLE_JOINT_POSITION_CONTROLLER_HPP_ 00030 00031 #include <sr_mechanism_controllers/sr_controller.hpp> 00032 #include <sr_robot_msgs/JointMusclePositionControllerState.h> 00033 00034 namespace controller 00035 { 00036 class SrhMuscleJointPositionController : public SrController 00037 { 00038 public: 00039 SrhMuscleJointPositionController(); 00040 00041 bool init(ros_ethercat_model::RobotState *robot, ros::NodeHandle &n); 00042 00043 virtual void starting(const ros::Time& time); 00044 00048 virtual void update(const ros::Time& time, const ros::Duration& period); 00049 00050 virtual void getGains(double &p, double &i, double &d, double &i_max, double &i_min); 00051 virtual bool resetGains(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp); 00052 bool setGains(sr_robot_msgs::SetPidGains::Request &req, sr_robot_msgs::SetPidGains::Response &resp); 00053 00054 private: 00055 boost::scoped_ptr<control_toolbox::Pid> pid_controller_position_; 00057 //publish our joint controller state 00058 boost::scoped_ptr<realtime_tools::RealtimePublisher<sr_robot_msgs::JointMusclePositionControllerState> > controller_state_publisher_; 00059 00061 double position_deadband; 00062 00064 sr_deadband::HysteresisDeadband<double> hysteresis_deadband; 00065 00067 int command_acc_; 00068 00070 void read_parameters(); 00071 00073 void setCommandCB(const std_msgs::Float64ConstPtr& msg); 00074 00075 void resetJointState(); 00076 }; 00077 } // namespace 00078 00079 /* For the emacs weenies in the crowd. 00080 Local Variables: 00081 c-basic-offset: 2 00082 End: 00083 */ 00084 00085 00086 #endif