$search
00001 00012 /************************************************************************ 00013 * Copyright (C) 2012 Eindhoven University of Technology (TU/e). * 00014 * All rights reserved. * 00015 ************************************************************************ 00016 * Redistribution and use in source and binary forms, with or without * 00017 * modification, are permitted provided that the following conditions * 00018 * are met: * 00019 * * 00020 * 1. Redistributions of source code must retain the above * 00021 * copyright notice, this list of conditions and the following * 00022 * disclaimer. * 00023 * * 00024 * 2. Redistributions in binary form must reproduce the above * 00025 * copyright notice, this list of conditions and the following * 00026 * disclaimer in the documentation and/or other materials * 00027 * provided with the distribution. * 00028 * * 00029 * THIS SOFTWARE IS PROVIDED BY TU/e "AS IS" AND ANY EXPRESS OR * 00030 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * 00031 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 00032 * ARE DISCLAIMED. IN NO EVENT SHALL TU/e OR CONTRIBUTORS BE LIABLE * 00033 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * 00034 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT * 00035 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; * 00036 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * 00037 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 00038 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE * 00039 * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH * 00040 * DAMAGE. * 00041 * * 00042 * The views and conclusions contained in the software and * 00043 * documentation are those of the authors and should not be * 00044 * interpreted as representing official policies, either expressed or * 00045 * implied, of TU/e. * 00046 ************************************************************************/ 00047 00048 #include "amigo_gazebo/base_controller.h" 00049 #include <pluginlib/class_list_macros.h> 00050 00051 #define WHEELRAD 0.06 00052 #define DIS2CENT 0.28991378 // distance of wheel to center 00053 #define HALFSQRT2 0.7071 00054 00056 PLUGINLIB_REGISTER_CLASS(BaseControllerPlugin, 00057 controller::BaseController, 00058 pr2_controller_interface::Controller) 00059 00060 using namespace controller; 00061 00062 const static double EPS = 1e-8; 00063 00064 BaseController::BaseController() { 00065 00066 //set all variables to zero 00067 00068 current_pos_x=0; 00069 current_pos_y=0; 00070 current_pos_phi=0; 00071 00072 desired_pos_x=0; 00073 desired_pos_y=0; 00074 desired_pos_phi=0; 00075 00076 ref_vel_last_.linear.x=0.0; 00077 ref_vel_last_.linear.y=0.0; 00078 ref_vel_last_.angular.z=0.0; 00079 } 00080 00081 BaseController::~BaseController() { 00082 } 00083 00084 bool BaseController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) { 00085 00086 std::string wheel_inner_left_front, wheel_inner_right_front, wheel_inner_left_rear, wheel_inner_right_rear; 00087 00088 //Get params from parameter server or set defaults 00089 n.param<double> ("max_translational_velocity", max_translational_velocity_,0.3); 00090 n.param<double> ("min_translational_velocity", trans_vel_min,0.0); 00091 n.param<double> ("max_rotational_velocity", max_rotational_velocity_, 0.5); 00092 n.param<double> ("min_rotational_velocity", rot_vel_min,0.0); 00093 n.param<double> ("max_translational_acceleration/x", max_accel_.linear.x, .25); 00094 n.param<double> ("max_translational_acceleration/y", max_accel_.linear.y, .25); 00095 n.param<double> ("max_rotational_acceleration", max_accel_.angular.z, .2); 00096 n.param<double> ("max_control_effort_x", u_x_max,1000.0); 00097 n.param<double> ("max_control_effort_y", u_y_max, 1000.0); 00098 n.param<double> ("max_control_effort_phi", u_phi_max,1000.0); 00099 n.param<double> ("timeout", timeout_, 1.0); 00100 00101 // feedback 00102 n.param<double> ("Kp_x", Kp_x, 0.0); 00103 n.param<double> ("Kd_x", Kd_x, 0.0); 00104 00105 n.param<double> ("Kp_y", Kp_y, 0.0); 00106 n.param<double> ("Kd_y", Kd_y, 0.0); 00107 00108 n.param<double> ("Kp_phi", Kp_phi, 0.0); 00109 n.param<double> ("Kd_phi", Kd_phi, 0.0); 00110 00111 // feed forward 00112 n.param<double> ("mass_x", mass_x, 0.0); 00113 n.param<double> ("mass_y", mass_y, 0.0); 00114 n.param<double> ("inertia_phi", inertia_phi, 0.0); 00115 00116 n.param<double> ("damping_x", damping_x, 0.0); 00117 n.param<double> ("damping_y", damping_y, 0.0); 00118 n.param<double> ("damping_phi", damping_phi, 0.0); 00119 00120 //wheel_inner_left_front 00121 if (!n.getParam("wheel_inner_left_front", wheel_inner_left_front)) 00122 { 00123 ROS_ERROR("No joint given in namespace: '%s')", 00124 n.getNamespace().c_str()); 00125 return false; 00126 } 00127 00128 wheel_inner_left_front_state = robot->getJointState(wheel_inner_left_front); 00129 if (!wheel_inner_left_front_state ) 00130 { 00131 ROS_ERROR("BaseController could not find joint named '%s'", 00132 wheel_inner_left_front.c_str()); 00133 return false; 00134 } 00135 00136 //wheel_inner_right_front 00137 if (!n.getParam("wheel_inner_right_front", wheel_inner_right_front)) 00138 { 00139 ROS_ERROR("No joint given in namespace: '%s')", 00140 n.getNamespace().c_str()); 00141 return false; 00142 } 00143 00144 wheel_inner_right_front_state = robot->getJointState(wheel_inner_right_front); 00145 if (!wheel_inner_right_front_state ) 00146 { 00147 ROS_ERROR("BaseController could not find joint named '%s'", 00148 wheel_inner_right_front.c_str()); 00149 return false; 00150 } 00151 00152 //wheel_inner_left_rear 00153 if (!n.getParam("wheel_inner_left_rear", wheel_inner_left_rear)) 00154 { 00155 ROS_ERROR("No joint given in namespace: '%s')", 00156 n.getNamespace().c_str()); 00157 return false; 00158 } 00159 00160 wheel_inner_left_rear_state = robot->getJointState(wheel_inner_left_rear); 00161 if (!wheel_inner_left_rear_state ) 00162 { 00163 ROS_ERROR("BaseController could not find joint named '%s'", wheel_inner_left_rear.c_str()); 00164 return false; 00165 } 00166 00167 //wheel_inner_right_rear 00168 if (!n.getParam("wheel_inner_right_rear", wheel_inner_right_rear)) 00169 { 00170 ROS_ERROR("No joint given in namespace: '%s')", n.getNamespace().c_str()); 00171 return false; 00172 } 00173 00174 wheel_inner_right_rear_state = robot->getJointState(wheel_inner_right_rear); 00175 if (!wheel_inner_right_rear_state ) 00176 { 00177 ROS_ERROR("BaseController could not find joint named '%s'", wheel_inner_right_rear.c_str()); 00178 return false; 00179 } 00180 00181 //set topic 00182 vel_sub = n.subscribe("/cmd_vel", 1000, &BaseController::VelCallback,this); 00183 00184 // copy robot pointer so we can access time 00185 robot_ = robot; 00186 00187 return true; 00188 } 00189 00190 void BaseController::starting() { 00191 time_of_last_cycle_ = robot_->getTime(); 00192 } 00193 00194 void BaseController::update() { 00195 current_time = robot_->getTime(); 00196 double dT = std::min<double>((current_time - time_of_last_cycle_).toSec(), 0.02); 00197 00198 geometry_msgs::Twist ref_vel; 00199 00200 //if no command received in timeout period 00201 if((current_time - cmd_received_timestamp_).toSec() > timeout_) 00202 { 00203 ref_vel.linear.x = 0; 00204 ref_vel.linear.y = 0; 00205 ref_vel.angular.z = 0; 00206 } 00207 else 00208 { 00209 //interpolate and limit command velocities 00210 ref_vel = interpolateCommand(ref_vel_last_, cmd_vel_, max_accel_, dT); 00211 } 00212 00213 00214 //wheel velocity conversion to current x, y and phi velocities 00215 geometry_msgs::Twist current_vel; 00216 current_vel.linear.x = 0.25*WHEELRAD*(wheel_inner_right_front_state->velocity_+wheel_inner_left_front_state->velocity_-wheel_inner_left_rear_state->velocity_-wheel_inner_right_rear_state->velocity_)*1/HALFSQRT2; 00217 current_vel.linear.y = 0.25*WHEELRAD*(-wheel_inner_right_front_state->velocity_+wheel_inner_left_front_state->velocity_+wheel_inner_left_rear_state->velocity_-wheel_inner_right_rear_state->velocity_)*1/HALFSQRT2; 00218 current_vel.angular.z = -0.25*WHEELRAD/DIS2CENT*(wheel_inner_right_front_state->velocity_+wheel_inner_left_front_state->velocity_+wheel_inner_left_rear_state->velocity_+wheel_inner_right_rear_state->velocity_); 00219 00220 /*TODO: Filtering to get better velocity signal, or use transfer function as controller ///////////////////////////////////////////////////////////// 00221 chain.update(vel, vel_filt); 00222 */ 00223 00224 //compute translational and rotational velocity 00225 double trans_vel_mag_sq = current_vel.linear.x * current_vel.linear.x + current_vel.linear.y * current_vel.linear.y; 00226 double rot_vel_mag = fabs(current_vel.angular.z); 00227 00228 00229 double desired_acc_x, desired_acc_y, desired_acc_phi; 00230 double error_vel_x, error_vel_y, error_vel_phi; 00231 double error_pos_x, error_pos_y, error_pos_phi; 00232 00233 //if zero desired velocity and standing (as good as) still, set stuff to zero 00234 if (ref_vel.linear.x == 0 && ref_vel.linear.y == 0 && ref_vel.angular.z == 0 && trans_vel_mag_sq < trans_vel_min && rot_vel_mag < rot_vel_min){ 00235 //reset position 00236 current_pos_x = 0.0; 00237 current_pos_y = 0.0; 00238 current_pos_phi = 0.0; 00239 00240 desired_pos_x = 0.0; 00241 desired_pos_y = 0.0; 00242 desired_pos_phi = 0.0; 00243 00244 //reset acceleration 00245 desired_acc_x = 0.0; 00246 desired_acc_y = 0.0; 00247 desired_acc_phi = 0.0; 00248 00249 //set zero error 00250 error_vel_x = 0.0; 00251 error_vel_y = 0.0; 00252 error_vel_phi = 0.0; 00253 00254 error_pos_x = 0.0; 00255 error_pos_y = 0.0; 00256 error_pos_phi = 0.0; 00257 00258 } else { 00259 //compute desired positions 00260 desired_pos_x += ref_vel.linear.x * dT; 00261 desired_pos_y += ref_vel.linear.y * dT; 00262 desired_pos_phi += ref_vel.angular.z * dT; 00263 00264 //compute current positions 00265 current_pos_x += current_vel.linear.x * dT; 00266 current_pos_y += current_vel.linear.y * dT; 00267 current_pos_phi += current_vel.angular.z * dT; 00268 00269 //compute desired accelerations 00270 desired_acc_x = (ref_vel.linear.x - ref_vel_last_.linear.x) / dT; 00271 desired_acc_y = (ref_vel.linear.y - ref_vel_last_.linear.y) / dT; 00272 desired_acc_phi = (ref_vel.angular.z - ref_vel_last_.angular.z) / dT; 00273 00274 //compute position errors 00275 error_pos_x = desired_pos_x - current_pos_x; 00276 error_pos_y = desired_pos_y - current_pos_y; 00277 error_pos_phi = desired_pos_phi - current_pos_phi; 00278 00279 //compute velocity errors 00280 error_vel_x = ref_vel.linear.x - current_vel.linear.x; 00281 error_vel_y = ref_vel.linear.y - current_vel.linear.y; 00282 error_vel_phi = ref_vel.angular.z - current_vel.angular.z; 00283 } 00284 00285 //compute control effort 00286 double u_x = mass_x*desired_acc_x + damping_x * ref_vel.linear.x + Kp_x * error_pos_x + Kd_x * error_vel_x; 00287 double u_y = mass_y*desired_acc_y + damping_y * ref_vel.linear.y + Kp_y * error_pos_y + Kd_y * error_vel_y; 00288 double u_phi = -(inertia_phi * desired_acc_phi + damping_phi * ref_vel.angular.z + Kp_phi * error_pos_phi + Kd_phi * error_vel_phi); 00289 00290 //limit control effort 00291 u_x = std::max<double>(std::min<double>(u_x,u_x_max),-u_x_max); 00292 u_y = std::max<double>(std::min<double>(u_y,u_y_max),-u_y_max); 00293 u_phi = std::max<double>(std::min<double>(u_phi,u_phi_max),-u_phi_max); 00294 00295 //convert the control effort to the effort on the wheel torques 00296 wheel_inner_left_front_state->commanded_effort_ = 0.25*WHEELRAD*(+1/HALFSQRT2*u_x + 1/HALFSQRT2*u_y + 1/DIS2CENT*u_phi); 00297 wheel_inner_right_front_state->commanded_effort_ = 0.25*WHEELRAD*(1/HALFSQRT2*u_x - 1/HALFSQRT2*u_y + 1/DIS2CENT*u_phi); 00298 wheel_inner_left_rear_state->commanded_effort_ = 0.25*WHEELRAD*(-1/HALFSQRT2*u_x + 1/HALFSQRT2*u_y + 1/DIS2CENT*u_phi); 00299 wheel_inner_right_rear_state->commanded_effort_ = 0.25*WHEELRAD*(-1/HALFSQRT2*u_x - 1/HALFSQRT2*u_y + 1/DIS2CENT*u_phi); 00300 00301 //make current values last values 00302 time_of_last_cycle_ = robot_->getTime(); 00303 00304 ref_vel_last_ = ref_vel; 00305 } 00306 00307 00308 void BaseController::stopping() { 00309 00310 } 00311 00312 void BaseController::VelCallback(const geometry_msgs::Twist::ConstPtr& msg) { 00313 geometry_msgs::Twist base_vel_msg_ = *msg; 00314 00315 cmd_received_timestamp_ = robot_->getTime(); 00316 00317 ROS_DEBUG("BaseController:: command received: %f %f %f",base_vel_msg_.linear.x,base_vel_msg_.linear.y,base_vel_msg_.angular.z); 00318 ROS_DEBUG("BaseController:: command current: %f %f %f", ref_vel_last_.linear.x,ref_vel_last_.linear.y,ref_vel_last_.angular.z); 00319 00320 double vel_mag = sqrt(base_vel_msg_.linear.x * base_vel_msg_.linear.x + base_vel_msg_.linear.y * base_vel_msg_.linear.y); 00321 ROS_DEBUG("BaseController:: vel: %f", vel_mag); 00322 00323 if(vel_mag > EPS) 00324 { 00325 //limit translational velocity 00326 double clamped_vel_mag = std::max<double>(std::min<double>(vel_mag, max_translational_velocity_), -max_translational_velocity_); 00327 ROS_DEBUG("BaseController:: clamped vel: %f", clamped_vel_mag); 00328 00329 cmd_vel_.linear.x = base_vel_msg_.linear.x * clamped_vel_mag / vel_mag; 00330 cmd_vel_.linear.y = base_vel_msg_.linear.y * clamped_vel_mag / vel_mag; 00331 } 00332 else 00333 { 00334 ROS_DEBUG("Basecontroller:: angular velocity is smaller/equal to epsilon; %f", EPS); 00335 cmd_vel_.linear.x = 0.0; 00336 cmd_vel_.linear.y = 0.0; 00337 } 00338 00339 double ang_mag =(double)base_vel_msg_.angular.z; 00340 00341 //limit rotational velocity 00342 cmd_vel_.angular.z = std::max<double>(std::min<double>(ang_mag, max_rotational_velocity_), -max_rotational_velocity_); 00343 } 00344 00345 00346 //linear interpolation, accounting for acceleration limits 00347 geometry_msgs::Twist BaseController::interpolateCommand(const geometry_msgs::Twist &start, const geometry_msgs::Twist &end, const geometry_msgs::Twist &max_rate, const double &dT) { 00348 geometry_msgs::Twist result; //to return 00349 geometry_msgs::Twist alpha; //multiplier for delta 00350 geometry_msgs::Twist delta; //difference between start and end 00351 double max_delta(0); //maximum acceleration allowed 00352 00353 00354 delta.linear.x = end.linear.x - start.linear.x; 00355 delta.linear.y = end.linear.y - start.linear.y; 00356 delta.angular.z = end.angular.z - start.angular.z; 00357 00358 max_delta = max_rate.linear.x * dT; 00359 if(fabs(delta.linear.x) <= max_delta || max_delta < EPS) 00360 alpha.linear.x = 1; 00361 else 00362 alpha.linear.x = max_delta / fabs(delta.linear.x); 00363 00364 max_delta = max_rate.linear.y * dT; 00365 if(fabs(delta.linear.y) <= max_delta || max_delta < EPS) 00366 alpha.linear.y = 1; 00367 else 00368 alpha.linear.y = max_delta / fabs(delta.linear.y); 00369 00370 max_delta = max_rate.angular.z * dT; 00371 if(fabs(delta.angular.z) <= max_delta || max_delta < EPS) 00372 alpha.angular.z = 1; 00373 else 00374 alpha.angular.z = max_delta / fabs(delta.angular.z); 00375 00376 double alpha_min = std::min<double>(std::min<double>(alpha.linear.x, alpha.linear.y), alpha.angular.z); 00377 00378 result.linear.x = start.linear.x + alpha_min * (end.linear.x - start.linear.x); 00379 result.linear.y = start.linear.y + alpha_min * (end.linear.y - start.linear.y); 00380 result.angular.z = start.angular.z + alpha_min * (end.angular.z - start.angular.z); 00381 00382 return result; 00383 }