VelocityControl.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, LABUST, UNIZG-FER
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 LABUST 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  *  Author: Dula Nad
00035  *  Created: 01.02.2013.
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         //Initialize publishers
00076         tauOut = nh.advertise<auv_msgs::BodyForceReq>("tauOut", 1);
00077         tauAchW = nh.advertise<auv_msgs::BodyForceReq>("tauAchVelCon", 1);
00078 
00079         //Initialze subscribers
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         //Configure service
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         //Added to avoid safety tests in simulation time.
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         //Configure the dynamic reconfigure server
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         //Copy into controller
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         //newReference = true;
00178         //if (newEstimate) step();
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         //Tune controller
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                 //Stop the identification if it was aborted remotely.
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         //Copy into controller
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         //ROS_INFO("Semi-travel time:%f",(lastEst - estimate->header.stamp).toSec());
00264         //lastEst = estimate->header.stamp;
00265         //newEstimate = true;
00266         //if (newReference) step();
00267 };
00268 
00269 void VelocityControl::handleMeasurement(const auv_msgs::NavSts::ConstPtr& meas)
00270 {
00271         //Copy into controller
00272         double dT = (ros::Time::now() - lastMeas).toSec();
00273         //ROS_INFO("Estimated rate for integration: %f",dT);
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         //newEstimate = true;
00283         //if (newReference) step();
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         //Update only on change.
00307         //if (changed) this->updateDynRecConfig();
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                 //Reset measurement
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                 //Get parameters
00329                 SOIdentification::ParameterContainer params;
00330                 ident[i]->parameters(&params);
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                 //Tune controller
00335                 PIFFController_tune(&controller[i]);
00336                 //Write parameters to server
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                 //Stop identification
00346                 axis_control[i] = disableAxis;
00347                 ident[i].reset();
00348                 //Report
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                 //Update dynamic parameters
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                 //If some of the axis are timed-out just ignore
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         //Copy to tau
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         //Restart values
00462         //newReference = newEstimate = false;
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 }


labust_uvapp
Author(s): Dula Nad
autogenerated on Sun Mar 1 2015 12:12:22