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();
}
