pr2_base_controller2.cpp
Go to the documentation of this 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/pr2_base_controller2.h"
00039 #include "pluginlib/class_list_macros.h"
00040 
00041 PLUGINLIB_EXPORT_CLASS( controller::Pr2BaseController2, pr2_controller_interface::Controller)
00042 
00043 namespace controller {
00044 
00045   const static double EPS = 1e-5;
00046 
00047 Pr2BaseController2::Pr2BaseController2()
00048 {
00049   //init variables
00050   cmd_vel_.linear.x = 0;
00051   cmd_vel_.linear.y = 0;
00052   cmd_vel_.angular.z = 0;
00053 
00054   desired_vel_.linear.x = 0;
00055   desired_vel_.linear.y = 0;
00056   desired_vel_.angular.z = 0;
00057 
00058   cmd_vel_t_.linear.x = 0;
00059   cmd_vel_t_.linear.y = 0;
00060   cmd_vel_t_.angular.z = 0;
00061 
00062   new_cmd_available_ = false;
00063   last_publish_time_ = ros::Time(0.0);
00064 
00065   pthread_mutex_init(&pr2_base_controller_lock_, NULL);
00066 }
00067 
00068 Pr2BaseController2::~Pr2BaseController2()
00069 {
00070   cmd_sub_.shutdown();
00071   cmd_sub_deprecated_.shutdown();
00072 }
00073 
00074 bool Pr2BaseController2::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00075 {
00076   if(!base_kinematics_.init(robot,n))
00077     return false;
00078   node_ = n;
00079   state_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_mechanism_controllers::BaseControllerState2>(n, base_kinematics_.name_ + "/state", 1));
00080 
00081   int num_joints = base_kinematics_.num_wheels_ + base_kinematics_.num_casters_;
00082   state_publisher_->msg_.joint_names.resize(num_joints);
00083   state_publisher_->msg_.joint_velocity_commanded.resize(num_joints);
00084   state_publisher_->msg_.joint_velocity_measured.resize(num_joints);
00085   state_publisher_->msg_.joint_effort_measured.resize(num_joints);
00086   state_publisher_->msg_.joint_command.resize(num_joints);
00087   state_publisher_->msg_.joint_effort_commanded.resize(num_joints);
00088   state_publisher_->msg_.joint_error.resize(num_joints);
00089   state_publisher_->msg_.joint_effort_error.resize(num_joints);
00090 
00091   //Get params from param server
00092   node_.param<double> ("max_translational_velocity", max_translational_velocity_,0.5);
00093   node_.param<double> ("max_rotational_velocity", max_rotational_velocity_, 10.0); //0.5
00094   node_.param<double> ("max_translational_acceleration/x", max_accel_.linear.x, .2);
00095   node_.param<double> ("max_translational_acceleration/y", max_accel_.linear.y, .2);
00096   node_.param<double> ("max_rotational_acceleration", max_accel_.angular.z, 10.0); //0.2
00097 
00098   node_.param<double> ("kp_caster_steer", kp_caster_steer_, 80.0);
00099   node_.param<double> ("timeout", timeout_, 1.0);
00100   node_.param<double> ("state_publish_rate", state_publish_rate_,2.0);
00101   if(state_publish_rate_ <= 0.0)
00102   {
00103     publish_state_ = false;
00104     state_publish_time_ = 0.0;
00105   }
00106   else
00107   {
00108     publish_state_ = true;
00109     state_publish_time_ = 1.0/state_publish_rate_;
00110   }
00111 
00112   //  cmd_sub_deprecated_ = root_handle_.subscribe<geometry_msgs::Twist>("cmd_vel", 1, &Pr2BaseController2::commandCallback, this);
00113   cmd_sub_ = node_.subscribe<geometry_msgs::Twist>("command", 1, &Pr2BaseController2::commandCallback, this);
00114 
00115   //casters
00116   caster_controller_.resize(base_kinematics_.num_casters_);
00117   caster_position_pid_.resize(base_kinematics_.num_casters_);
00118   for(int i = 0; i < base_kinematics_.num_casters_; i++)
00119   {
00120     control_toolbox::Pid p_i_d;
00121     state_publisher_->msg_.joint_names[i] = base_kinematics_.caster_[i].joint_name_;
00122     if(!p_i_d.init(ros::NodeHandle(node_, base_kinematics_.caster_[i].joint_name_+"/velocity_controller")))
00123     {
00124       ROS_ERROR("Could not initialize pid for %s",base_kinematics_.caster_[i].joint_name_.c_str());
00125       return false;
00126     }
00127 
00128     if(!caster_position_pid_[i].init(ros::NodeHandle(node_, base_kinematics_.caster_[i].joint_name_+"/position_controller")))
00129     {
00130       ROS_ERROR("Could not initialize position pid controller for %s",base_kinematics_.caster_[i].joint_name_.c_str());
00131       return false;
00132     }
00133     caster_controller_[i].reset(new JointVelocityController());
00134     if(!caster_controller_[i]->init(base_kinematics_.robot_state_, base_kinematics_.caster_[i].joint_name_, p_i_d))
00135     {
00136       ROS_ERROR("Could not initialize pid for %s",base_kinematics_.caster_[i].joint_name_.c_str());
00137       return false;
00138     }
00139     if (!caster_controller_[i]->joint_state_->calibrated_)
00140     {
00141       ROS_ERROR("Caster joint \"%s\" not calibrated (namespace: %s)",
00142                 base_kinematics_.caster_[i].joint_name_.c_str(), node_.getNamespace().c_str());
00143       return false;
00144     }
00145   }
00146   //wheels
00147   wheel_pid_controllers_.resize(base_kinematics_.num_wheels_);
00148   //  wheel_controller_.resize(base_kinematics_.num_wheels_);
00149   for(int j = 0; j < base_kinematics_.num_wheels_; j++)
00150   {
00151     control_toolbox::Pid p_i_d;
00152     state_publisher_->msg_.joint_names[j + base_kinematics_.num_casters_] = base_kinematics_.wheel_[j].joint_name_;
00153     if(!wheel_pid_controllers_[j].init(ros::NodeHandle(node_,base_kinematics_.wheel_[j].joint_name_)))
00154     {
00155       ROS_ERROR("Could not initialize pid for %s",base_kinematics_.wheel_[j].joint_name_.c_str());
00156       return false;
00157     }
00158     /*    wheel_controller_[j].reset(new JointVelocityController());
00159    if(!wheel_controller_[j]->init(base_kinematics_.robot_state_, base_kinematics_.wheel_[j].joint_name_, p_i_d))
00160    {
00161       ROS_ERROR("Could not initialize joint velocity controller for %s",base_kinematics_.wheel_[j].joint_name_.c_str());
00162       return false;
00163       }*/
00164   }
00165   for(int i = 0; i < base_kinematics_.num_casters_; ++i)
00166   {
00167     if(!base_kinematics_.caster_[i].joint_->calibrated_)
00168     {
00169       ROS_ERROR("The Base controller could not start because the casters were not calibrated. Relaunch the base controller after you see the caster calibration finish.");
00170       return false; // Casters are not calibrated
00171     }
00172   }
00173 
00174   if (!((filters::MultiChannelFilterBase<double>&)caster_vel_filter_).configure(base_kinematics_.num_casters_, std::string("caster_velocity_filter"), node_)){
00175      ROS_ERROR("BaseController: could not configure velocity filters for casters");
00176      return false;
00177   }
00178   if (!((filters::MultiChannelFilterBase<double>&)wheel_vel_filter_).configure(base_kinematics_.num_wheels_, std::string("wheel_velocity_filter"), node_)){
00179      ROS_ERROR("BaseController: could not configure velocity filters for wheels");
00180      return false;
00181   }
00182   filtered_velocity_.resize(base_kinematics_.num_casters_);
00183   filtered_wheel_velocity_.resize(base_kinematics_.num_wheels_);
00184   return true;
00185 }
00186 
00187 // Set the base velocity command
00188 void Pr2BaseController2::setCommand(const geometry_msgs::Twist &cmd_vel)
00189 {
00190   double vel_mag = sqrt(cmd_vel.linear.x * cmd_vel.linear.x + cmd_vel.linear.y * cmd_vel.linear.y);
00191   double clamped_vel_mag = filters::clamp(vel_mag,-max_translational_velocity_, max_translational_velocity_);
00192   if(vel_mag > EPS)
00193   {
00194     cmd_vel_t_.linear.x = cmd_vel.linear.x * clamped_vel_mag / vel_mag;
00195     cmd_vel_t_.linear.y = cmd_vel.linear.y * clamped_vel_mag / vel_mag;
00196   }
00197   else
00198   {
00199     cmd_vel_t_.linear.x = 0.0;
00200     cmd_vel_t_.linear.y = 0.0;
00201   }
00202   cmd_vel_t_.angular.z = filters::clamp(cmd_vel.angular.z, -max_rotational_velocity_, max_rotational_velocity_);
00203   cmd_received_timestamp_ = base_kinematics_.robot_state_->getTime();
00204 
00205   ROS_DEBUG("BaseController:: command received: %f %f %f",cmd_vel.linear.x,cmd_vel.linear.y,cmd_vel.angular.z);
00206   ROS_DEBUG("BaseController:: command current: %f %f %f", cmd_vel_.linear.x,cmd_vel_.linear.y,cmd_vel_.angular.z);
00207   ROS_DEBUG("BaseController:: clamped vel: %f", clamped_vel_mag);
00208   ROS_DEBUG("BaseController:: vel: %f", vel_mag);
00209 
00210   for(int i=0; i < (int) base_kinematics_.num_wheels_; i++)
00211   {
00212     ROS_DEBUG("BaseController:: wheel speed cmd:: %d %f",i,(base_kinematics_.wheel_[i].direction_multiplier_*base_kinematics_.wheel_[i].wheel_speed_cmd_));
00213   }
00214   for(int i=0; i < (int) base_kinematics_.num_casters_; i++)
00215   {
00216     ROS_DEBUG("BaseController:: caster speed cmd:: %d %f",i,(base_kinematics_.caster_[i].steer_velocity_desired_));
00217   }
00218   new_cmd_available_ = true;
00219 }
00220 
00221 geometry_msgs::Twist Pr2BaseController2::interpolateCommand(const geometry_msgs::Twist &start, const geometry_msgs::Twist &end, const geometry_msgs::Twist &max_rate, const double &dT)
00222 {
00223   geometry_msgs::Twist result;
00224   geometry_msgs::Twist alpha;
00225   double delta(0), max_delta(0);
00226 
00227   delta = end.linear.x - start.linear.x;
00228   max_delta = max_rate.linear.x * dT;
00229   if(fabs(delta) <= max_delta || max_delta < EPS)
00230     alpha.linear.x = 1;
00231   else
00232     alpha.linear.x = max_delta / fabs(delta);
00233 
00234   delta = end.linear.y - start.linear.y;
00235   max_delta = max_rate.linear.y * dT;
00236   if(fabs(delta) <= max_delta || max_delta < EPS)
00237     alpha.linear.y = 1;
00238   else
00239     alpha.linear.y = max_delta / fabs(delta);
00240 
00241   delta = end.angular.z - start.angular.z;
00242   max_delta = max_rate.angular.z * dT;
00243   if(fabs(delta) <= max_delta || max_delta < EPS)
00244     alpha.angular.z = 1;
00245   else
00246     alpha.angular.z = max_delta / fabs(delta);
00247 
00248   double alpha_min = alpha.linear.x;
00249   if(alpha.linear.y < alpha_min)
00250     alpha_min = alpha.linear.y;
00251   if(alpha.angular.z < alpha_min)
00252     alpha_min = alpha.angular.z;
00253 
00254   result.linear.x = start.linear.x + alpha_min * (end.linear.x - start.linear.x);
00255   result.linear.y = start.linear.y + alpha_min * (end.linear.y - start.linear.y);
00256   result.angular.z = start.angular.z + alpha_min * (end.angular.z - start.angular.z);
00257   return result;
00258 }
00259 
00260 geometry_msgs::Twist Pr2BaseController2::getCommand()// Return the current velocity command
00261 {
00262   geometry_msgs::Twist cmd_vel;
00263   pthread_mutex_lock(&pr2_base_controller_lock_);
00264   cmd_vel.linear.x = cmd_vel_.linear.x;
00265   cmd_vel.linear.y = cmd_vel_.linear.y;
00266   cmd_vel.angular.z = cmd_vel_.angular.z;
00267   pthread_mutex_unlock(&pr2_base_controller_lock_);
00268   return cmd_vel;
00269 }
00270 
00271 void Pr2BaseController2::starting()
00272 {
00273   last_time_ = base_kinematics_.robot_state_->getTime();
00274   cmd_received_timestamp_ = base_kinematics_.robot_state_->getTime();
00275   for(int i = 0; i < base_kinematics_.num_casters_; i++)
00276   {
00277     caster_controller_[i]->starting();
00278   }
00279   for(int j = 0; j < base_kinematics_.num_wheels_; j++)
00280   {
00281     //    wheel_controller_[j]->starting();
00282   }
00283 }
00284 
00285 void Pr2BaseController2::update()
00286 {
00287   ros::Time current_time = base_kinematics_.robot_state_->getTime();
00288   double dT = std::min<double>((current_time - last_time_).toSec(), base_kinematics_.MAX_DT_);
00289 
00290   if(new_cmd_available_)
00291   {
00292     if(pthread_mutex_trylock(&pr2_base_controller_lock_) == 0)
00293     {
00294       desired_vel_.linear.x = cmd_vel_t_.linear.x;
00295       desired_vel_.linear.y = cmd_vel_t_.linear.y;
00296       desired_vel_.angular.z = cmd_vel_t_.angular.z;
00297       new_cmd_available_ = false;
00298       pthread_mutex_unlock(&pr2_base_controller_lock_);
00299     }
00300   }
00301 
00302   if((current_time - cmd_received_timestamp_).toSec() > timeout_)
00303   {
00304     cmd_vel_.linear.x = 0;
00305     cmd_vel_.linear.y = 0;
00306     cmd_vel_.angular.z = 0;
00307   }
00308   else
00309     cmd_vel_ = interpolateCommand(cmd_vel_, desired_vel_, max_accel_, dT);
00310 
00311   computeJointCommands(dT);
00312 
00313   setJointCommands();
00314 
00315   updateJointControllers();
00316 
00317   if(publish_state_)
00318     publishState(current_time);
00319 
00320   last_time_ = current_time;
00321 
00322 }
00323 
00324 void Pr2BaseController2::publishState(const ros::Time &time)
00325 {
00326   if((time - last_publish_time_).toSec()  < state_publish_time_)
00327   {
00328     return;
00329   }
00330 
00331   if(state_publisher_->trylock())
00332   {
00333     state_publisher_->msg_.command.linear.x  = cmd_vel_.linear.x;
00334     state_publisher_->msg_.command.linear.y  = cmd_vel_.linear.y;
00335     state_publisher_->msg_.command.angular.z = cmd_vel_.angular.z;
00336 
00337     for(int i = 0; i < base_kinematics_.num_casters_; i++)
00338     {
00339       state_publisher_->msg_.joint_names[i] = base_kinematics_.caster_[i].joint_name_;
00340       state_publisher_->msg_.joint_velocity_measured[i] = base_kinematics_.caster_[i].joint_->velocity_;
00341       state_publisher_->msg_.joint_command[i]= base_kinematics_.caster_[i].steer_angle_desired_;
00342       state_publisher_->msg_.joint_error[i]  = base_kinematics_.caster_[i].joint_->position_ - base_kinematics_.caster_[i].steer_angle_desired_;
00343 
00344       state_publisher_->msg_.joint_effort_measured[i]  = base_kinematics_.caster_[i].joint_->measured_effort_;
00345       state_publisher_->msg_.joint_effort_commanded[i] = base_kinematics_.caster_[i].joint_->commanded_effort_;
00346       state_publisher_->msg_.joint_effort_error[i]     = base_kinematics_.caster_[i].joint_->measured_effort_ - base_kinematics_.caster_[i].joint_->commanded_effort_;
00347     }
00348     for(int i = 0; i < base_kinematics_.num_wheels_; i++)
00349     {
00350       state_publisher_->msg_.joint_names[i+base_kinematics_.num_casters_] = base_kinematics_.wheel_[i].joint_name_;
00351       state_publisher_->msg_.joint_velocity_commanded[i+base_kinematics_.num_casters_] = base_kinematics_.wheel_[i].wheel_speed_cmd_;
00352       state_publisher_->msg_.joint_velocity_measured[i+base_kinematics_.num_casters_] = base_kinematics_.wheel_[i].joint_->velocity_;
00353       state_publisher_->msg_.joint_command[i+base_kinematics_.num_casters_]= base_kinematics_.wheel_[i].joint_->velocity_-base_kinematics_.wheel_[i].wheel_speed_cmd_;
00354       state_publisher_->msg_.joint_error[i+base_kinematics_.num_casters_]    = base_kinematics_.wheel_[i].wheel_speed_cmd_;
00355 
00356       state_publisher_->msg_.joint_effort_measured[i+base_kinematics_.num_casters_]  = base_kinematics_.wheel_[i].joint_->measured_effort_;
00357       state_publisher_->msg_.joint_effort_commanded[i+base_kinematics_.num_casters_] = base_kinematics_.wheel_[i].joint_->commanded_effort_;
00358       state_publisher_->msg_.joint_effort_error[i+base_kinematics_.num_casters_]     = base_kinematics_.wheel_[i].joint_->measured_effort_ - base_kinematics_.wheel_[i].joint_->commanded_effort_;
00359     }
00360     state_publisher_->unlockAndPublish();
00361     last_publish_time_ = time;
00362   }
00363 }
00364 
00365 void Pr2BaseController2::computeJointCommands(const double &dT)
00366 {
00367   base_kinematics_.computeWheelPositions();
00368 
00369   computeDesiredCasterSteer(dT);
00370 
00371   computeDesiredWheelSpeeds(dT);
00372 }
00373 
00374 void Pr2BaseController2::setJointCommands()
00375 {
00376   setDesiredCasterSteer();
00377 
00378   setDesiredWheelSpeeds();
00379 }
00380 
00381 void Pr2BaseController2::computeDesiredCasterSteer(const double &dT)
00382 {
00383   geometry_msgs::Twist result;
00384 
00385   double steer_angle_desired(0.0), steer_angle_desired_m_pi(0.0);
00386   double error_steer(0.0), error_steer_m_pi(0.0);
00387   double trans_vel = sqrt(cmd_vel_.linear.x * cmd_vel_.linear.x + cmd_vel_.linear.y * cmd_vel_.linear.y);
00388 
00389   for(int i = 0; i < base_kinematics_.num_casters_; i++)
00390   {
00391     filtered_velocity_[i] = 0.0 - base_kinematics_.caster_[i].joint_->velocity_;
00392   }
00393   caster_vel_filter_.update(filtered_velocity_,filtered_velocity_);
00394 
00395   for(int i = 0; i < base_kinematics_.num_casters_; i++)
00396   {
00397     result = base_kinematics_.pointVel2D(base_kinematics_.caster_[i].offset_, cmd_vel_);
00398     if(trans_vel < EPS && fabs(cmd_vel_.angular.z) < EPS)
00399     {
00400       steer_angle_desired = base_kinematics_.caster_[i].steer_angle_stored_;
00401     }
00402     else
00403     {
00404       steer_angle_desired = atan2(result.linear.y, result.linear.x);
00405       base_kinematics_.caster_[i].steer_angle_stored_ = steer_angle_desired;
00406     }
00407     steer_angle_desired_m_pi = angles::normalize_angle(steer_angle_desired + M_PI);
00408     error_steer = angles::shortest_angular_distance(
00409           base_kinematics_.caster_[i].joint_->position_,
00410           steer_angle_desired);
00411     error_steer_m_pi = angles::shortest_angular_distance(
00412           base_kinematics_.caster_[i].joint_->position_,
00413           steer_angle_desired_m_pi);
00414 
00415     if(fabs(error_steer_m_pi) < fabs(error_steer))
00416     {
00417       error_steer = error_steer_m_pi;
00418       steer_angle_desired = steer_angle_desired_m_pi;
00419     }
00420     base_kinematics_.caster_[i].steer_angle_desired_ = steer_angle_desired;
00421     double command = caster_position_pid_[i].computeCommand(error_steer,
00422           filtered_velocity_[i], ros::Duration(dT));
00423     base_kinematics_.caster_[i].joint_->commanded_effort_ = command;
00424 
00425     base_kinematics_.caster_[i].caster_position_error_ = error_steer;
00426   }
00427 }
00428 
00429 void Pr2BaseController2::setDesiredCasterSteer()
00430 {
00431   for(int i = 0; i < base_kinematics_.num_casters_; i++)
00432   {
00433     //    caster_controller_[i]->setCommand(base_kinematics_.caster_[i].steer_velocity_desired_);
00434   }
00435 }
00436 
00437 void Pr2BaseController2::computeDesiredWheelSpeeds(const double &dT)
00438 {
00439   geometry_msgs::Twist wheel_point_velocity;
00440   geometry_msgs::Twist wheel_point_velocity_projected;
00441   geometry_msgs::Twist wheel_caster_steer_component;
00442   geometry_msgs::Twist caster_2d_velocity;
00443 
00444   caster_2d_velocity.linear.x = 0;
00445   caster_2d_velocity.linear.y = 0;
00446   caster_2d_velocity.angular.z = 0;
00447 
00448   for(int i = 0; i < base_kinematics_.num_wheels_; i++)
00449   {
00450     filtered_wheel_velocity_[i] = base_kinematics_.wheel_[i].joint_->velocity_;
00451   }
00452   wheel_vel_filter_.update(filtered_wheel_velocity_,filtered_wheel_velocity_);
00453 
00454   double steer_angle_actual = 0;
00455   for(int i = 0; i < (int) base_kinematics_.num_wheels_; i++)
00456   {
00457     base_kinematics_.wheel_[i].updatePosition();
00458     //    caster_2d_velocity.angular.z = base_kinematics_.wheel_[i].parent_->steer_velocity_desired_;
00459     caster_2d_velocity.angular.z = 0.0 - base_kinematics_.wheel_[i].parent_->caster_position_error_;
00460     steer_angle_actual = base_kinematics_.wheel_[i].parent_->joint_->position_;
00461     wheel_point_velocity = base_kinematics_.pointVel2D(base_kinematics_.wheel_[i].position_, cmd_vel_);
00462     wheel_caster_steer_component = base_kinematics_.pointVel2D(base_kinematics_.wheel_[i].offset_, caster_2d_velocity);
00463 
00464     double costh = cos(-steer_angle_actual);
00465     double sinth = sin(-steer_angle_actual);
00466 
00467     wheel_point_velocity_projected.linear.x = costh * wheel_point_velocity.linear.x - sinth * wheel_point_velocity.linear.y;
00468     wheel_point_velocity_projected.linear.y = sinth * wheel_point_velocity.linear.x + costh * wheel_point_velocity.linear.y;
00469     base_kinematics_.wheel_[i].wheel_speed_cmd_ = (wheel_point_velocity_projected.linear.x) / (base_kinematics_.wheel_[i].wheel_radius_);
00470     double command = wheel_pid_controllers_[i].computeCommand(
00471           - wheel_caster_steer_component.linear.x/base_kinematics_.wheel_[i].wheel_radius_,
00472           base_kinematics_.wheel_[i].wheel_speed_cmd_ - filtered_wheel_velocity_[i],
00473           ros::Duration(dT));
00474     base_kinematics_.wheel_[i].joint_->commanded_effort_ = command;
00475   }
00476 }
00477 
00478 void Pr2BaseController2::setDesiredWheelSpeeds()
00479 {
00480   /*  for(int i = 0; i < (int) base_kinematics_.num_wheels_; i++)
00481   {
00482    wheel_controller_[i]->setCommand(base_kinematics_.wheel_[i].direction_multiplier_ * base_kinematics_.wheel_[i].wheel_speed_cmd_);
00483     }*/
00484 }
00485 
00486 void Pr2BaseController2::updateJointControllers()
00487 {
00488   /*  for(int i = 0; i < base_kinematics_.num_wheels_; i++)
00489       wheel_controller_[i]->update();
00490   for(int i = 0; i < base_kinematics_.num_casters_; i++)
00491   caster_controller_[i]->update();*/
00492 }
00493 
00494 void Pr2BaseController2::commandCallback(const geometry_msgs::TwistConstPtr& msg)
00495 {
00496   pthread_mutex_lock(&pr2_base_controller_lock_);
00497   base_vel_msg_ = *msg;
00498   this->setCommand(base_vel_msg_);
00499   pthread_mutex_unlock(&pr2_base_controller_lock_);
00500 }
00501 } // namespace


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Thu Aug 27 2015 14:22:52