00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include "pr2_mechanism_controllers/pr2_base_controller.h"
00039 #include "pluginlib/class_list_macros.h"
00040
00041 PLUGINLIB_EXPORT_CLASS( controller::Pr2BaseController, pr2_controller_interface::Controller)
00042
00043 namespace controller {
00044
00045 const static double EPS = 1e-5;
00046
00047 Pr2BaseController::Pr2BaseController()
00048 {
00049
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_.joint_names.resize(num_joints);
00083 state_publisher_->msg_.joint_velocity_measured.resize(num_joints);
00084 state_publisher_->msg_.joint_effort_measured.resize(num_joints);
00085 state_publisher_->msg_.joint_velocity_commanded.resize(num_joints);
00086 state_publisher_->msg_.joint_effort_commanded.resize(num_joints);
00087 state_publisher_->msg_.joint_velocity_error.resize(num_joints);
00088 state_publisher_->msg_.joint_effort_error.resize(num_joints);
00089
00090
00091 node_.param<double> ("max_translational_velocity", max_translational_velocity_,0.5);
00092 node_.param<double> ("max_rotational_velocity", max_rotational_velocity_, 10.0);
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);
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
00112 cmd_sub_ = node_.subscribe<geometry_msgs::Twist>("command", 1, &Pr2BaseController::commandCallback, this);
00113
00114
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
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;
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
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()
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
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 }