joint_position_controller.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Modified BSD License)
00003  *
00004  *  Copyright (c) 2011, PAL Robotics, S.L.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of PAL Robotics, S.L. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00038 #include <kdl/jntarray.hpp>
00039 #include <pr2_controller_interface/controller.h>
00040 #include <pr2_mechanism_model/robot.h>
00041 #include <pr2_mechanism_model/tree.h>
00042 #include <ros/ros.h>
00043 #include <sensor_msgs/JointState.h>
00044 
00053 class JointPositionController: public pr2_controller_interface::Controller
00054 {
00055   public:
00066     bool init(pr2_mechanism_model::RobotState* robot_state, ros::NodeHandle& nh);
00067 
00074     void starting();
00075 
00083     void update();
00084 
00090     void stopping();
00091 
00092   private:
00093     pr2_mechanism_model::RobotState* robot_state_; // the current robot state (to get the time stamp)
00094     pr2_mechanism_model::Tree tree_;  // The tree handle (i.e. for getting current joint positions)
00095     unsigned int nrOfJnts_;           // number of joints
00096     ros::Time         last_time_;     // variable used for calculating the timestep
00097     double            dt_;            // timestep
00098     unsigned int      loop_count_;    // used for displaying debug messages
00099     bool              limit_error_;   // flag indicating whether or not to limit the error before calculating efforts
00100     double            q_err_max_;     // Maximum error taken into account
00101     double            q_err_dot_max_; // Maximum error velocity taken into account per timestep
00102 
00103     // KDL variables (which need to be pre-allocated).
00104     KDL::JntArray     q_;             // Joint positions
00105     KDL::JntArray     q_desi_;        // Desired joint positions
00106     KDL::JntArray     q_err_;         // Joints' position error
00107     KDL::JntArray     q_err_old_;     // Former joints' position error
00108     KDL::JntArray     q_err_dot_;     // Change of joints' position error
00109     KDL::JntArray     Kp_;            // Proportional gains
00110     KDL::JntArray     Kd_;            // Derivative gains
00111     KDL::JntArray     tau_;           // Joint torques
00112     KDL::JntArray     tau_max_;       // Maximum joint torques
00113 
00114     ros::Subscriber sub_joint_states_cmd; // subscriber to joint states commands
00115 
00123     void jointStatesCmdCB(const sensor_msgs::JointState::ConstPtr& command);
00124 };
00125 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


joint_position_controller
Author(s): Marcus Liebhardt , Adolfo Rodríguez Tsouroukdissian , Hilario Tome
autogenerated on Thu Jun 27 2013 16:12:18