pr2_mechanism_controllers: base_kinematics.cpp Source File

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 #include <pr2_mechanism_controllers/base_kinematics.h>
00039 #include <kdl/tree.hpp>
00040 
00041 using namespace controller;
00042 
00043 
00044 bool Wheel::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node, std::string link_name)
00045 {
00046   wheel_stuck_ = 0;
00047   direction_multiplier_ = 1;
00048   wheel_speed_filtered_ = 0;
00049   wheel_speed_error_ = 0;
00050   wheel_speed_cmd_ = 0;
00051   wheel_speed_actual_ = 0;
00052 
00053   boost::shared_ptr<const urdf::Link> link;
00054   link = robot_state->model_->robot_model_.getLink(link_name);
00055   if (!link){
00056     ROS_ERROR("Could not find link with name %s",link_name.c_str());
00057     return false;
00058   }
00059   
00060   if(!link->collision)
00061   {
00062     ROS_ERROR("Link %s does not have collision description. Add collision description for link to urdf.",link_name.c_str());
00063     return false;
00064   }
00065   if(!link->collision->geometry)
00066   {
00067     ROS_ERROR("Link %s does not have collision geometry description. Add collision geometry description for link to urdf.",link_name.c_str());
00068     return false;
00069   }
00070   if(link->collision->geometry->type != urdf::Geometry::CYLINDER)
00071   {
00072     ROS_ERROR("Link %s does not have cylinder geometry",link_name.c_str());
00073     return false;
00074   }
00075   wheel_radius_ = (dynamic_cast<urdf::Cylinder*>(link->collision->geometry.get()))->radius;
00076   ROS_DEBUG("wheel name: %s, radius: %f",link_name.c_str(),wheel_radius_);
00077   link_name_ = link_name;
00078   joint_name_ = link->parent_joint->name;
00079 
00080   joint_ = robot_state->getJointState(joint_name_);
00081   if (!joint_){
00082     ROS_ERROR("Could not find joint with name %s",joint_name_.c_str());
00083     return false;
00084   }
00085 
00086   urdf::Vector3 offset = link->parent_joint->parent_to_joint_origin_transform.position;
00087   offset_.x = offset.x;
00088   offset_.y = offset.y;
00089   offset_.z = offset.z;
00090 //  node.param<double> ("wheel_radius_scaler", wheel_radius_scaler_, 1.0);
00091   ROS_DEBUG("Loading wheel: %s",link_name_.c_str());
00092   ROS_DEBUG("offset_.x: %f, offset_.y: %f, offset_.z: %f", offset_.x, offset_.y, offset_.z);
00093   return true;
00094 }
00095 
00096 bool Caster::init(pr2_mechanism_model::RobotState *robot_state,  ros::NodeHandle &node, std::string link_name)
00097 {
00098   caster_stuck_ = 0;
00099   caster_speed_ = 0;
00100   caster_speed_filtered_ = 0;
00101   caster_speed_error_ = 0;
00102   caster_position_error_ = 0;
00103   steer_angle_stored_ = 0;
00104   steer_velocity_desired_ = 0;
00105   steer_angle_actual_ = 0;
00106   num_children_ = 0;
00107 
00108   boost::shared_ptr<const urdf::Link> link;
00109   link = robot_state->model_->robot_model_.getLink(link_name);
00110   if (!link){
00111     ROS_ERROR("Could not find link with name %s",link_name.c_str());
00112     return false;
00113   }
00114   ROS_DEBUG("caster name: %s",link_name.c_str());
00115   link_name_ = link_name;
00116   joint_name_ = link->parent_joint->name;
00117 
00118   joint_ = robot_state->getJointState(joint_name_);
00119   if (!joint_){
00120     ROS_ERROR("Could not find joint with name %s",joint_name_.c_str());
00121     return false;
00122   }
00123   urdf::Vector3 offset = link->parent_joint->parent_to_joint_origin_transform.position;
00124   offset_.x = offset.x;
00125   offset_.y = offset.y;
00126   offset_.z = offset.z;
00127 
00128   for(unsigned int i=0; i < link->child_links.size(); i++)
00129   {
00130     boost::shared_ptr<urdf::Link> child = link->child_links[i];
00131     Wheel tmp;
00132     parent_->wheel_.push_back(tmp);
00133     if(!parent_->wheel_[parent_->num_wheels_].init(robot_state, node, child->name))
00134     {
00135       ROS_ERROR("Could not initialize caster %s",link_name.c_str());
00136       return false;
00137     }
00138     parent_->num_wheels_++;
00139     num_children_++;
00140   }
00141   return true;
00142 }
00143 
00144 bool BaseKinematics::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node)
00145 {
00146   std::string caster_names_string;
00147   std::vector<std::string> caster_names;
00148   name_ = node.getNamespace();
00149   //Initialize stuff
00150   MAX_DT_ = 0.01;
00151   num_wheels_ = 0;
00152   num_casters_ = 0;
00153 
00154   robot_state_ = robot_state;
00155 
00156   node.param<std::string> ("caster_names",caster_names_string,"");
00157   std::stringstream ss(caster_names_string);
00158   std::string tmp;
00159   while(ss >> tmp)
00160   {
00161     caster_names.push_back(tmp);
00162   }
00163 
00164   for(unsigned int i=0; i < caster_names.size(); i++)
00165   {
00166     Caster tmp;
00167     caster_.push_back(tmp);
00168     caster_[num_casters_].parent_ = this;
00169     ROS_DEBUG("caster name: %s",caster_names[i].c_str());
00170     ros::NodeHandle n(name_);
00171     if(!caster_[num_casters_].init(robot_state, n, caster_names[i]))
00172     {
00173       ROS_ERROR("Could not initialize base kinematics");
00174       return false;
00175     }
00176     num_casters_++;
00177   }
00178 
00179   //  node.param<double> ("wheel_radius", wheel_radius_, 0.074792);
00180   double multiplier;
00181   node.param<double> ("wheel_radius_multiplier", multiplier, 1.0);
00182   int wheel_counter = 0;
00183   for(int j = 0; j < num_casters_; j++)
00184   {
00185     for(int i = 0; i < caster_[j].num_children_; i++)
00186     {
00187       wheel_[wheel_counter].parent_ = &(caster_[j]);
00188       wheel_[wheel_counter].wheel_radius_*=multiplier;
00189       wheel_counter++;
00190     }
00191   }
00192   return true;
00193 }
00194 
00195 void Wheel::updatePosition()
00196 {
00197   geometry_msgs::Point result = parent_->offset_;
00198   double costh = cos(parent_->joint_->position_);
00199   double sinth = sin(parent_->joint_->position_);
00200   result.x += costh * offset_.x - sinth * offset_.y;
00201   result.y += sinth * offset_.x + costh * offset_.y;
00202   result.z = 0.0;
00203   position_ = result;
00204 }
00205 
00206 void BaseKinematics::computeWheelPositions()
00207 {
00208   for(int i = 0; i < num_wheels_; i++)
00209   {
00210     wheel_[i].updatePosition();
00211   }
00212 
00213 }
00214 
00215 geometry_msgs::Twist BaseKinematics::pointVel2D(const geometry_msgs::Point& pos, const geometry_msgs::Twist& vel)
00216 {
00217   geometry_msgs::Twist result;
00218   result.linear.x = vel.linear.x - pos.y * vel.angular.z;
00219   result.linear.y = vel.linear.y + pos.x * vel.angular.z;
00220   result.angular.z = 0;
00221   return result;
00222 }