00001 00012 /************************************************************************ 00013 * Copyright (C) 2012 Eindhoven University of Technology (TU/e). * 00014 * All rights reserved. * 00015 ************************************************************************ 00016 * Redistribution and use in source and binary forms, with or without * 00017 * modification, are permitted provided that the following conditions * 00018 * are met: * 00019 * * 00020 * 1. Redistributions of source code must retain the above * 00021 * copyright notice, this list of conditions and the following * 00022 * disclaimer. * 00023 * * 00024 * 2. Redistributions in binary form must reproduce the above * 00025 * copyright notice, this list of conditions and the following * 00026 * disclaimer in the documentation and/or other materials * 00027 * provided with the distribution. * 00028 * * 00029 * THIS SOFTWARE IS PROVIDED BY TU/e "AS IS" AND ANY EXPRESS OR * 00030 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * 00031 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 00032 * ARE DISCLAIMED. IN NO EVENT SHALL TU/e OR CONTRIBUTORS BE LIABLE * 00033 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * 00034 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT * 00035 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; * 00036 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * 00037 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 00038 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE * 00039 * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH * 00040 * DAMAGE. * 00041 * * 00042 * The views and conclusions contained in the software and * 00043 * documentation are those of the authors and should not be * 00044 * interpreted as representing official policies, either expressed or * 00045 * implied, of TU/e. * 00046 ************************************************************************/ 00047 00048 #ifndef POSITIONCONTROLLER_H_ 00049 #define POSITIONCONTROLLER_H_ 00050 00051 #include <pr2_controller_interface/controller.h> 00052 #include <pr2_mechanism_model/joint.h> 00053 #include <ros/ros.h> 00054 #include <control_toolbox/pid.h> 00055 #include <sensor_msgs/JointState.h> 00056 00057 #include <amigo_gazebo/ref_generator.h> 00058 00059 #include <math.h> 00060 00061 #include <map> 00062 00063 namespace controller { 00064 00065 class PositionController: public pr2_controller_interface::Controller { 00066 00067 private: 00071 struct JointStruct { 00072 std::string name_; 00073 00074 controller::RefGenerator* ref_generator_; 00075 00076 pr2_mechanism_model::JointState* joint_state_; 00077 00079 double min_pos_; 00080 double max_pos_; 00081 double max_vel_; 00082 double max_acc_; 00084 00086 double mass_ff_; 00087 double damp_ff_; 00088 double grav_ff_; 00090 00091 double reference_; 00092 00093 control_toolbox::Pid pid_controller_; 00094 }; 00095 00096 std::vector<JointStruct*> joint_structs_; 00097 00098 std::map<std::string, JointStruct*> joint_map_; 00099 00100 pr2_mechanism_model::RobotState *robot_; 00101 00102 ros::Time time_of_last_cycle_; 00103 00104 ros::Time current_time; 00105 00106 ros::Subscriber references_sub_; 00107 ros::Publisher measurements_pub_; 00108 00109 ros::NodeHandle node_; 00110 00111 00112 public: 00113 00114 PositionController(); 00115 00122 virtual ~PositionController(); 00123 00132 void referenceCallback(const sensor_msgs::JointState::ConstPtr& msg); 00133 00144 virtual bool init(pr2_mechanism_model::RobotState* robot, ros::NodeHandle& n); 00145 00153 virtual void starting(); 00154 00159 virtual void update(); 00160 00166 virtual void stopping(); 00167 00168 }; 00169 00170 } 00171 00172 #endif /* POSITIONCONTROLLER_H_ */