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