fsi/motion-amroc/HingedWing/src/WingMechanism.h

Go to the documentation of this file.
00001 // -*- C++ -*-
00002 
00003 #ifndef WING_MECHANISM_H
00004 #define WING_MECHANISM_H
00005 
00006 //#define WING_DEBUG_ON
00007 #define WING_LOG_ON
00008 
00009 #include "Point.h"
00010 #include "Assembly.h"
00011 #include "DH_Chain.h"
00012 #include "Scene.h"
00013 
00014 class WingControl : public controlable {
00015 public:
00016   WingControl() {}
00017 
00018   ~WingControl() {}
00019 
00020   virtual void register_at(ControlDevice& Ctrl) {}
00021 
00022   virtual void register_at(ControlDevice& Ctrl, const std::string& prefix, int num) {
00023     char VariableName[32];
00024 
00025     idNum = num;
00026     std::sprintf(VariableName,"Wing(%02d)",num+1);
00027 
00028 #ifdef WING_DEBUG_ON
00029     std::cout << "Registering " << VariableName << std::endl;
00030 #endif
00031 
00032     TLocCtrl = Ctrl.getSubDevice(std::string(VariableName));
00033     RegisterAt(TLocCtrl,"Wing_name",Wing_name);
00034     RegisterAt(TLocCtrl,"Component1_name",comp0_name);
00035     RegisterAt(TLocCtrl,"Component2_name",comp1_name);
00036     RegisterAt(TLocCtrl,"Component1_file",comp0_file);
00037     RegisterAt(TLocCtrl,"Component2_file",comp1_file);
00038     RegisterAt(TLocCtrl,"Component1_filetype",comp0_filetype);
00039     RegisterAt(TLocCtrl,"Component2_filetype",comp1_filetype);
00040 
00041     comp0.register_at(TLocCtrl,"Component1");
00042     hinge0.register_at(TLocCtrl,"Hinge");
00043     scene_comp0.register_at(TLocCtrl,"scene_Component1");
00044 
00045     RegisterAt(TLocCtrl,"I_comp1",I_comp0);
00046     RegisterAt(TLocCtrl,"I_comp2",I_comp1);
00047 
00048     RegisterAt(TLocCtrl,"Frequency",freq);
00049     RegisterAt(TLocCtrl,"Phase",phi);
00050     RegisterAt(TLocCtrl,"RotationShape",sigma_R);
00051     RegisterAt(TLocCtrl,"TranslationShape",sigma_T);
00052     RegisterAt(TLocCtrl,"TranslationAmplitude",A0);
00053     RegisterAt(TLocCtrl,"SweepAngle",beta);
00054     RegisterAt(TLocCtrl,"Chord_comp1",C_comp0);
00055     RegisterAt(TLocCtrl,"Chord_comp2",C_comp1);
00056     RegisterAt(TLocCtrl,"Aspect_comp1",A_comp0);
00057     RegisterAt(TLocCtrl,"Aspect_comp2",A_comp1);
00058     RegisterAt(TLocCtrl,"HingeGap",hGap);
00059     RegisterAt(TLocCtrl,"TorsionSpring",Kts);
00060     RegisterAt(TLocCtrl,"brake_set",brake_set);
00061     RegisterAt(TLocCtrl,"brake_release",brake_release);
00062 
00063   }
00064 
00065   void print() {
00066     ( mklog() << "Wing CONTROL DEVICE\n"
00067               << "\tComponent1_name = " << comp0_name << " Component2_name = " << comp1_name << std::endl).flush();
00068   }
00069 
00070 public:
00071   double LastTime;
00072   int Outputs, OutputEvery, CheckPointEvery;
00073   int idNum;
00074 
00075   std::string Wing_name, comp0_name, comp0_file, comp1_name, comp1_file;
00076   int comp0_filetype, comp1_filetype;
00077 
00078   JointControl comp0, hinge0;
00079   LinkControl scene_comp0;
00080 
00081   ControlDevice TLocCtrl;
00082 
00083   DataType I_comp0, I_comp1;
00084   DataType C_comp0, C_comp1, A_comp0, A_comp1, hGap, Kts;
00085   DataType brake_set, brake_release;
00086   DataType freq, phi, sigma_R, sigma_T, A0, beta;
00087 
00088 };
00089 
00091 class SolidSolverSpecific;
00092 
00093 class Wing {
00094   typedef Assembly<DataType,3> AssemblyType;
00095   typedef Scene<DataType,3> SceneType;
00096   typedef Loft<DataType> LoftType;
00097   typedef Joint<DataType,3> JointType;
00098   typedef DH_Link<DataType,3> LinkType;
00099   typedef DH_Chain<DataType,3> ChainType;
00100 
00101 public:
00102   Wing() {
00103     p_comp0 = 0.;
00104     p_comp1 = 0.;
00105     v_comp0 = 0.;
00106     v_comp1 = 0;
00107 
00108   }
00109 
00110   ~Wing() {}
00111 
00112   bool specificSetup(SceneType& ThisScene, WingControl * WingCtrl ) {
00113     std::string nameTmp;
00114     ( mklog() << "\n\n*********\t Wing specificSetup \tBegin\n*********" << std::endl ).flush();
00115 
00117     nameTmp = "Wing_Assembly0";
00118     Wing0pt = new AssemblyType;
00119     Wing0pt->setName(WingCtrl->Wing_name);
00120 
00121     ThisScene.AddAssembly(*Wing0pt);
00122     ThisScene.GetNthSceneAssembly(WingCtrl->idNum).print(0);
00123 
00124     Wing0pt = ThisScene.GetNthAssembly(WingCtrl->idNum);
00125 
00126     Comp0 = new LoftType;
00127     Wing0pt->AddPart(Comp0);
00128 
00129     Wing0pt->GetNthPart(0)->read(WingCtrl->comp0_filetype,WingCtrl->comp0_file);
00130     Wing0pt->GetNthPart(0)->setName(WingCtrl->comp0_name);
00131     Comp0 = (LoftType *) Wing0pt->GetNthPart(0);
00132     Comp0->measure();
00133 
00134     Comp1 = new LoftType;
00135     Wing0pt->AddPart(Comp1);
00136     Wing0pt->GetNthPart(1)->read(WingCtrl->comp1_filetype,WingCtrl->comp1_file);
00137     Wing0pt->GetNthPart(1)->setName(WingCtrl->comp1_name);
00138     Comp1 =  (LoftType *) Wing0pt->GetNthPart(1);
00139     Comp1->measure();
00140 
00141     ( mklog() << "\nparts added\n").flush();
00142 
00143     // Kinematic Chain
00144     PType o(0.), x(0.), y(0.), z(0.);
00145     o(0) = 0.; o(1) = 0.; o(2) = 0.;
00146     x(0) = 1.; x(1) = 0.; x(2) = 0.;
00147     y(0) = 0.; y(1) = 1.; y(2) = 0.;
00148     z(0) = 0.; z(1) = 0.; z(2) = 1.;
00149 
00150     o(0) = 0.; o(1) = 0.; o(2) = 0.;
00151     x(0) = 1.; x(1) = 0.; x(2) = 0.;
00152     z(0) = 0.; z(1) = 0.; z(2) = 1.;
00153 
00154     JointType * gantry;
00155     gantry = new JointType(o,z,x,JointType::revolute,Comp0->IdTag());
00156 
00157     L0 = new LinkType(*ThisScene.GetCoordFrame(),*gantry); //link connecting scene to gantry translation
00158     L0->setId(-1);
00159 
00160     o(0) = 0.; o(1) = 0.; o(2) = 0.;
00161     z(0) = 0.; z(1) = 0.; z(2) = 1.;
00162     x(0) = 1.; x(1) = 0.; x(2) = 0.;
00163     JointType * turnTable;
00164     turnTable = new JointType(o,z,x,JointType::revolute,Comp0->IdTag()); // turntable at center of comp0
00165     LinkType * L1;
00166     L1 = new LinkType(*gantry,*turnTable); //link connecting gantry translation to rotation of comp0
00167 
00168     o(0) = 0.; o(1) = 0.; o(2) = 0.;
00169     z(0) = 0.; z(1) = 0.; z(2) = 1.;
00170     x(0) = 1.; x(1) = 0.; x(2) = 0.;
00171     JointType * zero;
00172     zero = new JointType(o,z,x,JointType::revolute,Comp0->IdTag()); // Joint at center of comp0
00173     LinkType * L2;
00174     L2 = new LinkType(*turnTable,*zero); // link connecting turntable to center of comp0
00176     o(0) = -(WingCtrl->C_comp1/2. + WingCtrl->hGap); o(1) = 0; o(2) = 0.;
00177     z(0) = 0.; z(1) = 0.; z(2) = 1.;
00178     x(0) = 0; x(1) = 1; x(2) = 0.;
00179     JointType * one;
00180     one = new JointType(o,z,x,JointType::revolute,Comp1->IdTag()); // Joint at hinge
00181     LinkType * L3;
00182     L3 = new LinkType(*zero,*one, (WingCtrl->C_comp1/2. + WingCtrl->hGap), 0., 0., 0.); // link connecting center of comp0 to hinge
00183 
00184     o(0) = 0.; o(1) = 0; o(2) = 0.;
00185     z(0) = 0.; z(1) = 0.; z(2) = 1.;
00186     x(0) = 1.; x(1) = 0.; x(2) = 0.;
00187     JointType * two;
00188     two = new JointType(o,z,x,JointType::revolute,Comp1->IdTag()); // Joint at center of comp1
00189     LinkType * L4;
00190     L4 = new LinkType(*one,*two); //link connecting hinge to center of comp1
00192 
00193     ThisScene.addLink(L0);
00194     L0_index = ThisScene.GetNumLinks();
00195     L0 = ThisScene.GetNthLink(L0_index-1);
00196 
00197     C1 = new ChainType;
00198     C1->AddJoint(gantry);
00199     C1->AddJoint(turnTable);
00200     C1->AddJoint(zero);
00201     C1->AddJoint(one);
00202     C1->AddJoint(two);
00203     C1->addLink(L1);
00204     C1->addLink(L2);
00205     C1->addLink(L3);
00206     C1->addLink(L4);
00207     C1->setParentChain(-L0_index);
00208     C1->GetNthLink(0)->setQ_pre(0.0);
00209     ThisScene.AddChain(C1);
00210     C1_index = ThisScene.GetNumChains()-1;
00211 
00212     if (WingCtrl->sigma_T == 0.628) {
00213       gtmax = 0.556673523183792;
00214     }
00215     else if (WingCtrl->sigma_T == 1.885) {
00216       gtmax = 0.954934721087105;
00217     }
00218     else if (WingCtrl->sigma_T == 3.770) {
00219       gtmax = 0.998937769223312;
00220     }
00221 
00222 
00223     if (WingCtrl->sigma_R == 0.628) {
00224       grmax = 0.556673523183792;
00225     }
00226     else if (WingCtrl->sigma_R == 1.885) {
00227       grmax = 0.954934721087105;
00228     }
00229     else if (WingCtrl->sigma_R == 3.770) {
00230       grmax = 0.998937769223312;
00231     }
00232 
00233     drive(WingCtrl,0.);
00234 
00235     return true;
00236   }
00237 
00238 
00239   void drive(WingControl * WingCtrl, DataType time) {
00240     DataType t, gt, gr, cstart;
00241 
00242     t = time - WingCtrl->brake_release;
00243     if (t<0) t = 0.;
00244 
00245     gt = std::tanh(WingCtrl->sigma_T*std::cos(2.*pi*WingCtrl->freq*t));
00246 
00247     cstart = ( std::tanh(8.*WingCtrl->freq*t-2.) + std::tanh(2.) )/( 1. + std::tanh(2.) );
00248 
00249     gr = std::tanh(WingCtrl->sigma_R*cos(2.*pi*WingCtrl->freq*t + WingCtrl->phi));
00250 
00251     tval = WingCtrl->A0*cstart*gt/(2.*gtmax);
00252     rval = -WingCtrl->beta*gr/grmax;
00253     ( mklog() << "\ntime = " << time << " tval = " << tval << " rval = " << rval << std::endl).flush();
00254 
00255     PType o(0.), x(0.), y(0.), z(0.), tmp(0.);
00256 
00257     o(0) = 0.; o(1) = 0.; o(2) = 0.;
00258     x(0) = 1.; x(1) = 0.; x(2) = 0.;
00259     z(0) = 0.; z(1) = 0.; z(2) = 1.;
00260     ( mklog() << "===================  Joint at base of" << WingCtrl->comp0_name << " ===================\n").flush();
00261 
00262     o(0) = tval;
00263     ( mklog() << " Translating Gantry " << o << std::endl).flush();
00264     L0->getChild()->setOrigin(o);
00265     L0->update();
00266 
00267     ( mklog() << " Rotating turntable " << rval << std::endl).flush();
00268     C1->GetNthLink(0)->actuate(rval); // rotation
00269   }
00270 
00271   void rotate(DataType val) {
00272     C1->GetNthLink(3)->actuate(val);
00273   }
00274 
00275   void response(Wing * ThisWing, WingControl * WingCtrl, double& t, double& dt, int nsteps, int logevery) {
00276 
00277     PType arm, ftmp, ratmp;
00278     DataType rtmp, springForce, mass, inertia, narm;
00279     MType mtmp;
00280 
00281     if ( (t-WingCtrl->brake_release) < dt) {
00282       p_comp0 = 0.;
00283       p_comp1 = 0.;
00284     }
00285 
00286     if ( (WingCtrl->brake_set > 0 && t >= WingCtrl->brake_release) || (WingCtrl->brake_set <= 0 ) ) {
00287 #ifdef WING_DEBUG_ON
00288       if (nsteps % logevery == 0 )
00289         ( mklog() << "\t" << WingCtrl->Wing_name << " FSI response at time = " << t << " , dt = " << dt << std::endl ).flush();
00290 #endif
00291       drive(WingCtrl, t);
00292 
00293       ratmp(0) = (WingCtrl->C_comp0/2. + WingCtrl->hGap)*std::cos(rval*d2r);
00294       ratmp(1) = (WingCtrl->C_comp0/2. + WingCtrl->hGap)*std::sin(rval*d2r);
00295       ratmp(2) = 0.;
00296       arm = ThisWing->Comp0->Centroid() + ratmp - ThisWing->Comp1->Centroid();
00297       ftmp =  ThisWing->Comp1->GetNetP();
00298       T_comp1 = cross3D(arm, ftmp);
00299       narm = norm_2(arm);
00300       ( mklog() << "T_comp1 = " << T_comp1 << " ratmp = " << ratmp
00301                 << "\n\t arm = " << arm << " norm(arm) = " << narm
00302                 << "\n\t ThisWing->Comp0->Centroid() =" << ThisWing->Comp0->Centroid()
00303                 << "\n\t ThisWing->Comp1->Centroid() =" << ThisWing->Comp1->Centroid()
00304                 << "\n\t ThisWing->Comp1->GetNetP() = " << ftmp << std::endl ).flush();
00305 
00306       ratmp(0) = 0.;  ratmp(1) = 0.;  ratmp(2) = 1.;
00307       rtmp = projectScalar(T_comp1,ratmp);
00308       springForce = -WingCtrl->Kts*p_comp1;
00309       //springForce = -(WingCtrl->Kts/500.)*p_comp1*p_comp1 - (WingCtrl->Kts/1.5)*p_comp1;
00310       mass = WingCtrl->I_comp1*ThisWing->Comp1->Volume(); // mass
00311       ( mklog() << "T_comp1 = " << T_comp1 << " rtmp = " << rtmp
00312                 << "volume = " << ThisWing->Comp1->Volume() << " mass = " << mass
00313                 << std::endl ).flush();
00314       inertia = 0.5*(1.+1./(WingCtrl->A_comp1*WingCtrl->A_comp1))*(WingCtrl->C_comp1*WingCtrl->C_comp1)*mass; // inertia about comp1 central axis
00315       inertia = inertia + mass*narm*narm; // inertial about hinge axis
00316       a_comp1 = ( (rtmp + springForce)/inertia );
00317       v_comp1 += a_comp1*dt;
00318       p_comp1 += (DataType(0.5)*a_comp1*dt*dt + v_comp1*dt)/d2r;
00319 
00320       ( mklog() << " intertia = " << inertia << " rtmp = " << rtmp << " springForce = " << springForce << " p_comp = " << p_comp1/d2r << std::endl ).flush();
00321       ThisWing->rotate(p_comp1/d2r);
00322 
00323     }
00324   }
00325 
00326   virtual void logKinetics(Wing * ThisWing, WingControl * WingCtrl, std::ofstream& ofs, DataType time_, int steps ) {
00327 
00328     PType cen, ftmp, axis;
00329 
00330     ofs << "\t\t\"" << WingCtrl->Wing_name << "\": {\n";
00331     ofs << "\t\t\t\"rotor\": {\n\t\t\t\t\"acc\": " << a_comp0/d2r;
00332     ofs << ", \"vel\": " << v_comp0/d2r;
00333     ofs << ", \"pos\": " << p_comp0/d2r;
00334 
00335   }
00336 
00337   void movement(Wing * ThisWing, WingControl * WingCtrl, double& t, double& dt) {
00338 
00339 #ifdef WING_DEBUG_ON
00340     ( mklog() << "\n\n*********\tSolidSolverSpecific movement aDVANCE bEGIN\n*********" << std::endl ).flush();
00341     ( mklog() << " time = " << t << " time*DataType(360.0/6.) = " << t*DataType(360.0/6.)
00342               << " time*DataType(360.0/6.)*d2r = " << t*DataType(360.0/6.)*d2r << std::endl
00343               << " dt = " << dt << std::endl ).flush();
00344 #endif
00345 
00346 #ifdef WING_DEBUG_ON
00347     ( mklog() << "\n\n*********\tSolidSolverSpecific movement aDVANCE fINISHED\n*********" << std::endl ).flush();
00348 #endif
00349 
00350   }
00351 
00352 
00353   virtual void Restart(std::ifstream& ifs, int& pos, double& t, double& dt) {
00354 #ifdef CHECKRESTART_DEBUG_ON
00355     ( mklog() << "\nRestarting WingMECHANISM SOLVER t= " << t << " dt = " << dt << " pos = " << pos << std::endl ).flush();
00356 #endif
00357 
00358     L0->Restart(ifs,pos,t,dt);
00359     pos = ifs.tellg();
00360     C1->Restart(ifs,pos,t,dt);
00361     pos = ifs.tellg();
00362 
00363     ifs.read((char*)&L0_index,sizeof(int));
00364     ifs.read((char*)&C1_index,sizeof(int));
00365 
00366     ifs.read((char*)&T_comp0(0),sizeof(DataType));
00367     ifs.read((char*)&T_comp0(1),sizeof(DataType));
00368     ifs.read((char*)&T_comp0(2),sizeof(DataType));
00369 
00370     ifs.read((char*)&T_comp1(0),sizeof(DataType));
00371     ifs.read((char*)&T_comp1(1),sizeof(DataType));
00372     ifs.read((char*)&T_comp1(2),sizeof(DataType));
00373 
00374     ifs.read((char*)&a_comp0,sizeof(DataType));
00375     ifs.read((char*)&v_comp0,sizeof(DataType));
00376     ifs.read((char*)&p_comp0,sizeof(DataType));
00377 
00378     ifs.read((char*)&a_comp1,sizeof(DataType));
00379     ifs.read((char*)&v_comp1,sizeof(DataType));
00380     ifs.read((char*)&p_comp1,sizeof(DataType));
00381 
00382     pos = ifs.tellg();
00383 #ifdef CHECKRESTART_DEBUG_ON
00384     ( mklog() << "\nDone Restarting WingMECHANISM t = " << t << " dt = " << dt << " pos = " << pos << std::endl ).flush();
00385 #endif
00386   }
00387 
00388   virtual void Checkpointing(std::ofstream& ofs) {
00389 #ifdef CHECKRESTART_DEBUG_ON
00390     ( mklog() << "\nCHECKPOINTING WingMECHANISM SOLVER pos = " << ofs.tellp() << std::endl ).flush();
00391 #endif
00392 
00393     L0->Checkpointing(ofs);
00394     C1->Checkpointing(ofs);
00395 
00396     ofs.write((char*)&L0_index,sizeof(int));
00397     ofs.write((char*)&C1_index,sizeof(int));
00398 
00399     ofs.write((char*)&T_comp0(0),sizeof(DataType));
00400     ofs.write((char*)&T_comp0(1),sizeof(DataType));
00401     ofs.write((char*)&T_comp0(2),sizeof(DataType));
00402 
00403     ofs.write((char*)&T_comp1(0),sizeof(DataType));
00404     ofs.write((char*)&T_comp1(1),sizeof(DataType));
00405     ofs.write((char*)&T_comp1(2),sizeof(DataType));
00406 
00407     ofs.write((char*)&a_comp0,sizeof(DataType));
00408     ofs.write((char*)&v_comp0,sizeof(DataType));
00409     ofs.write((char*)&p_comp0,sizeof(DataType));
00410 
00411     ofs.write((char*)&a_comp1,sizeof(DataType));
00412     ofs.write((char*)&v_comp1,sizeof(DataType));
00413     ofs.write((char*)&p_comp1,sizeof(DataType));
00414 
00415 #ifdef CHECKRESTART_DEBUG_ON
00416     ( mklog() << "\n+++ CHECKPOINTING WingMECHANISM COMPLETE pos = " << ofs.tellp() <<std::endl ).flush();
00417 #endif
00418   }
00419 
00420 
00421 protected:
00422 
00423   AssemblyType * Wing0pt;
00424   LoftType * Comp0;
00425   LoftType * Comp1;
00426 
00427   ChainType * C1, * C2;
00428   LinkType * L0;
00429 
00430   int L0_index, C1_index;
00431 
00432   PType T_comp0, T_comp1;
00433 
00434   DataType tval, rval, gtmax, grmax;
00435 
00436   DataType a_comp0, a_comp1;
00437   DataType v_comp0, v_comp1;
00438   DataType p_comp0, p_comp1;
00439 
00440 };
00441 
00442 #endif
Generated on Thu Jun 30 02:21:03 2016 for Virtual Test Facility Coupled Applications by 1.6.3