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 #include <labust/control/VelocityControl.hpp>
00038 #include <labust/tools/MatrixLoader.hpp>
00039 #include <labust/simulation/matrixfwd.hpp>
00040 #include <labust/simulation/DynamicsParams.hpp>
00041 #include <labust/tools/DynamicsLoader.hpp>
00042 #include <labust/math/NumberManipulation.hpp>
00043 #include <labust/tools/conversions.hpp>
00044
00045 #include <auv_msgs/BodyForceReq.h>
00046 #include <boost/bind.hpp>
00047
00048 #include <cmath>
00049 #include <vector>
00050 #include <string>
00051
00052 using labust::control::VelocityControl;
00053
00054 const std::string VelocityControl::dofName[]=
00055 {"Surge","Sway","Heave","Roll","Pitch","Yaw"};
00056
00057 VelocityControl::VelocityControl():
00058 nh(ros::NodeHandle()),
00059 ph(ros::NodeHandle("~")),
00060 lastEst(ros::Time::now()),
00061 lastRef(ros::Time::now()),
00062 lastMan(ros::Time::now()),
00063 lastMeas(ros::Time::now()),
00064 timeout(0.5),
00065 joy_scale(1),
00066 Ts(0.1),
00067 externalIdent(false),
00068 server(serverMux),
00069 doSafetyTest(true)
00070 {this->onInit();}
00071
00072 void VelocityControl::onInit()
00073 {
00074 std::string name;
00075
00076 tauOut = nh.advertise<auv_msgs::BodyForceReq>("tauOut", 1);
00077 tauAchW = nh.advertise<auv_msgs::BodyForceReq>("tauAchVelCon", 1);
00078
00079
00080 velocityRef = nh.subscribe<auv_msgs::BodyVelocityReq>("nuRef", 1,
00081 &VelocityControl::handleReference,this);
00082 stateHat = nh.subscribe<auv_msgs::NavSts>("stateHat", 1,
00083 &VelocityControl::handleEstimates,this);
00084 measSub = nh.subscribe<auv_msgs::NavSts>("meas", 1,
00085 &VelocityControl::handleMeasurement,this);
00086 tauAch = nh.subscribe<auv_msgs::BodyForceReq>("tauAch", 1,
00087 &VelocityControl::handleWindup,this);
00088 manualIn = nh.subscribe<sensor_msgs::Joy>("joy",1,
00089 &VelocityControl::handleManual,this);
00090 modelUpdate = nh.subscribe<navcon_msgs::ModelParamsUpdate>("model_update", 1,
00091 &VelocityControl::handleModelUpdate,this);
00092
00093 highLevelSelect = nh.advertiseService("ConfigureVelocityController",
00094 &VelocityControl::handleServerConfig, this);
00095 enableControl = nh.advertiseService("VelCon_enable",
00096 &VelocityControl::handleEnableControl, this);
00097
00098 nh.param("velocity_controller/joy_scale",joy_scale,joy_scale);
00099 nh.param("velocity_controller/timeout",timeout,timeout);
00100 nh.param("velocity_controller/external_ident",externalIdent, externalIdent);
00101
00102
00103 bool sim(false);
00104 nh.param("use_sim_time",sim,sim);
00105 doSafetyTest = !sim;
00106
00107 if (externalIdent)
00108 {
00109 identExt = nh.subscribe<auv_msgs::BodyForceReq>("tauIdent",1,
00110 &VelocityControl::handleExt,this);
00111 for (int i=0; i<5; ++i) tauExt[i] = 0;
00112 }
00113
00114
00115 server.setCallback(boost::bind(&VelocityControl::dynrec_cb, this, _1, _2));
00116
00117 initialize_controller();
00118 config.__fromServer__(ph);
00119 server.setConfigDefault(config);
00120 this->updateDynRecConfig();
00121 }
00122
00123 bool VelocityControl::handleEnableControl(labust_uvapp::EnableControl::Request& req,
00124 labust_uvapp::EnableControl::Response& resp)
00125 {
00126 if (!req.enable)
00127 {
00128 for (int i=u; i<=r;++i) axis_control[i] = disableAxis;
00129 this->updateDynRecConfig();
00130 }
00131
00132 return true;
00133 }
00134
00135 void VelocityControl::updateDynRecConfig()
00136 {
00137 ROS_INFO("Updating the dynamic reconfigure parameters.");
00138
00139 config.Surge_mode = axis_control[u];
00140 config.Sway_mode = axis_control[v];
00141 config.Heave_mode = axis_control[w];
00142 config.Roll_mode = axis_control[p];
00143 config.Pitch_mode = axis_control[q];
00144 config.Yaw_mode = axis_control[r];
00145
00146 config.High_level_controller="0 - None\n 1 - DP";
00147
00148 server.updateConfig(config);
00149 }
00150
00151 bool VelocityControl::handleServerConfig(labust_uvapp::ConfigureVelocityController::Request& req,
00152 labust_uvapp::ConfigureVelocityController::Response& resp)
00153 {
00154 for (int i=0; i<req.desired_mode.size();++i)
00155 {
00156 if (axis_control[i] != req.desired_mode[i])
00157 {
00158 axis_control[i] = req.desired_mode[i];
00159 controller[i].integratorState = 0;
00160 }
00161 }
00162 this->updateDynRecConfig();
00163 return true;
00164 }
00165
00166 void VelocityControl::handleReference(const auv_msgs::BodyVelocityReq::ConstPtr& ref)
00167 {
00168
00169 controller[u].desired = ref->twist.linear.x;
00170 controller[v].desired = ref->twist.linear.y;
00171 controller[w].desired = ref->twist.linear.z;
00172 controller[p].desired = ref->twist.angular.x;
00173 controller[q].desired = ref->twist.angular.y;
00174 controller[r].desired = ref->twist.angular.z;
00175
00176 lastRef = ros::Time::now();
00177
00178
00179 }
00180
00181 void VelocityControl::handleManual(const sensor_msgs::Joy::ConstPtr& joy)
00182 {
00183 tauManual[X] = config.Surge_joy_scale * joy->axes[1];
00184 tauManual[Y] = -config.Sway_joy_scale * joy->axes[0];
00185 tauManual[Z] = -config.Heave_joy_scale * joy->axes[3];
00186 tauManual[K] = 0;
00187 tauManual[M] = 0;
00188 tauManual[N] = -config.Yaw_joy_scale * joy->axes[2];
00189 lastMan = ros::Time::now();
00190 }
00191
00192 void VelocityControl::handleModelUpdate(const navcon_msgs::ModelParamsUpdate::ConstPtr& update)
00193 {
00194 ROS_INFO("Updating controller parameters for %d DoF",update->dof);
00195 controller[update->dof].modelParams[alpha] = update->alpha;
00196 if (update->use_linear)
00197 {
00198 controller[update->dof].modelParams[beta] = update->beta;
00199 controller[update->dof].modelParams[betaa] = 0;
00200 }
00201 else
00202 {
00203 controller[update->dof].modelParams[beta] = 0;
00204 controller[update->dof].modelParams[betaa] = update->betaa;
00205 }
00206
00207
00208 PIFFController_tune(&controller[update->dof]);
00209 }
00210
00211 void VelocityControl::dynrec_cb(labust_uvapp::VelConConfig& config, uint32_t level)
00212 {
00213 this->config = config;
00214
00215 config.__toServer__(ph);
00216 for(size_t i=u; i<=r; ++i)
00217 {
00218 int newMode(0);
00219 ph.getParam(dofName[i]+"_mode", newMode);
00220
00221 if ((axis_control[i] == identAxis) &&
00222 (newMode != identAxis) &&
00223 (ident[i] != 0)) ident[i].reset();
00224
00225 axis_control[i] = newMode;
00226 }
00227 }
00228
00229 void VelocityControl::handleWindup(const auv_msgs::BodyForceReq::ConstPtr& tau)
00230 {
00231 if (!controller[u].autoTracking) controller[u].tracking = tau->wrench.force.x;
00232 if (!controller[v].autoTracking) controller[v].tracking = tau->wrench.force.y;
00233 if (!controller[w].autoTracking) controller[w].tracking = tau->wrench.force.z;
00234 if (!controller[p].autoTracking) controller[p].tracking = tau->wrench.torque.x;
00235 if (!controller[q].autoTracking) controller[q].tracking = tau->wrench.torque.y;
00236 if (!controller[r].autoTracking) controller[r].tracking = tau->wrench.torque.z;
00237
00238 if (!controller[u].autoTracking) controller[u].windup = tau->disable_axis.x;
00239 if (!controller[v].autoTracking) controller[v].windup = tau->disable_axis.y;
00240 if (!controller[w].autoTracking) controller[w].windup = tau->disable_axis.z;
00241 if (!controller[p].autoTracking) controller[p].windup = tau->disable_axis.roll;
00242 if (!controller[q].autoTracking) controller[q].windup = tau->disable_axis.pitch;
00243 if (!controller[r].autoTracking) controller[r].windup = tau->disable_axis.yaw;
00244 };
00245
00246 void VelocityControl::handleExt(const auv_msgs::BodyForceReq::ConstPtr& tau)
00247 {
00248 labust::tools::pointToVector(tau->wrench.force, tauExt);
00249 labust::tools::pointToVector(tau->wrench.torque, tauExt, 3);
00250 };
00251
00252 void VelocityControl::handleEstimates(const auv_msgs::NavSts::ConstPtr& estimate)
00253 {
00254
00255 controller[u].state = estimate->body_velocity.x;
00256 controller[v].state = estimate->body_velocity.y;
00257 controller[w].state = estimate->body_velocity.z;
00258 controller[p].state = estimate->orientation_rate.roll;
00259 controller[q].state = estimate->orientation_rate.pitch;
00260 controller[r].state = estimate->orientation_rate.yaw;
00261
00262 lastEst = ros::Time::now();
00263
00264
00265
00266
00267 };
00268
00269 void VelocityControl::handleMeasurement(const auv_msgs::NavSts::ConstPtr& meas)
00270 {
00271
00272 double dT = (ros::Time::now() - lastMeas).toSec();
00273
00274 measurement[u] += meas->body_velocity.x*dT;
00275 measurement[v] += meas->body_velocity.y*dT;
00276 measurement[w] = meas->position.depth;
00277 measurement[p] = meas->orientation.roll;
00278 measurement[q] = meas->orientation.pitch;
00279 measurement[r] = meas->orientation.yaw;
00280
00281 lastMeas = ros::Time::now();
00282
00283
00284 };
00285
00286 void VelocityControl::safetyTest()
00287 {
00288 bool refTimeout = (ros::Time::now() - lastRef).toSec() > timeout;
00289 bool estTimeout = (ros::Time::now() - lastEst).toSec() > timeout;
00290 bool manTimeout = (ros::Time::now() - lastMan).toSec() > timeout;
00291 bool measTimeout = (ros::Time::now() - lastMeas).toSec() > timeout;
00292 bool changed = false;
00293
00294 for (int i=u; i<=r;++i)
00295 {
00296 bool cntChannel = (refTimeout || estTimeout) && (axis_control[i] == controlAxis);
00297 if (cntChannel) ROS_WARN("Timeout on the control channel: %d. Controlled axes will be disabled.",i);
00298 bool measChannel = measTimeout && (axis_control[i] == identAxis);
00299 if (measChannel) ROS_WARN("Timeout on the measurement channel: %d. Stopping identifications in progress.",i);
00300 bool manChannel = manTimeout && (axis_control[i] == manualAxis);
00301 if (manChannel) ROS_WARN("Timeout on the manual channel: %d. Manual axes will be disabled.",i);
00302
00303 suspend_axis[i] = (cntChannel || measChannel || manChannel);
00304 }
00305
00306
00307
00308 }
00309
00310 double VelocityControl::doIdentification(int i)
00311 {
00312 if (ident[i] == 0)
00313 {
00314 double C,X,ref;
00315 ph.getParam(dofName[i]+"_ident_amplitude",C);
00316 ph.getParam(dofName[i]+"_ident_hysteresis",X);
00317 ph.getParam(dofName[i]+"_ident_ref",ref);
00318 ident[i].reset(new SOIdentification());
00319 ident[i]->setRelay(C,X);
00320 ident[i]->Ref(ref);
00321
00322 measurement[i] = 0;
00323 ROS_INFO("Started indentification of %s DOF.",dofName[i].c_str());
00324 }
00325
00326 if (ident[i]->isFinished())
00327 {
00328
00329 SOIdentification::ParameterContainer params;
00330 ident[i]->parameters(¶ms);
00331 controller[i].modelParams[alpha] = params[SOIdentification::alpha];
00332 controller[i].modelParams[beta] = params[SOIdentification::kx];
00333 controller[i].modelParams[betaa] = params[SOIdentification::kxx];
00334
00335 PIFFController_tune(&controller[i]);
00336
00337 XmlRpc::XmlRpcValue vparam;
00338 vparam.setSize(SOIdentification::numParams);
00339 vparam[0] = params[SOIdentification::alpha];
00340 vparam[1] = params[SOIdentification::kx];
00341 vparam[2] = params[SOIdentification::kxx];
00342 vparam[3] = params[SOIdentification::delta];
00343 vparam[4] = params[SOIdentification::wn];
00344 nh.setParam(dofName[i]+"_identified_params", vparam);
00345
00346 axis_control[i] = disableAxis;
00347 ident[i].reset();
00348
00349 ROS_INFO("Stoped indentification of %s DOF.",dofName[i].c_str());
00350 ROS_INFO("Identified parameters: %f %f %f %f",
00351 params[SOIdentification::alpha],
00352 params[SOIdentification::kx],
00353 params[SOIdentification::kxx],
00354 params[SOIdentification::delta],
00355 params[SOIdentification::wn]);
00356
00357 this->updateDynRecConfig();
00358
00359 return 0;
00360 }
00361
00362 if (i>=3)
00363 {
00364 return ident[i]->step(labust::math::wrapRad(labust::math::wrapRad(ident[i]->Ref())-labust::math::wrapRad(measurement[i])),Ts);
00365 }
00366
00367 return ident[i]->step(ident[i]->Ref()-measurement[i],Ts);
00368 }
00369
00370 void VelocityControl::step()
00371 {
00372 auv_msgs::BodyForceReq tau, tauach;
00373 if (doSafetyTest) this->safetyTest();
00374
00375 for (int i=u; i<=r;++i)
00376 {
00377
00378 if (suspend_axis[i])
00379 {
00380 controller[i].output = 0;
00381 continue;
00382 }
00383
00384 switch (axis_control[i])
00385 {
00386 case manualAxis:
00387 controller[i].output = tauManual[i];
00388 break;
00389 case controlAxis:
00390 ROS_DEBUG("Controller %d : %f %f %f",
00391 i,
00392 controller[i].desired,
00393 controller[i].state,
00394 controller[i].output);
00395 PIFFController_step(&controller[i], Ts);
00396 break;
00397 case identAxis:
00398 controller[i].output = externalIdent?tauExt[i]:doIdentification(i);
00399 break;
00400 case directAxis:
00401 controller[i].output = controller[i].desired;
00402 if (controller[i].autoTracking)
00403 {
00404 if (fabs(controller[i].desired) > controller[i].outputLimit)
00405 controller[i].desired = controller[i].desired/fabs(controller[i].desired)*controller[i].outputLimit;
00406 tau.wrench.force.x = controller[i].desired;
00407 }
00408 break;
00409 case disableAxis:
00410 default:
00411 controller[i].output = 0;
00412 break;
00413 }
00414 }
00415
00416 tau.wrench.force.x = controller[u].output;
00417 tau.wrench.force.y = controller[v].output;
00418 tau.wrench.force.z = controller[w].output;
00419 tau.wrench.torque.x = controller[p].output;
00420 tau.wrench.torque.y = controller[q].output;
00421 tau.wrench.torque.z = controller[r].output;
00422
00423 tauach.wrench.force.x = tau.wrench.force.x;
00424 tauach.wrench.force.y = tau.wrench.force.y;
00425 tauach.wrench.force.z = tau.wrench.force.z;
00426 tauach.wrench.torque.x = tau.wrench.torque.x;
00427 tauach.wrench.torque.y = tau.wrench.torque.y;
00428 tauach.wrench.torque.z = tau.wrench.torque.z;
00429
00430 if (controller[u].autoTracking)
00431 {
00432 tauach.disable_axis.x = controller[u].windup;
00433 tauach.windup.x = controller[u].windup;
00434 }
00435 if (controller[v].autoTracking)
00436 {
00437 tauach.disable_axis.y = controller[v].windup;
00438 tauach.windup.y = controller[v].windup;
00439 }
00440 if (controller[w].autoTracking)
00441 {
00442 tauach.disable_axis.z = controller[w].windup;
00443 tauach.windup.z = controller[w].windup;
00444 }
00445 if (controller[p].autoTracking)
00446 {
00447 tauach.disable_axis.roll = controller[p].windup;
00448 tauach.windup.roll = controller[p].windup;
00449 }
00450 if (controller[q].autoTracking)
00451 {
00452 tauach.disable_axis.pitch = controller[q].windup;
00453 tauach.windup.pitch = controller[q].windup;
00454 }
00455 if (controller[r].autoTracking)
00456 {
00457 tauach.disable_axis.yaw = controller[r].windup;
00458 tauach.windup.yaw = controller[r].windup;
00459 }
00460
00461
00462
00463 tauach.header.stamp = tau.header.stamp = lastEst;
00464 tauAchW.publish(tauach);
00465 tauOut.publish(tau);
00466 }
00467
00468 void VelocityControl::start()
00469 {
00470 ros::Rate rate(1/Ts);
00471
00472 while (nh.ok())
00473 {
00474 ros::spinOnce();
00475 step();
00476 rate.sleep();
00477 }
00478 }
00479
00480 void VelocityControl::initialize_controller()
00481 {
00482 ROS_INFO("Initializing velocity controller...");
00483
00484 typedef Eigen::Matrix<double,6,1> Vector6d;
00485 using labust::simulation::vector;
00486 vector closedLoopFreq(vector::Ones());
00487 labust::tools::getMatrixParam(nh,"velocity_controller/closed_loop_freq", closedLoopFreq);
00488 vector outputLimit(vector::Zero());
00489 labust::tools::getMatrixParam(nh,"velocity_controller/output_limits", outputLimit);
00490 vector disAxis(vector::Ones());
00491 labust::tools::getMatrixParam(nh,"velocity_controller/disable_axis", disAxis);
00492 vector manAxis(vector::Ones());
00493 labust::tools::getMatrixParam(nh,"velocity_controller/manual_axis", manAxis);
00494 vector autoTracking(vector::Zero());
00495 labust::tools::getMatrixParam(nh,"velocity_controller/auto_tracking", autoTracking);
00496
00497 labust::simulation::DynamicsParams model;
00498 labust::tools::loadDynamicsParams(nh,model);
00499 vector alphas(model.Ma.diagonal());
00500 vector alpha_mass;
00501 alpha_mass<<model.m,model.m,model.m,model.Io.diagonal();
00502 alphas += alpha_mass;
00503
00504 nh.param("velocity_controller/Ts",Ts,Ts);
00505
00506 for (int32_t i = u; i <=r; ++i)
00507 {
00508 PIDController_init(&controller[i]);
00509 controller[i].closedLoopFreq = closedLoopFreq(i);
00510 controller[i].outputLimit = outputLimit(i);
00511 controller[i].modelParams[alpha] = alphas(i);
00512 controller[i].modelParams[beta] = model.Dlin(i,i);
00513 controller[i].modelParams[betaa] = model.Dquad(i,i);
00514 PIFFController_tune(&controller[i]);
00515 controller[i].autoTracking = autoTracking(i);
00516
00517 if (!manAxis(i)) axis_control[i] = controlAxis;
00518 if (manAxis(i)) axis_control[i] = manualAxis;
00519 if (disAxis(i)) axis_control[i] = disableAxis;
00520 suspend_axis[i]=false;
00521
00522 ph.setParam(dofName[i]+"_Kp",controller[i].gains[Kp]);
00523 ph.setParam(dofName[i]+"_Ki",controller[i].gains[Ki]);
00524
00525 ROS_INFO("Controller %d:",i);
00526 ROS_INFO("ModelParams: %f %f %f",controller[i].modelParams[alpha], controller[i].modelParams[beta],
00527 controller[i].modelParams[betaa]);
00528 ROS_INFO("Gains: %f %f %f",controller[i].gains[Kp], controller[i].gains[Ki],
00529 controller[i].gains[Kt]);
00530 }
00531
00532 ROS_INFO("Velocity controller initialized.");
00533 }