Go to the documentation of this file.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 #ifndef THRUSTALLOCATION_HPP_
00038 #define THRUSTALLOCATION_HPP_
00039 #include <labust/simulation/matrixfwd.hpp>
00040 #include <labust/math/NumberManipulation.hpp>
00041
00042 #include <vector>
00043 #include <ros/ros.h>
00044
00045 namespace labust
00046 {
00047 namespace vehicles
00048 {
00059 class ThrustAllocator
00060 {
00061 enum {NoAlloc=0,SimpleAlloc=1, ScaleAlloc=2};
00062 public:
00066 ThrustAllocator():
00067 dofs(6,1),
00068 groups(1,dofs),
00069 B(labust::simulation::matrix::Identity()),
00070 Binv(B),
00071 tmax(1),
00072 tmin(-tmax),
00073 coercion(Eigen::VectorXi::Zero(6)),
00074 type(NoAlloc),
00075 num_thrusters(6){};
00076
00080 void allocate(const labust::simulation::vector& tauIn,
00081 labust::simulation::vector& tauOut)
00082 {
00083 Eigen::VectorXd vi(dofs.size());
00084 for (int i=0; i<dofs.size(); ++i) vi(i)=tauIn(dofs[i]);
00085
00086 tdes = Binv*vi;
00087
00088
00089
00090 switch (type)
00091 {
00092 case NoAlloc:
00093
00094 break;
00095 case SimpleAlloc:
00096 simple_alloc();
00097 break;
00098 case ScaleAlloc:
00099 scale_alloc();
00100 break;
00101 default:
00102 throw std::runtime_error("ThrustAllocator: Undefined allocation type.");
00103 }
00104
00105
00106 vi = B*tdes;
00107
00108 for (int i=0; i<dofs.size(); ++i) tauOut(dofs[i])=vi(i);
00109
00110
00111 switch (type)
00112 {
00113 case NoAlloc:
00114
00115 break;
00116 case SimpleAlloc:
00117
00118 for (int i=0; i<dofs.size(); ++i)
00119 {
00120 coercion(dofs[i]) = 0;
00121 if (tauIn(dofs[i])>tauOut(dofs[i])) coercion(dofs[i])=1;
00122 else if (tauIn(dofs[i])<tauOut(dofs[i])) coercion(dofs[i])=-1;
00123 }
00124 break;
00125 case ScaleAlloc:
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138 for (int i=0; i<dofs.size(); ++i)
00139 {
00140 coercion(dofs[i]) = 0;
00141
00142 if (fabs(tauIn(dofs[i]) - tauOut(dofs[i])) > 0.01)
00143 {
00144 if (tauIn(dofs[i])>tauOut(dofs[i])) coercion(dofs[i])=1;
00145 else if (tauIn(dofs[i])<tauOut(dofs[i])) coercion(dofs[i])=-1;
00146 }
00147 }
00148 break;
00149 default:
00150 break;
00151 }
00152 }
00153
00154
00155 template <class AllocMatrix, class DOFMatrix, class ThrusterGroups>
00156 void init(int alloc_type, const AllocMatrix& alloc, const DOFMatrix& use_dofs,
00157 const ThrusterGroups& thrust_groups)
00158 {
00159 type = alloc_type;
00160 if (type >= 0)
00161 {
00162 dofs.clear();
00163 for (size_t i=0; i<use_dofs.size(); ++i) if (use_dofs(i)) dofs.push_back(i);
00164 groups.clear();
00165 num_thrusters=0;
00166
00167 if ((thrust_groups.cols() == 0) || (thrust_groups.rows() == 0))
00168 {
00169
00170 groups.push_back(std::vector<int>());
00171 for(int i=0; i<alloc.cols(); ++i) groups[0].push_back(i);
00172 num_thrusters=alloc.cols();
00173 }
00174 else
00175 {
00176
00177 for (size_t i=0; i<thrust_groups.rows();++i)
00178 {
00179
00180 groups.push_back(std::vector<int>());
00181
00182 for (size_t j=0; j<thrust_groups.cols(); ++j)
00183 if (thrust_groups(i,j))
00184 {
00185 ++num_thrusters;
00186 groups[i].push_back(j);
00187 }
00188 }
00189 }
00190 group_scales.resize(groups.size());
00191
00192 this->B=alloc;
00193 Binv = B.transpose()*(B*B.transpose()).inverse();
00194 }
00195 else
00196 {
00197 type = NoAlloc;
00198 };
00199
00200 assert((B.cols() == num_thrusters) &&
00201 "Allocation matrix must have number of columns equal to number of thrusters.");
00202
00203 assert((B.rows() == dofs.size()) &&
00204 "Allocation matrix must have the same number of rows as controllable DOFs.");
00205 }
00206
00210 inline void setThrusterMinMax(double min, double max)
00211 {
00212 this->tmax = max;
00213 this->tmin = min;
00214 }
00215
00216 inline const Eigen::VectorXd& getThrusterForces(){return tdes;};
00217
00218 inline const Eigen::VectorXi& getCoercion(){return coercion;};
00219
00220 protected:
00224 inline void simple_alloc()
00225 {
00226 for (size_t i=0; i<tdes.rows();++i)
00227 {
00228 tdes(i)=labust::math::coerce(tdes(i),tmin,tmax);
00229 }
00230 };
00231
00235 void scale_alloc()
00236 {
00237 for (size_t i=0; i<groups.size(); ++i)
00238 {
00239
00240 Eigen::VectorXd ttdes(groups[i].size());
00241
00242 for (size_t j=0; j<groups[i].size(); ++j) ttdes(j)=tdes(groups[i][j]);
00243
00244
00245
00246 double scale_max = 1;
00247 for (size_t j=0; j<ttdes.rows();++j)
00248 {
00249 double scale = fabs((ttdes(j)>0)?ttdes(j)/tmax:ttdes(j)/tmin);
00250 if (scale>scale_max) scale_max=scale;
00251 }
00252
00253 group_scales[i] = scale_max;
00254 ttdes = ttdes/scale_max;
00255
00256
00257
00258 for (size_t j=0; j<groups[i].size(); ++j) tdes(groups[i][j])=ttdes(j);
00259 }
00260 }
00261
00265 std::vector<int> dofs;
00269 std::vector< std::vector<int> > groups;
00273 std::vector<double> group_scales;
00274
00278 Eigen::MatrixXd B, Binv;
00282 double tmax, tmin;
00286 Eigen::VectorXd tdes;
00290 Eigen::VectorXi coercion;
00294 int type, num_thrusters;
00295 };
00296 }
00297 }
00298
00299
00300 #endif