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


labust_uvapp
Author(s): Dula Nad
autogenerated on Fri Feb 7 2014 11:36:37