void SimpleShell::add_frequency_squared(RawArray<T> frequency_squared) const { const T max_stiff = stiffness().maxabs(); Hashtable<int,T> particle_frequency_squared; for (const auto& I : info) { const T elastic_squared = max_stiff/(sqr(I.inv_Dm.inverse().simplex_minimum_altitude())*density); for (const int n : I.nodes) { T& data = particle_frequency_squared.get_or_insert(n); data = max(data,elastic_squared); } } for (auto& it : particle_frequency_squared) frequency_squared[it.x] += it.y; }
MatrixXS MaterialQuad2D::getStiffness(Element2D* ele, ElementMesh2D * mesh) { int ndof = 2* ele->nV(); MatrixXS K = MatrixXS::Zero(ndof, ndof); for(unsigned int ii = 0; ii<q->x.size();ii++) { K += q->w[ii] * stiffness(ii, this, ele, mesh); } //std::cout << K <<std::endl << std::endl; cfgScalar vol = ele->getVol(mesh->X); K *= vol; return K; }
void AnisotropicLinearElasticMaterial :: giveInputRecord(DynamicInputRecord &input) { Material :: giveInputRecord(input); FloatArray stiffness(21); int k = 1; for ( int i = 1; i <= 6; i++ ) { for ( int j = i; j <= 6; j++ ) { stiffness.at(k++) = stiffmat.at(i, j); } } input.setField(stiffness, _IFT_AnisotropicLinearElasticMaterial_stiff); input.setField(alpha, _IFT_AnisotropicLinearElasticMaterial_talpha); }
T SimpleShell::elastic_energy() const { if (tweak) GEODE_WARNING("Linear shell energy enabled: use for debugging purposes only"); T energy = 0; const auto stiff = stiffness(); for (const auto& I : info) energy -= !tweak ? I.scale*( stiff.x00*sqr(I.Fh.x00-1) +stiff.x10*sqr(I.Fh.x10) +stiff.x11*sqr(I.Fh.x11-1)) : I.scale*( stiff.x00*I.Fh.x00 +stiff.x10*I.Fh.x10 +stiff.x11*I.Fh.x11); return !tweak ? energy/2 : energy; }
void initialize() { init_ = true; // Enable stiffness AL::ALValue stiffness_name("Body"); AL::ALValue stiffness(1.0f); AL::ALValue stiffness_time(1.0f); motion_proxy_ptr->stiffnessInterpolation(stiffness_name,stiffness,stiffness_time); // Init moving motion_proxy_ptr->moveInit(); // Robot not detected robotDetected = false; }
int main(int argc, char** argv) { ros::init(argc, argv,"nao_behavior"); ros::NodeHandle nh; // Robot parameters ros::NodeHandle pnh("~"); std::string NAO_IP; int NAO_PORT; pnh.param("NAO_IP",NAO_IP,std::string("127.0.0.1")); pnh.param("NAO_PORT",NAO_PORT,int(9559)); // Enable stiffness AL::ALMotionProxy* motion_proxy_ptr; motion_proxy_ptr = new AL::ALMotionProxy(NAO_IP,NAO_PORT); AL::ALValue stiffness_name("Body"); AL::ALValue stiffness(1.0f); AL::ALValue stiffness_time(1.0f); motion_proxy_ptr->stiffnessInterpolation(stiffness_name,stiffness,stiffness_time); // Behavior manager proxy AL::ALBehaviorManagerProxy* behavior_proxy_ptr; behavior_proxy_ptr = new AL::ALBehaviorManagerProxy(NAO_IP,NAO_PORT); // Install behavior behavior_proxy_ptr->installBehavior("/home/olivier/ros_workspace/nao_custom/behavior_hello/behavior.xar"); // Behavior list std::vector<std::string> list = behavior_proxy_ptr->getInstalledBehaviors(); printf("Behaviors list:\n"); for(size_t i = 0; i < list.size(); i++) { std::cout << list.at(i) << std::endl; } // Run behavior behavior_proxy_ptr->runBehavior("behavior_hello"); ros::Rate loop_rate(100); while(ros::ok()) { loop_rate.sleep(); ros::spinOnce(); } return 0; }
BTAction(std::string name) : as_(nh_, name, boost::bind(&BTAction::executeCB, this, _1), false), action_name_(name) { as_.start(); robot_ip = "192.168.0.188"; std::cout << "Robot ip to use is: " << robot_ip << std::endl; motion_proxy_ptr = new AL::ALMotionProxy("192.168.0.188", 9559); //put this in init() AL::ALValue stiffness_name("Body"); AL::ALValue stiffness(1.0f); AL::ALValue stiffness_time(1.0f); //motion_proxy_ptr->stiffnessInterpolation(stiffness_name, // stiffness, // stiffness_time); //motion_proxy_ptr->moveInit(); }
void initialize() { init_ = true; // Enable stiffness AL::ALValue stiffness_name("Body"); AL::ALValue stiffness(1.0f); AL::ALValue stiffness_time(1.0f); motion_proxy_ptr->stiffnessInterpolation(stiffness_name,stiffness,stiffness_time); // Stand //robotPosture->goToPosture("Stand",0.5f); // Init moving motion_proxy_ptr->moveInit(); // Initial Odometry x_0 = motion_proxy_ptr->getRobotPosition(true).at(0); y_0 = motion_proxy_ptr->getRobotPosition(true).at(1); z_0 = motion_proxy_ptr->getRobotPosition(true).at(2); }
void initialize() { init_ = true; // Enable stiffness AL::ALValue stiffness_name("Body"); AL::ALValue stiffness(1.0f); AL::ALValue stiffness_time(1.0f); motion_proxy_ptr->stiffnessInterpolation(stiffness_name,stiffness,stiffness_time); // Stand robotPosture->goToPosture("Stand",0.5f); // Init moving motion_proxy_ptr->moveInit(); // Init bearing std::vector<float> odometry = motion_proxy_ptr->getRobotPosition(useSensorValues); theta_temp = odometry.at(2); r.theta = angle(p.x-r.x,p.y-r.y); }
//Iteration function //Uses two loops to calculate solution for the given parameters void BeamModel::iteration(const Parameters parameters, Backfill backfill, Creep creep) { extern File file; max_iterations=50; not_converged = 1; iterations = 0; no_crack_opening = 0; error = 0; //Checked against 2 as checkboxes are assigned 2 when selected rather than 1 if ((parameters.outflow_model_on==2) & (parameters.verbose==2)) { dialog *e = new dialog; e->warning("Starting outflowLength refinement with outflow length = ", outflow_length); e->exec(); } do { // Refine given outflow length using the discharge analysis: lambda_last = outflow_length; zetabackfilledlast = zetabackfilled; // Dimensionless foundation stiffness stiffness(); // Dimensionless crack speed crackSpeed(parameters, backfill); file.collect(this, 0); // Determine by FD method the opening profile v*(zeta) for a given outflow length control.lambda and the properties of it which are needed for analysis. // Make first guess for closure length -- two nodes less than input value -- and construct FD solution: node_at_closure -= 2; double wstarmax = 0.0; FDprofile fdSolution(alpha, m, zetabackfilled, pressurefactor, residualpressure, parameters.elements_in_l, node_at_closure); short nodeAtClosure_previous = node_at_closure; // Store position of last node in this FD array double errorLast = fdSolution.closureMoment(); // ...and resulting d2v/dz2 at closure point, divided by that at crack tip if (parameters.verbose==2) { dialog *e = new dialog; e->warning("Starting closure length refinement with closure node = ",nodeAtClosure_previous, "and closure moment = ", errorLast); e->exec(); } // Make second guess for closure length (two nodes MORE than input value): node_at_closure += 4; if (parameters.verbose==2) { dialog *e = new dialog; e->warning("Second-guess closure node = ", node_at_closure); e->exec(); } // Prepare to refine closure length by iteration double dontNeedThis; error = 0; short maximumNonContact = 0; //Need to declare again here! Without this, program exits first loop immediately, hence provides incorrect solutions short notConverged = 1; do { // ...until bending moment at closure point is negligible compared to that at crack tip: fdSolution = FDprofile(alpha, m, zetabackfilled, pressurefactor, residualpressure, parameters.elements_in_l, node_at_closure); error = fdSolution.closureMoment(); short noSurfaceContact = 1; // FIXME: necessary? short minPoint = fdSolution.nodeAtMinimum(); // this is set to -1 if NO minimum is found within domain if (parameters.verbose==2) { dialog *e = new dialog; e->warning("At closure length iteration ",iterations," closure moment = ", error, " min at point ",minPoint); e->exec(); } if (minPoint > 0) { if (parameters.verbose==2) { dialog *e = new dialog; e->warning("BUT there's a minimum (crack surface overlap) to left of closure point ", minPoint); e->exec(); } // So back up to find maximum closure length with NO overlap: double tempError; node_at_closure = minPoint - 2; if (node_at_closure < (parameters.elements_in_l + 1)) node_at_closure = parameters.elements_in_l + 1; // Is this really necessary? Closure might really occur inside decompression length! short newMin; do { fdSolution = FDprofile(alpha, m, zetabackfilled, pressurefactor, residualpressure, parameters.elements_in_l, node_at_closure); tempError = fdSolution.closureMoment(); newMin = fdSolution.nodeAtMinimum(); if (parameters.verbose==2) { dialog *e = new dialog; e->warning("nodeAtClosure = ",node_at_closure, " error = ", tempError, " min point = ", newMin); e->exec(); } node_at_closure++; } while (newMin < 0); node_at_closure = node_at_closure - 1; if (parameters.verbose==2) { dialog *e = new dialog; e->warning("Least worst non-contacting solution nodeAtClosure = ", node_at_closure); e->exec(); } maximumNonContact = true; fdSolution = FDprofile(alpha, m, -1.0, pressurefactor, residualpressure, parameters.elements_in_l, node_at_closure); error = fdSolution.closureMoment(); integral_wstar2 = fdSolution.integral_wStar2(); fdSolution.findBackfillEjectPoint(zetabackfilleject, dontNeedThis); } else { node_at_closure++; if (parameters.verbose==2) { dialog *e = new dialog; e->warning("node interpolated = ", node_at_closure); e->exec(); } } if (maximumNonContact) // not necessarily converged, but the best available notConverged = 0; else if ((fabs(error) < 0.01 and noSurfaceContact) or maximumNonContact) { // converged: notConverged = false; fdSolution.outflowPointValues(wstar2, wstar2dash, wstar2dash2, integral_wstar2); fdSolution.findBackfillEjectPoint(zetabackfilleject, dontNeedThis); wstarmax = fdSolution.wStarMax(node_at_closure); } else { if (parameters.verbose==2) { dialog *e = new dialog; e->warning("Next try for iteration will be nodeAtClosure = ", node_at_closure); e->exec(); } errorLast = error; iterations++; } file.collect(this, 0); // done that refinement iteration } while (notConverged & (iterations < max_iterations)); if (parameters.verbose==2) { dialog *e = new dialog; e->warning("At nodeAtClosure = ", node_at_closure, " converged in ", iterations," iterations with error = ", error); e->exec(); } // done FD solution if (parameters.verbose==2) { dialog *e = new dialog; e->warning("Computed profile properties", "Ejection point = ", zetabackfilleject, "Opening at outflow point = ", wstarmax, "1st-deriv at outflow = ", wstar2dash, "2nd-deriv at outflow = ", wstar2dash2, "Integral to outflow = ", integral_wstar2); e->exec(); } if (integral_wstar2 > 0.0) { if (parameters.outflow_model_on==2) { double throatArea = integral_wstar2; OutflowProcess outflow(p1bar); outflow_length = pow(factor * outflow.get_tStarOutflow() / throatArea, 0.2); } // tStarOutflow being the number of characteristic times for discharge if (parameters.verbose==2) { dialog *e = new dialog; e->warning("alpha = ", alpha[1], "m = ", m[1], "integral_wStar2 = ", integral_wstar2, "New outflowLength = ", outflow_length); e->exec(); } lambdapow4 = pow(outflow_length, 4); v0 = v00 * lambdapow4; vStarRes = creep.residual_crack_closure / v0 / Constants::kilo; if ((fabs(1.0 - lambda_last / outflow_length) < node_resolution) and (fabs(1.0 - zetabackfilledlast / zetabackfilled) < node_resolution)) not_converged = false; iterations++; } else { no_crack_opening = 1; } file.collect(this, 0); } // end outflow length refinement while (not_converged & (iterations < max_iterations) and not no_crack_opening); }
template<bool definite> inline SM2 SimpleShell::simple_DP(const Info& I) const { // Both of these cases are unconditionally definite by default return !tweak ? I.scale*stiffness() : SM2(); }
int main(int argc, char **argv) { /********************* variables declaration **************************/ int info, itype, lda, ldb, lwork, order; /* variables for lapack function */ char jobz, uplo; /* variables for lapack function */ int nfreq; /* number of frequencies displayed on the screen */ int d; /* dimension of the problem - determine the size r of the partial basis*/ int shape; /* shape of the body */ int r; /* actual size of the partial basis */ int i, j; /* indices */ int ir1; int *itab, *ltab, *mtab, *ntab; /* tabulation of indices */ int *irk; int k; int ns; /* symmetry of the system */ int hextype; /* type of hexagonal symmetry - VTI or HTI*/ double d1, d2, d3; /* dimension of the sample */ double rho; /* density */ double **cm; double ****c; /* stiffness tensor */ double **e, **gamma, *work, **w; /* matrices of the eigenvalue problem */ double *wsort; int outeigen; /* 1 if eigenvectors calculated */ char *eigenfile; /** FILE *file; */ /********************* end variables declaration **********************/ /* hook up getpar to handle the parameters */ initargs(argc,argv); requestdoc(1); /* get required parameters */ if (!getparint("d", &d)) err("must specify d!\n"); if (!getpardouble("d1", &d1)) err("must specify d1!\n"); if (!getpardouble("d2", &d2)) err("must specify d2!\n"); if (!getpardouble("d3", &d3)) err("must specify d3!\n"); if (!getpardouble("rho", &rho)) err("must specify rho!\n"); if (!getparint("ns", &ns)) err("must specify ns!\n"); cm=ealloc2double(6,6); for (i=0; i<6; ++i) for (j=0; j<6; ++j) cm[i][j]=0.0; if (ns==2) { /* isotropic */ if (!getpardouble("c11", &cm[0][0])) err("must specify c11!\n"); if (!getpardouble("c44", &cm[3][3])) err("must specify c44!\n"); cm[0][0]=cm[0][0]/100; cm[3][3]=cm[3][3]/100; cm[1][1]=cm[2][2]=cm[0][0]; cm[4][4]=cm[5][5]=cm[3][3]; cm[0][1]=cm[0][2]=cm[1][2]=cm[0][0]- 2.0*cm[3][3]; cm[1][0]=cm[2][0]=cm[2][1]=cm[0][0]- 2.0*cm[3][3]; } else if (ns==3) { /* cubic */ if (!getpardouble("c11", &cm[0][0])) err("must specify c11!\n"); if (!getpardouble("c12", &cm[0][1])) err("must specify c12!\n"); if (!getpardouble("c44", &cm[3][3])) err("must specify c44!\n"); cm[0][0]=cm[0][0]/100; cm[0][1]=cm[0][1]/100; cm[3][3]=cm[3][3]/100; cm[1][1]=cm[2][2]=cm[0][0]; cm[4][4]=cm[5][5]=cm[3][3]; cm[0][2]=cm[1][2]=cm[0][1]; cm[2][0]=cm[2][1]=cm[1][0]=cm[0][1]; } else if (ns==5) { /* hexagonal */ if (!getparint("hextype", &hextype)) err("must specify hextype!\n"); if (hextype==1) { /* VTI */ if (!getpardouble("c33", &cm[2][2])) err("must specify c33!\n"); if (!getpardouble("c23", &cm[1][2])) err("must specify c23!\n"); if (!getpardouble("c12", &cm[0][1])) err("must specify c12!\n"); if (!getpardouble("c44", &cm[3][3])) err("must specify c44!\n"); if (!getpardouble("c66", &cm[5][5])) err("must specify c66!\n"); cm[2][2]=cm[2][2]/100; cm[1][2]=cm[1][2]/100; cm[0][1]=cm[0][1]/100; cm[3][3]=cm[3][3]/100; cm[5][5]=cm[5][5]/100; cm[0][0]=cm[1][1]=2.0*cm[5][5] + cm[0][1]; cm[0][2]=cm[2][0]=cm[2][1]=cm[1][2]; cm[1][0]=cm[0][1]; cm[4][4]=cm[3][3]; } else if (hextype==2) { /* HTI */ if (!getpardouble("c11", &cm[0][0])) err("must specify c11!\n"); if (!getpardouble("c33", &cm[2][2])) err("must specify c33!\n"); if (!getpardouble("c12", &cm[0][1])) err("must specify c12!\n"); if (!getpardouble("c44", &cm[3][3])) err("must specify c44!\n"); if (!getpardouble("c66", &cm[5][5])) err("must specify c66!\n"); cm[0][0]=cm[0][0]/100; cm[2][2]=cm[2][2]/100; cm[0][1]=cm[0][1]/100; cm[3][3]=cm[3][3]/100; cm[5][5]=cm[5][5]/100; cm[1][2]=cm[2][1]=cm[2][2] - 2.0*cm[3][3]; cm[0][2]=cm[1][0]=cm[2][0]=cm[0][1]; cm[1][1]=cm[2][2]; cm[4][4]=cm[5][5]; } else { err("for hexagonal symmetry hextype must equal 1 (VTI) or 2 (HTI)!\n"); } } else if (ns==6){ /* tetragonal */ if (!getpardouble("c11", &cm[0][0])) err("must specify c11!\n"); if (!getpardouble("c33", &cm[2][2])) err("must specify c33!\n"); if (!getpardouble("c23", &cm[1][2])) err("must specify c23!\n"); if (!getpardouble("c12", &cm[0][1])) err("must specify c12!\n"); if (!getpardouble("c44", &cm[3][3])) err("must specify c44!\n"); if (!getpardouble("c66", &cm[5][5])) err("must specify c66!\n"); cm[0][0]=cm[0][0]/100; cm[2][2]=cm[2][2]/100; cm[1][2]=cm[1][2]/100; cm[3][3]=cm[3][3]/100; cm[0][1]=cm[0][1]/100; cm[5][5]=cm[5][5]/100; cm[1][1]=cm[0][0]; cm[0][2]=cm[2][0]=cm[1][2]; cm[1][0]=cm[0][1]; cm[2][1]=cm[1][2]; cm[4][4]=cm[3][3]; } else if (ns==9){/* orthorhombic */ if (!getpardouble("c11", &cm[0][0])) err("must specify c11!\n"); if (!getpardouble("c22", &cm[1][1])) err("must specify c22!\n"); if (!getpardouble("c33", &cm[2][2])) err("must specify c33!\n"); if (!getpardouble("c23", &cm[1][2])) err("must specify c23!\n"); if (!getpardouble("c13", &cm[0][2])) err("must specify c13!\n"); if (!getpardouble("c12", &cm[0][1])) err("must specify c12!\n"); if (!getpardouble("c44", &cm[3][3])) err("must specify c44!\n"); if (!getpardouble("c55", &cm[4][4])) err("must specify c55!\n"); if (!getpardouble("c66", &cm[5][5])) err("must specify c66!\n"); cm[0][0]=cm[0][0]/100; cm[1][1]=cm[1][1]/100; cm[2][2]=cm[2][2]/100; cm[1][2]=cm[1][2]/100; cm[0][2]=cm[0][2]/100; cm[0][1]=cm[0][1]/100; cm[3][3]=cm[3][3]/100; cm[4][4]=cm[4][4]/100; cm[5][5]=cm[5][5]/100; cm[2][0]=cm[0][2]; cm[1][0]=cm[0][1]; cm[2][1]=cm[1][2]; } else err("given elatic moduli does not fit given ns"); /* get optional parameters */ if (!getparint("outeigen", &outeigen)) outeigen=0; if (outeigen!=0) if (!getparstring("eigenfile", &eigenfile)) err("must specify eigenfile since outeigen>0!\n"); if (!getparint("shape", &shape)) shape=1; /* changed from zero default to 1 */ if (!getparint("nfreq", &nfreq)) nfreq=10; /* dimension of the problem */ r= 3*(d+1)*(d+2)*(d+3)/6; d1=d1/2.0; /* half sample dimensions are used in calculations */ d2=d2/2.0; d3=d3/2.0; /* alloc work space*/ itab=ealloc1int(r); ltab=ealloc1int(r); mtab=ealloc1int(r); ntab=ealloc1int(r); /* relationship between ir and l,m,n - filling tables */ irk=ealloc1int(8); index_relationship(itab, ltab, mtab, ntab, d, irk); /* alloc workspace to solve for eigenvalues and eigenfunctions */ e= (double **) malloc(8*sizeof(double *)); for (k=0; k<8; ++k) e[k] = ealloc1double(irk[k]*irk[k]); gamma= (double **) malloc(8*sizeof(double *)); for (k=0; k<8; ++k) gamma[k] = ealloc1double(irk[k]*irk[k]); /* filling matrix e */ for (k=0; k<8; ++k) e_fill(e[k], itab, ltab, mtab, ntab, r, d1, d2, d3, rho, shape, k, irk); /* stiffness tensor calculation*/ c= (double ****) malloc(sizeof(double ***)*3); for (i=0; i<3; ++i) c[i]=ealloc3double(3,3,3); stiffness (c, cm); /* filling matrix gamma */ for (k=0; k<8; ++k) gamma_fill(gamma[k], itab, ltab, mtab, ntab, r, d1, d2, d3, c, shape, k, irk); /* clean workspace */ free1int(itab); free1int(ltab); free1int(mtab); free1int(ntab); for (i=0; i<3; ++i) free3double(c[i]); free(c); fprintf(stderr,"done preparing matrices\n"); /*-------------------------------------------------------------*/ /*--------- solve the generalized eigenvalue problem ----------*/ /*-------------------------------------------------------------*/ w= (double **) malloc(sizeof(double *)*8); itype=1; if (outeigen==0) jobz='N'; else jobz='V'; uplo='U'; for (k=0; k<8; ++k){ w[k] =ealloc1double(irk[k]); lda=ldb=irk[k]; order=irk[k]; lwork=MAX(1, 3*order-1); work=ealloc1double(lwork); /* lapack routine */ dsygv_(&itype, &jobz, &uplo, &order, gamma[k], &lda, e[k], &ldb, w[k], work, &lwork, &info); free1double(work); } /*-------------------------------------------------------------*/ /*-------------------------------------------------------------*/ /*-------------------------------------------------------------*/ wsort=ealloc1double(r); for (i=0, k=0; k<8; ++k) for (ir1=0;ir1<irk[k];++ir1,++i) wsort[i]=w[k][ir1]; /* sorting the eigenfrequencies */ dqksort(r,wsort); for (i=0, ir1=0; ir1<nfreq;++i) if ((wsort[i]>0) && ((sqrt(wsort[i])/(2.0*PI))>0.00001)){ ++ir1; /*fprintf(stderr," f%d = %f\n", ir1, 1000000*sqrt(wsort[i])/(2.0*PI));*/ fprintf(stderr," f%d = %f\n", ir1, 1000000*sqrt(wsort[i])/(2.0*PI)); } /* modify output of freq values here*/ /* for (k=0;k<8;++k){ for (ir2=0;ir2<irk[k]*irk[k];++ir2){ fprintf(stderr,"gamma[%d][%d]=%f\n",k,ir2,gamma[k][ir2]); fprintf(stderr,"e[%d][%d]=%f\n",k,ir2,e[k][ir2]); } }*/ /******************* write eigenvectors in files ***************/ /*if (outeigen==1){ z=ealloc2double(r,r); for (ir1=0; ir1<r; ++ir1) for (ir2=0; ir2<r; ++ir2) z[ir2][ir1]=gamma[ir1][ir2*r+ir1]; */ /* change the order of the array at the same time */ /* since we go from fortran array */ /* to C array */ /* clean workspace */ /* free1double(gamma); file = efopen(eigenfile, "w"); efwrite(&irf, sizeof(int), 1, file); efwrite(w, sizeof(double), r, file); efwrite(z[0], sizeof(double), r*r, file); efclose(file);*/ /* clean workspace */ /* free2double(z); */ /* }*/ /* clean workspace */ /* free1double(w); */ /* end of main */ return EXIT_SUCCESS; }