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_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
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
00092 node_.param<double> ("max_translational_velocity", max_translational_velocity_,0.5);
00093 node_.param<double> ("max_rotational_velocity", max_rotational_velocity_, 10.0);
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);
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
00113 cmd_sub_ = node_.subscribe<geometry_msgs::Twist>("command", 1, &Pr2BaseController2::commandCallback, this);
00114
00115
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
00147 wheel_pid_controllers_.resize(base_kinematics_.num_wheels_);
00148
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
00159
00160
00161
00162
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;
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
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()
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
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] = 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(steer_angle_desired, base_kinematics_.caster_[i].joint_->position_);
00409 error_steer_m_pi = angles::shortest_angular_distance(steer_angle_desired_m_pi, base_kinematics_.caster_[i].joint_->position_);
00410
00411 if(fabs(error_steer_m_pi) < fabs(error_steer))
00412 {
00413 error_steer = error_steer_m_pi;
00414 steer_angle_desired = steer_angle_desired_m_pi;
00415 }
00416 base_kinematics_.caster_[i].steer_angle_desired_ = steer_angle_desired;
00417
00418
00419 double command = caster_position_pid_[i].updatePid(error_steer,filtered_velocity_[i],ros::Duration(dT));
00420 base_kinematics_.caster_[i].joint_->commanded_effort_ = command;
00421
00422 base_kinematics_.caster_[i].caster_position_error_ = error_steer;
00423 }
00424 }
00425
00426 void Pr2BaseController2::setDesiredCasterSteer()
00427 {
00428 for(int i = 0; i < base_kinematics_.num_casters_; i++)
00429 {
00430
00431 }
00432 }
00433
00434 void Pr2BaseController2::computeDesiredWheelSpeeds(const double &dT)
00435 {
00436 geometry_msgs::Twist wheel_point_velocity;
00437 geometry_msgs::Twist wheel_point_velocity_projected;
00438 geometry_msgs::Twist wheel_caster_steer_component;
00439 geometry_msgs::Twist caster_2d_velocity;
00440
00441 caster_2d_velocity.linear.x = 0;
00442 caster_2d_velocity.linear.y = 0;
00443 caster_2d_velocity.angular.z = 0;
00444
00445 for(int i = 0; i < base_kinematics_.num_wheels_; i++)
00446 {
00447 filtered_wheel_velocity_[i] = base_kinematics_.wheel_[i].joint_->velocity_;
00448 }
00449 wheel_vel_filter_.update(filtered_wheel_velocity_,filtered_wheel_velocity_);
00450
00451 double steer_angle_actual = 0;
00452 for(int i = 0; i < (int) base_kinematics_.num_wheels_; i++)
00453 {
00454 base_kinematics_.wheel_[i].updatePosition();
00455
00456 caster_2d_velocity.angular.z = base_kinematics_.wheel_[i].parent_->caster_position_error_;
00457 steer_angle_actual = base_kinematics_.wheel_[i].parent_->joint_->position_;
00458 wheel_point_velocity = base_kinematics_.pointVel2D(base_kinematics_.wheel_[i].position_, cmd_vel_);
00459 wheel_caster_steer_component = base_kinematics_.pointVel2D(base_kinematics_.wheel_[i].offset_, caster_2d_velocity);
00460
00461 double costh = cos(-steer_angle_actual);
00462 double sinth = sin(-steer_angle_actual);
00463
00464 wheel_point_velocity_projected.linear.x = costh * wheel_point_velocity.linear.x - sinth * wheel_point_velocity.linear.y;
00465 wheel_point_velocity_projected.linear.y = sinth * wheel_point_velocity.linear.x + costh * wheel_point_velocity.linear.y;
00466 base_kinematics_.wheel_[i].wheel_speed_cmd_ = (wheel_point_velocity_projected.linear.x) / (base_kinematics_.wheel_[i].wheel_radius_);
00467 double command = wheel_pid_controllers_[i].updatePid(wheel_caster_steer_component.linear.x/base_kinematics_.wheel_[i].wheel_radius_,filtered_wheel_velocity_[i]-base_kinematics_.wheel_[i].wheel_speed_cmd_,ros::Duration(dT));
00468 base_kinematics_.wheel_[i].joint_->commanded_effort_ = command;
00469 }
00470 }
00471
00472 void Pr2BaseController2::setDesiredWheelSpeeds()
00473 {
00474
00475
00476
00477
00478 }
00479
00480 void Pr2BaseController2::updateJointControllers()
00481 {
00482
00483
00484
00485
00486 }
00487
00488 void Pr2BaseController2::commandCallback(const geometry_msgs::TwistConstPtr& msg)
00489 {
00490 pthread_mutex_lock(&pr2_base_controller_lock_);
00491 base_vel_msg_ = *msg;
00492 this->setCommand(base_vel_msg_);
00493 pthread_mutex_unlock(&pr2_base_controller_lock_);
00494 }
00495 }