$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009, Willow Garage, Inc. 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 the Willow Garage 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 /* 00035 * Author: Sachin Chitta and Matthew Piccoli 00036 */ 00037 00038 #ifndef PR2_BASE_KINEMATICS_H 00039 #define PR2_BASE_KINEMATICS_H 00040 00041 #include <pr2_mechanism_model/robot.h> 00042 #include <pr2_controller_interface/controller.h> 00043 #include <geometry_msgs/Twist.h> 00044 #include <geometry_msgs/Point.h> 00045 #include <control_toolbox/filters.h> 00046 #include <boost/thread/condition.hpp> 00047 00048 namespace controller 00049 { 00050 class Wheel; 00051 class Caster; 00052 class BaseKinematics; 00056 class Wheel 00057 { 00058 public: 00059 00063 pr2_mechanism_model::JointState *joint_; 00064 00068 geometry_msgs::Point offset_; 00069 00073 std::string joint_name_; 00074 00078 std::string link_name_; 00079 00080 00084 geometry_msgs::Point position_; 00085 00089 Caster *parent_; 00090 00094 double wheel_speed_actual_; 00095 00099 double wheel_speed_cmd_; 00100 00104 double wheel_speed_error_; 00105 00109 double wheel_speed_filtered_; 00110 00114 int direction_multiplier_; 00115 00119 int wheel_stuck_; 00120 00124 double wheel_radius_; 00125 00131 bool init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node, std::string link_name); 00132 00136 void updatePosition(); 00137 }; 00138 00142 class Caster 00143 { 00144 public: 00145 00149 pr2_mechanism_model::JointState *joint_; 00150 00154 geometry_msgs::Point offset_; 00155 00159 std::string link_name_; 00160 00164 std::string joint_name_; 00165 00166 //geometry_msgs::Point position_; 00167 00171 BaseKinematics *parent_; 00172 00176 int num_children_; 00177 00181 double steer_angle_actual_; 00182 00186 double steer_angle_desired_; 00187 00191 double steer_velocity_desired_; 00192 00196 double steer_angle_stored_; 00197 00201 double caster_position_error_; 00202 00206 double caster_speed_error_; 00207 00211 double caster_speed_filtered_; 00212 00216 double caster_speed_; 00217 00221 int caster_stuck_; 00222 00223 bool init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node, std::string link_name); 00224 }; 00225 00229 class BaseKinematics 00230 { 00231 public: 00232 00239 bool init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node); 00240 00244 void computeWheelPositions(); 00245 00252 geometry_msgs::Twist pointVel2D(const geometry_msgs::Point& pos, const geometry_msgs::Twist& vel); 00253 00257 pr2_mechanism_model::RobotState *robot_state_; 00258 00262 int num_wheels_; 00263 00267 int num_casters_; 00268 00272 std::vector<Wheel> wheel_; 00273 00277 std::vector<Caster> caster_; 00278 00282 std::string xml_caster_name_; 00283 00287 std::string xml_wheel_name_; 00288 00292 std::string name_; 00293 00297 double MAX_DT_; 00298 00302 std::string robot_base_id_; 00303 }; 00304 } // namespace 00305 #endif