int _seze(int elen, double *e, double b, double *h) { double Q, sum, hh, product1, product0, enow, _bvr, _avr, _brn, _arn, c; double abig, ahi, alo, bhi, blo, err1, err2, err3; int eindex, hindex; SPLT(b, bhi, blo); TPP(e[0], b, bhi, blo, Q, hh); hindex = 0; if (hh != 0) { h[hindex++] = hh; } for (eindex = 1; eindex < elen; eindex++) { enow = e[eindex]; TPP(enow, b, bhi, blo, product1, product0); TWS(Q, product0, sum, hh); if (hh != 0) { h[hindex++] = hh; } FTS(product1, sum, Q, hh); if (hh != 0) { h[hindex++] = hh; } } if ((Q != 0.0) || (hindex == 0)) { h[hindex++] = Q; } return hindex; }
//------------------------------------------------------------------ VectorXui generateRandomGrasp(TargetObjectPtr object,const FParamList f_parameters, WrenchSpacePtr const tws) { assert(object->getNumCp() >= f_parameters.size()); struct timeval c_time; uint n_fingers=f_parameters.size(); VectorXui centerpoint_ids(n_fingers); while(1) { std::list<uint> l; for(uint i=0; i<n_fingers;i++) { while (1) { gettimeofday(&c_time,0); srand(c_time.tv_usec); bool is_unique=true; uint rand_val=(rand() % (object->getNumCp())); for (uint j=0; j<i;j++) if (centerpoint_ids(j)==rand_val) is_unique=false; if (is_unique) { centerpoint_ids(i)=rand_val; break; } } } Grasp grasp; grasp.init(f_parameters,object,centerpoint_ids); if (tws->getWrenchSpaceType()==Spherical) { if(grasp.getGWS()->getOcInsphereRadius() >= dynamic_cast<const SphericalWrenchSpace*>(tws.get())->getRadius()) break; } else if (tws->getWrenchSpaceType()==Discrete) { //Should ascertain that the TWS contains the origin ... uint nTW = dynamic_cast<const DiscreteTaskWrenchSpace*>(tws.get())->getNumWrenches(); Eigen::MatrixXd TWS(6,nTW); doubleArrayToEigenMatrix(dynamic_cast<const DiscreteTaskWrenchSpace*>(tws.get())->getWrenches().get(),TWS); bool contained =true; Eigen::MatrixXd nh(6,1); Eigen::MatrixXd eh(1,1); facetT* curr_f= grasp.getGWS()->getConvexHull()->beginFacet().getFacetT(); for (uint h=0; h< grasp.getGWS()->getConvexHull()->facetCount();h++) { doubleArrayToEigenMatrix(curr_f->normal,nh); eh(0,0)=-curr_f->offset; nh=-nh; if((TWS.transpose()*nh+eh.replicate(nTW,1)).minCoeff() <= 0) { contained =false; break; } curr_f=curr_f->next; } if (contained) break; } else { std::cout<<"Error in generateRandomGrasp(TargetObjectPtr object,const FParamList f_parameters,const WrenchSpace* const tws) - Invalid task wrench space type, cannot compute a valid random grasp. Exiting ..."<<std::endl; exit(0); } } return centerpoint_ids; }
//Computes a new tokenLength for the next token //Modifies: size_t tokenLength, and bool complete //(Optionally): may modify offset //Does NOT modify any other member variable of Tokenizer void Tokenizer::prepareNextToken() { if (str->length() == 0) //Empty string is detected { complete = true; //complete } //trim white spaces if (!complete && !comment) { TWS(); } if (!complete) { //Initialize variables int strLength = str->length(); int i = offset; bool found = false;//find tokens for each case if (comment)//Comment case { tokenLength = strLength - offset; //Everything after '--' is a token } else { while (i < strLength && !found) { switch (str->at(i)) { //Case of space and a tab case ' ': case '\t': tokenLength = i - offset; if (tokenLength == 0) { if (i == strLength) { complete = true; found = true; } else { offset++; i++; found = false; } } found = true; break; case '-': tokenLength = i - offset; if ((i < strLength - 1) && (str->at(i + 1) == '-') && (tokenLength == 0))//Detect '--' { tokenLength = 2; } if (!comment && tokenLength == 0) { tokenLength = 1; } found = true; break; case '=': tokenLength = i - offset; if ((i < strLength - 1) && (str->at(i + 1) == '>') && (tokenLength == 0))//Detect '=>' { tokenLength = 2; //Length of '=>' } else if (tokenLength == 0) { tokenLength = 1; } found = true; break; case '<': tokenLength = i - offset; if ( (i < strLength - 1) && (str->at(i + 1) == '=') && (tokenLength == 0))//Detect '<=' { tokenLength = 2; //Length of '<=' } if ((i < strLength - 1) && (str->at(i + 1) == '>') && (tokenLength == 0))//Detect '<>' { tokenLength = 2; //Length of '<>' } else if (tokenLength == 0) { tokenLength = 1; } found = true; break; case '/': case '>': case ':': tokenLength = i - offset; if ((i < strLength - 1) && (str->at(i + 1) == '=') && (tokenLength == 0))//Detect '/=','>=','==' { tokenLength = 2; } else if (tokenLength == 0) { tokenLength = 1; } found = true; break; case '*': tokenLength = i - offset; if ((i < strLength - 1) && (str->at(i + 1) == '*') && (tokenLength == 0))//Detect '**' { tokenLength = 2; //Length of '**' } else if (tokenLength == 0) { tokenLength = 1; } found = true; break; case '"': while (i < strLength - 1 && !(str->at(i) != '--' && str->at(i + 1) == '"')) { i++; } tokenLength = i + 2 - offset; found = true; break; case '\'': if ((i < strLength - 1) && (str->at(i + 2) != '\'') && (tokenLength == 0)) { tokenLength = 1; offset = i; } else if((i < strLength - 1) && (str->at(i + 2) == '\'') && (tokenLength == 0)) { tokenLength = 3; } found = true; break; case ',': case '.': case '+': case '&': case '|': case ';': case '#': case '(': case ')': tokenLength = i - offset; if (tokenLength == 0) { tokenLength = 1; } found = true; break; default: i++; }//SWITCH STATMENT }//while (i < strLength && !found) }//ELSE if (i == strLength) { tokenLength = strLength - offset; } }//if (!iscomplete) if (offset == str->length()) //At end of lsit { complete = true; //return complete } }//void
int _fesze(int elen, double *e, int flen, double *f, double *h) { double Q, Qnew, hh, _bvr, _avr, _brn, _arn, enow, fnow; int eindex, findex, hindex; enow = e[0]; fnow = f[0]; eindex = findex = 0; if ((fnow > enow) == (fnow > -enow)) { Q = enow; enow = e[++eindex]; } else { Q = fnow; fnow = f[++findex]; } hindex = 0; if ((eindex < elen) && (findex < flen)) { if ((fnow > enow) == (fnow > -enow)) { FTS(enow, Q, Qnew, hh); enow = e[++eindex]; } else { FTS(fnow, Q, Qnew, hh); fnow = f[++findex]; } Q = Qnew; if (hh != 0.0) { h[hindex++] = hh; } while ((eindex < elen) && (findex < flen)) { if ((fnow > enow) == (fnow > -enow)) { TWS(Q, enow, Qnew, hh); enow = e[++eindex]; } else { TWS(Q, fnow, Qnew, hh); fnow = f[++findex]; } Q = Qnew; if (hh != 0.0) { h[hindex++] = hh; } } } while (eindex < elen) { TWS(Q, enow, Qnew, hh); enow = e[++eindex]; Q = Qnew; if (hh != 0.0) { h[hindex++] = hh; } } while (findex < flen) { TWS(Q, fnow, Qnew, hh); fnow = f[++findex]; Q = Qnew; if (hh != 0.0) { h[hindex++] = hh; } } if ((Q != 0.0) || (hindex == 0)) { h[hindex++] = Q; } return hindex; }
int main(int argc, char **argv) { vector<const Row*> table; CapabilityController controller(table); TestStrategyControllable RRM("RRM"); BarBased TWS("TWS"); TestStrategyControllable CAL("CAL"); TestStrategyControllable CS("CS"); RRM.start(); CAL.start(); TWS.start(); CS.start(); map<const ICapability*, const IStrategyCommandFactory*> tws_capabilites; TestStrategyCommand shutdown_command("Shutdown"); SimpleCommandFactory shutdown_factory(shutdown_command); TestStrategyCommand tws_a_command("TWS to A!"); SimpleCommandFactory tws_a_factory(tws_a_command); tws_capabilites[&TestCapability::A] = &tws_a_factory; TestStrategyCommand tws_b_command("TWS to B!"); SimpleCommandFactory tws_b_factory(tws_b_command); tws_capabilites[&TestCapability::B] = &tws_b_factory; tws_capabilites[&TestCapability::Shutdown] = &shutdown_factory; vector<const IStrategyControllable*> tws_prereqs; tws_prereqs.push_back(&RRM); tws_prereqs.push_back(&CAL); Row tws_row(TWS, tws_capabilites, tws_prereqs); table.push_back(&tws_row); map<const ICapability*, const IStrategyCommandFactory*> cal_capabilites; TestStrategyCommand cal_a_command("CAL to A!"); SimpleCommandFactory cal_a_factory(cal_a_command); cal_capabilites[&TestCapability::A] = &cal_a_factory; TestStrategyCommand cal_b_command("CAL to B!"); SimpleCommandFactory cal_b_factory(cal_b_command); cal_capabilites[&TestCapability::B] = &cal_b_factory; cal_capabilites[&TestCapability::Shutdown] = &shutdown_factory; vector<const IStrategyControllable*> cal_prereqs; cal_prereqs.push_back(&RRM); Row cal_row(CAL, cal_capabilites, cal_prereqs); table.push_back(&cal_row); map<const ICapability*, const IStrategyCommandFactory*> cs_capabilites; TestStrategyCommand cs_a_command("CS to A!"); SimpleCommandFactory cs_a_factory(cs_a_command); cs_capabilites[&TestCapability::A] = &cs_a_factory; TestStrategyCommand cs_b_command("CS to B!"); SimpleCommandFactory cs_b_factory(cs_b_command); cs_capabilites[&TestCapability::B] = &cs_b_factory; cs_capabilites[&TestCapability::Shutdown] = &shutdown_factory; vector<const IStrategyControllable*> cs_prereqs; cs_prereqs.push_back(&RRM); cs_prereqs.push_back(&CAL); Row cs_row(CS, cs_capabilites, cs_prereqs); table.push_back(&cs_row); map<const ICapability*, const IStrategyCommandFactory*> rrm_capabilites; TestStrategyCommand rrm_a_command("RRM to A!"); SimpleCommandFactory rrm_a_factory(rrm_a_command); rrm_capabilites[&TestCapability::A] = &rrm_a_factory; TestStrategyCommand rrm_b_command("RRM to B!"); SimpleCommandFactory rrm_b_factory(rrm_b_command); rrm_capabilites[&TestCapability::B] = &rrm_b_factory; rrm_capabilites[&TestCapability::Shutdown] = &shutdown_factory; vector<const IStrategyControllable*> rrm_prereqs; Row rrm_row(RRM, rrm_capabilites, rrm_prereqs); table.push_back(&rrm_row); Thread::sleep(500); ChangeAt change_at_bar(ChangeAt::END_OF_BAR); TestStrategyChangeParameters params_bar(change_at_bar); cout << endl << "**********************" << endl; cout << "main: Asking for change to Capability A at end of bar" << endl; controller.changeCapability(TestCapability::A, params_bar); ChangeAt change_at_frame(ChangeAt::END_OF_FRAME); TestStrategyChangeParameters params_frame(change_at_frame); cout << endl << "**********************" << endl; cout << "main: Asking for change to Capability B at end of frame" << endl; controller.changeCapability(TestCapability::B, params_frame); ChangeAt change_at_now(ChangeAt::IMMEDIATE); TestStrategyChangeParameters params_now(change_at_now); cout << endl << "**********************" << endl; cout << "main: Asking for immediate shutdown" << endl; controller.changeCapability(TestCapability::Shutdown, params_now); RRM.join(); CAL.join(); TWS.join(); CS.join(); }