$search
00001 /* 00002 * Copyright (c) 2008, Willow Garage, Inc. 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 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00069 #ifndef CARTESIAN_POSE_CONTROLLER_H 00070 #define CARTESIAN_POSE_CONTROLLER_H 00071 00072 #include <vector> 00073 #include <boost/scoped_ptr.hpp> 00074 #include <boost/thread/condition.hpp> 00075 00076 #include <ros/node_handle.h> 00077 00078 #include <geometry_msgs/PoseStamped.h> 00079 #include <geometry_msgs/Twist.h> 00080 00081 #include <control_toolbox/pid.h> 00082 #include <kdl/chainfksolver.hpp> 00083 #include <kdl/chain.hpp> 00084 #include <kdl/chainjnttojacsolver.hpp> 00085 #include <kdl/frames.hpp> 00086 #include <message_filters/subscriber.h> 00087 #include <pr2_controller_interface/controller.h> 00088 #include <pr2_mechanism_model/chain.h> 00089 #include <realtime_tools/realtime_publisher.h> 00090 #include <tf/message_filter.h> 00091 #include <tf/transform_datatypes.h> 00092 #include <tf/transform_listener.h> 00093 00094 00095 namespace controller { 00096 00097 class CartesianPoseController : public pr2_controller_interface::Controller 00098 { 00099 public: 00100 CartesianPoseController(); 00101 ~CartesianPoseController(); 00102 00103 bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 00104 void starting(); 00105 void update(); 00106 00107 void command(const geometry_msgs::PoseStamped::ConstPtr& pose_msg); 00108 00109 // input of the controller 00110 KDL::Frame pose_desi_, pose_meas_; 00111 KDL::Twist twist_ff_; 00112 00113 // state output 00114 KDL::Twist twist_error_; 00115 00116 private: 00117 KDL::Frame getPose(); 00118 00119 ros::NodeHandle node_; 00120 std::string controller_name_, root_name_; 00121 ros::Time last_time_; 00122 00123 // robot structure 00124 pr2_mechanism_model::RobotState *robot_state_; 00125 pr2_mechanism_model::Chain chain_; 00126 00127 // pid controllers 00128 std::vector<control_toolbox::Pid> pid_controller_; 00129 00130 // kdl stuff for kinematics 00131 KDL::Chain kdl_chain_; 00132 boost::scoped_ptr<KDL::ChainFkSolverPos> jnt_to_pose_solver_; 00133 boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_; 00134 KDL::JntArray jnt_pos_; 00135 KDL::JntArray jnt_eff_; 00136 KDL::Jacobian jacobian_; 00137 00138 // reatltime publisher 00139 boost::scoped_ptr<realtime_tools::RealtimePublisher<geometry_msgs::Twist> > state_error_publisher_; 00140 boost::scoped_ptr<realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped> > state_pose_publisher_; 00141 unsigned int loop_count_; 00142 00143 tf::TransformListener tf_; 00144 message_filters::Subscriber<geometry_msgs::PoseStamped> sub_command_; 00145 boost::scoped_ptr<tf::MessageFilter<geometry_msgs::PoseStamped> > command_filter_; 00146 //boost::scoped_ptr<tf::MessageNotifier<geometry_msgs::PoseStamped> > command_notifier_; 00147 }; 00148 00149 } // namespace 00150 00151 00152 #endif