transf PositionStateEllipsoid::getCoreTran() const { double a = getParameter("a"); double b = getParameter("b"); double c = getParameter("c"); double beta = readVariable("beta"); double gamma = readVariable("gamma"); double tau = readVariable("tau"); double distance = readVariable("dist"); double px,py,pz; px = a * cos(beta) * cos(gamma); py = b * cos(beta) * sin(gamma); pz = c * sin(beta); //compute normal direction - for some reason this always points INSIDE the ellipsoid vec3 n1( -a*sin(beta)*cos(gamma), -b*sin(beta)*sin(gamma), c*cos(beta) ); vec3 n2( -a*cos(beta)*sin(gamma), b*cos(beta)*cos(gamma), 0 ); vec3 normal = normalise(n1) * normalise(n2); vec3 xdir(1,0,0); vec3 ydir = normal * normalise(xdir); xdir = ydir * normal; mat3 r(xdir,ydir,normal); transf handTran = transf(r, vec3(px,py,pz) - distance * normal); Quaternion zrot(tau, vec3(0,0,1)); handTran = transf(zrot, vec3(0,0,0) ) * handTran; return mHand->getApproachTran().inverse() * handTran; //So: hand tranform is: move onto the ellipsoid and rotate z axis in normal direction // --> hand approach transform inverted to get to hand origin }
/*! Right now we define the distance as the max distance between any two variables. Alternatively, the mean distance between all variables could also be used. For position states we could also look at the distance in actual position. */ double VariableSet::distance(const VariableSet *s) const { if (getNumVariables() != s->getNumVariables()) return -1; double distance = 0; for (int i=0; i<getNumVariables(); i++) { double altD; double d = readVariable(i) - s->readVariable(i); if ( getVariable(i)->isCircular() ) { //if the variable is circular, the shortest distance might be around the end //going one direction or the other altD = fabs( readVariable(i) - getVariable(i)->mMinVal ) + fabs( s->readVariable(i) - getVariable(i)->mMaxVal ); d = std::min(d, altD); altD = fabs( s->readVariable(i) - getVariable(i)->mMinVal ) + fabs( readVariable(i) - getVariable(i)->mMaxVal ); d = std::min(d, altD); } d = fabs(d) / getVariable(i)->getRange(); distance = std::max(d, distance); } return distance; }
void AGOSEngine_Feeble::off_loadUserGame() { // 133: load game if (readVariable(55) == 999) { loadGame(getFileName(GAME_RESTFILE), true); } else { loadGame(genSaveName(readVariable(55))); } }
transf PositionStateApproach::getCoreTran() const { double dist = readVariable("dist"); double rx = readVariable("wrist 1"); double ry = readVariable("wrist 2"); transf handTran = transf(Quaternion::IDENTITY, vec3(0,0,dist)); handTran = handTran * rotate_transf(rx, vec3(1,0,0) ) * rotate_transf( ry, vec3(0,1,0) ); return mHand->getApproachTran().inverse() * handTran * mHand->getApproachTran(); }
void AGOSEngine::o_add() { // 43: add uint var = getVarWrapper(); writeVariable(var, readVariable(var) + getVarOrWord()); // WORKAROUND: The conversation of the male in Vid-Phone Booth at Dave's Space Bar // is based on variable 116, but stops due to a missing option (37). if (getGameType() == GType_FF && _currentTable->id == 10538 && readVariable(116) == 37) writeVariable(116, 38); }
uint AGOSEngine::getVarOrWord() { uint a = READ_BE_UINT16(_codePtr); _codePtr += 2; if (getGameType() == GType_PP) { if (a >= 60000 && a < 62048) { return readVariable(a - 60000); } } else { if (a >= 30000 && a < 30512) { return readVariable(a - 30000); } } return a; }
//all input instructions are assumed to be valid. //i.e. sytactically correct and no illegal operations //(e.g. division by zero) //variables may not start with digits //variables are assumed to be integers int main(){ char curr_var[MAX_VAR_IDENT_LENGTH]; char curr_operator; char curr_operand[MAX_VAR_IDENT_LENGTH]; /*Input from file for testing purposes FILE *fin; if( (fin = fopen("in.txt","r")) == NULL){ printf("file not found\n"); exit(1); } fscanf(fin,"%s",inst); */ /*input from keyboard*/ scanf("%s",inst); /**/ instIndex = 0; int instLen = strlen(inst)-1; while(instIndex<instLen){ readVariable(curr_var); readOperator(&curr_operator); readOperand(curr_operand); compute(curr_var, curr_operator, curr_operand); } printVars(); return 0; }
void AGOSEngine_Feeble::off_saveUserGame() { // 132: save game _noOracleScroll = 0; _system->setFeatureState(OSystem::kFeatureVirtualKeyboard, true); saveUserGame(countSaveGames() + 1 - readVariable(55)); _system->setFeatureState(OSystem::kFeatureVirtualKeyboard, false); }
void AGOSEngine::o_mod() { // 51: mod uint var = getVarWrapper(); int value = getVarOrWord(); if (value == 0) error("o_mod: Division by zero"); writeVariable(var, readVariable(var) % value); }
void AGOSEngine::o_modf() { // 52: mod f uint var = getVarWrapper(); int value = getNextVarContents(); if (value == 0) error("o_modf: Division by zero"); writeVariable(var, readVariable(var) % value); }
void AGOSEngine::o_div() { // 48: div uint var = getVarWrapper(); int value = getVarOrWord(); if (value == 0) error("o_div: Division by zero"); writeVariable(var, readVariable(var) / value); }
uint AGOSEngine::getVarOrByte() { if (getGameType() == GType_ELVIRA1) { return getVarOrWord(); } else { uint a = *_codePtr++; if (a != 255) return a; return readVariable(*_codePtr++); } }
string Request::get(string key, string fallback) { const char *dataField; string output; // Looking on the query string dataField = connection->query_string; if (dataField != NULL && readVariable(dataField, key, output)) { return output; } // Looking on the POST data dataField = data.c_str(); if (dataField != NULL && readVariable(dataField, key, output)) { return output; } return fallback; }
void PostureStateEigen::getHandDOF(double *dof) const { double *eg = new double[ mHand->getEigenGrasps()->getSize() ]; for (int i=0; i < mHand->getEigenGrasps()->getSize(); i++) { eg[i] = readVariable(i); } bool r = mHand->getEigenGrasps()->isRigid(); mHand->getEigenGrasps()->setRigid(true); mHand->getEigenGrasps()->getDOF( eg, dof ); mHand->checkSetDOFVals(dof); mHand->getEigenGrasps()->setRigid(r); delete [] eg; }
/*! This is a redundant way of saving the transform (since the Q doesn't need 4 variables since it's normalized. It's just the simplest way to interface with a transf so it's used as an interface between other types. Not really meant for performing searches. */ transf PositionStateComplete::getCoreTran() const { double tx = readVariable("Tx"); double ty = readVariable("Ty"); double tz = readVariable("Tz"); double qw = readVariable("Qw"); double qx = readVariable("Qx"); double qy = readVariable("Qy"); double qz = readVariable("Qz"); return transf(Quaternion(qw,qx,qy,qz), vec3(tx,ty,tz)); }
void AGOSEngine_Feeble::off_checkCD() { // 135: switch CD uint16 disc = readVariable(97); if (!strcmp(getExtra(), "4CD")) { _sound->switchVoiceFile(gss, disc); } else if (!strcmp(getExtra(), "2CD")) { if (disc == 1 || disc == 2) _sound->switchVoiceFile(gss, 1); else if (disc == 3 || disc == 4) _sound->switchVoiceFile(gss, 2); } debug(0, "Switch to CD number %d", disc); }
void AGOSEngine_Simon1::os1_getPathPosn() { // 178: path find uint x = getVarOrWord(); uint y = getVarOrWord(); uint var_1 = getVarOrByte(); uint var_2 = getVarOrByte(); const uint16 *p; uint i, j; uint prev_i; uint x_diff, y_diff; uint best_i = 0, best_j = 0, best_dist = 0xFFFFFFFF; uint maxPath = (getGameType() == GType_FF || getGameType() == GType_PP) ? 100 : 20; if (getGameType() == GType_FF || getGameType() == GType_PP) { x += _scrollX; y += _scrollY; } else if (getGameType() == GType_SIMON2) { x += _scrollX * 8; } int end = (getGameType() == GType_FF) ? 9999 : 999; prev_i = maxPath + 1 - readVariable(12); for (i = maxPath; i != 0; --i) { p = (const uint16 *)_pathFindArray[maxPath - i]; if (!p) continue; for (j = 0; readUint16Wrapper(&p[0]) != end; j++, p += 2) { x_diff = ABS((int16)(readUint16Wrapper(&p[0]) - x)); y_diff = ABS((int16)(readUint16Wrapper(&p[1]) - 12 - y)); if (x_diff < y_diff) { x_diff /= 4; y_diff *= 4; } x_diff += y_diff /= 4; if ((x_diff < best_dist) || ((x_diff == best_dist) && (prev_i == i))) { best_dist = x_diff; best_i = maxPath + 1 - i; best_j = j; } } } writeVariable(var_1, best_i); writeVariable(var_2, best_j); }
// load a polynomial from the specified file bool PolynomialFunction::load( const string &file_in ) { // open the specified file fs::fstream ifs( file_in, std::ios_base::in ); if ( !ifs.is_open() ) { std::cerr << "PolynomialFunction::Load: Failed to open file for reading " << file_in << std::endl; return false; } VariableID unique_var_id = 0; string line; try { std::getline( ifs, line ); string name, domain; while ( !ifs.eof() && !ifs.bad() ) { // ignore empty or comment lines if ( line.empty() || line[0] == '#' ) { std::getline( ifs, line ); continue; } size_t equals_pos = line.find( '=' ); if ( equals_pos != string::npos ) { readVariable( line, equals_pos, unique_var_id ); } else { readFactor( line, unique_var_id ); } std::getline( ifs, line ); } } catch ( std::exception & e ) { std::cerr << "PolynomialFunction::load(" << file_in << ") caught exception: " << std::endl << e.what() << std::endl; } // close file if ( ifs.is_open() ) ifs.close(); init(); return ( variables.size() > 0 && factors.size() > 0 ); }
transf PositionStateAA::getCoreTran() const { double tx = readVariable("Tx"); double ty = readVariable("Ty"); double tz = readVariable("Tz"); double theta = readVariable("theta"); double phi = readVariable("phi"); double alpha = readVariable("alpha"); transf coreTran = rotate_transf(alpha, vec3( sin(theta)*cos(phi) , sin(theta)*sin(phi) , cos(theta) )) * translate_transf(vec3(tx,ty,tz)); //transform now returned relative to hand approach transform return mHand->getApproachTran().inverse() * coreTran; }
void ProblemsReader::readProblem(unsigned int /*numProblem*/) { unsigned int numVariables; *_stream >> numVariables; // create a new problem boost::shared_ptr<Problem> problem = boost::make_shared<Problem>(numVariables); // add it to the existing problems _problems->addProblem(problem); LOG_DEBUG(streamproblemreaderlog) << "reading " << numVariables << " variables" << std::endl; for (unsigned int i = 0; i < numVariables; i++) readVariable(*problem, i); unsigned int numConstraints; *_stream >> numConstraints; for (unsigned int i = 0; i < numConstraints; i++) readConstraint(*problem, i); }
Foam::Istream& Foam::ISstream::read(token& t) { static const int maxLen = 128; static char buf[maxLen]; // Return the put back token if it exists if (Istream::getBack(t)) { return *this; } // Assume that the streams supplied are in working order. // Lines are counted by '\n' // Get next 'valid character': i.e. proceed through any whitespace // and/or comments until a semantically valid character is found char c = nextValid(); // Set the line number of this token to the current stream line number t.lineNumber() = lineNumber(); // return on error if (!c) { t.setBad(); return *this; } // Analyse input starting with this character. switch (c) { // Check for punctuation first case token::END_STATEMENT : case token::BEGIN_LIST : case token::END_LIST : case token::BEGIN_SQR : case token::END_SQR : case token::BEGIN_BLOCK : case token::END_BLOCK : case token::COLON : case token::COMMA : case token::ASSIGN : case token::ADD : // NB: token::SUBTRACT handled later as the possible start of a Number case token::MULTIPLY : case token::DIVIDE : { t = token::punctuationToken(c); return *this; } // String: enclosed by double quotes. case token::BEGIN_STRING : { putback(c); string* sPtr = new string; if (read(*sPtr).bad()) { delete sPtr; t.setBad(); } else { t = sPtr; } return *this; } // Possible verbatim string or dictionary functionEntry case token::HASH : { char nextC; if (read(nextC).bad()) { // Return hash as word t = token(word(c)); return *this; } else if (nextC == token::BEGIN_BLOCK) { // Verbatim string string* sPtr = new string; if (readVerbatim(*sPtr).bad()) { delete sPtr; t.setBad(); } else { t = sPtr; t.type() = token::VERBATIMSTRING; } return *this; } else { // Word beginning with # putback(nextC); putback(c); readWordToken(t); return *this; } } case '$': { // Look ahead char nextC; if (read(nextC).bad()) { // Return $ as word t = token(word(c)); return *this; } else if (nextC == token::BEGIN_BLOCK) { putback(nextC); putback(c); string* sPtr = new string; if (readVariable(*sPtr).bad()) { delete sPtr; t.setBad(); } else { t = sPtr; t.type() = token::VARIABLE; } return *this; } else { putback(nextC); putback(c); readWordToken(t); return *this; } } // Number: integer or floating point // // ideally match the equivalent of this regular expression // // /[-+]?([0-9]+\.?[0-9]*|\.[0-9]+)([Ee][-+]?[0-9]+)?/ // case '-' : case '.' : case '0' : case '1' : case '2' : case '3' : case '4' : case '5' : case '6' : case '7' : case '8' : case '9' : { bool asLabel = (c != '.'); int nChar = 0; buf[nChar++] = c; // get everything that could resemble a number and let // readScalar determine the validity while ( is_.get(c) && ( isdigit(c) || c == '+' || c == '-' || c == '.' || c == 'E' || c == 'e' ) ) { if (asLabel) { asLabel = isdigit(c); } buf[nChar++] = c; if (nChar == maxLen) { // runaway argument - avoid buffer overflow buf[maxLen-1] = '\0'; FatalIOErrorInFunction(*this) << "number '" << buf << "...'\n" << " is too long (max. " << maxLen << " characters)" << exit(FatalIOError); t.setBad(); return *this; } } buf[nChar] = '\0'; setState(is_.rdstate()); if (is_.bad()) { t.setBad(); } else { is_.putback(c); if (nChar == 1 && buf[0] == '-') { // a single '-' is punctuation t = token::punctuationToken(token::SUBTRACT); } else { if (asLabel) { label labelVal = 0; if (Foam::read(buf, labelVal)) { t = labelVal; } else { // Maybe too big? Try as scalar scalar scalarVal; if (readScalar(buf, scalarVal)) { t = scalarVal; } else { t.setBad(); } } } else { scalar scalarVal; if (readScalar(buf, scalarVal)) { t = scalarVal; } else { t.setBad(); } } } } return *this; } // Should be a word (which can also be a single character) default: { putback(c); readWordToken(t); return *this; } } }
void PostureStateDOF::getHandDOF(double *dof) const { for (int i=0; i<mHand->getNumDOF(); i++) { dof[i] = readVariable(i); } }
int main(int argc, char* argv[]) { if(argc < 2) { std::cerr << "You should provide a system as argument, either gazebo or flat_fish." << std::endl; return 1; } //======================================================================================== // SETTING VARIABLES //======================================================================================== std::string system = argv[1]; double weight; double buoyancy; std::string dataFolder; if(system == "gazebo") { weight = 4600; buoyancy = 4618; dataFolder = "./config/gazebo/"; } else if(system == "flatfish" || system == "flat_fish") { weight = 2800; buoyancy = 2812; dataFolder = "./config/flatfish/"; } else { std::cerr << "Unknown system " << system << "." << std::endl; return 1; } double uncertainty = 0; /* true - Export the code to the specified folder * false - Simulates the controller inside Acado's environment */ std::string rockFolder = "/home/rafaelsaback/rock/1_dev/control/uwv_model_pred_control/src"; /*********************** * CONTROLLER SETTINGS ***********************/ // Prediction and control horizon double horizon = 2; double sampleTime = 0.1; int iteractions = horizon / sampleTime; bool positionControl = false; //======================================================================================== // DEFINING VARIABLES //======================================================================================== DifferentialEquation f; // Variable that will carry 12 ODEs (6 for position and 6 for velocity) DifferentialState v("Velocity", 6, 1); // Velocity States DifferentialState n("Pose", 6, 1); // Pose States DifferentialState tau("Efforts", 6, 1); // Effort Control tauDot("Efforts rate", 6, 1); // Effort rate of change DVector rg(3); // Center of gravity DVector rb(3); // Center of buoyancy DMatrix M(6, 6); // Inertia matrix DMatrix Minv(6, 6); // Inverse of inertia matrix DMatrix Dl(6, 6); // Linear damping matrix DMatrix Dq(6, 6); // Quadratic damping matrix // H-representation of the output domain's polyhedron DMatrix AHRep(12,6); DMatrix BHRep(12,1); Expression linearDamping(6, 6); // Dl*v Expression quadraticDamping(6, 6); // Dq*|v|*v Expression gravityBuoyancy(6); // g(n) Expression absV(6, 6); // |v| Expression Jv(6); // J(n)*v Expression vDot(6); // AUV Fossen's equation Function J; // Cost function Function Jn; // Terminal cost function AHRep = readVariable("./config/", "a_h_rep.dat"); BHRep = readVariable("./config/", "b_h_rep.dat"); M = readVariable(dataFolder, "m_matrix.dat"); Dl = readVariable(dataFolder, "dl_matrix.dat"); Dq = readVariable(dataFolder, "dq_matrix.dat"); /*********************** * LEAST-SQUARES PROBLEM ***********************/ // Cost Functions if (positionControl) { J << n; Jn << n; } else { J << v; Jn << v; } J << tauDot; // Weighting Matrices for simulational controller DMatrix Q = eye<double>(J.getDim()); DMatrix QN = eye<double>(Jn.getDim()); Q = readVariable("./config/", "q_matrix.dat"); //======================================================================================== // MOTION MODEL EQUATIONS //======================================================================================== rg(0) = 0; rg(1) = 0; rg(2) = 0; rb(0) = 0; rb(1) = 0; rb(2) = 0; M *= (2 -(1 + uncertainty)); Dl *= 1 + uncertainty; Dq *= 1 + uncertainty; SetAbsV(absV, v); Minv = M.inverse(); linearDamping = Dl * v; quadraticDamping = Dq * absV * v; SetGravityBuoyancy(gravityBuoyancy, n, rg, rb, weight, buoyancy); SetJv(Jv, v, n); // Dynamic Equation vDot = Minv * (tau - linearDamping - quadraticDamping - gravityBuoyancy); // Differential Equations f << dot(v) == vDot; f << dot(n) == Jv; f << dot(tau) == tauDot; //======================================================================================== // OPTIMAL CONTROL PROBLEM (OCP) //======================================================================================== OCP ocp(0, horizon, iteractions); // Weighting Matrices for exported controller BMatrix Qexp = eye<bool>(J.getDim()); BMatrix QNexp = eye<bool>(Jn.getDim()); ocp.minimizeLSQ(Qexp, J); ocp.minimizeLSQEndTerm(QNexp, Jn); ocp.subjectTo(f); // Individual degrees of freedom constraints ocp.subjectTo(tau(3) == 0); ocp.subjectTo(tau(4) == 0); /* Note: If the constraint for Roll is not defined the MPC does not * work since there's no possible actuation in that DOF. * Always consider Roll equal to zero. */ // Polyhedron set of inequalities ocp.subjectTo(AHRep*tau - BHRep <= 0); //======================================================================================== // CODE GENERATION //======================================================================================== // Export code to ROCK OCPexport mpc(ocp); int Ni = 1; mpc.set(HESSIAN_APPROXIMATION, GAUSS_NEWTON); mpc.set(DISCRETIZATION_TYPE, SINGLE_SHOOTING); mpc.set(INTEGRATOR_TYPE, INT_RK4); mpc.set(NUM_INTEGRATOR_STEPS, iteractions * Ni); mpc.set(HOTSTART_QP, YES); mpc.set(QP_SOLVER, QP_QPOASES); mpc.set(GENERATE_TEST_FILE, NO); mpc.set(GENERATE_MAKE_FILE, NO); mpc.set(GENERATE_MATLAB_INTERFACE, NO); mpc.set(GENERATE_SIMULINK_INTERFACE, NO); mpc.set(CG_USE_VARIABLE_WEIGHTING_MATRIX, YES); mpc.set(CG_HARDCODE_CONSTRAINT_VALUES, YES); if (mpc.exportCode(rockFolder.c_str()) != SUCCESSFUL_RETURN) exit( EXIT_FAILURE); mpc.printDimensionsQP(); return EXIT_SUCCESS; }
uint AGOSEngine::getNextVarContents() { return (uint16)readVariable(getVarWrapper()); }
void AGOSEngine_Feeble::saveUserGame(int slot) { WindowBlock *window; Common::InSaveFile *in; char name[108]; int len; memset(name, 0, 108); window = _windowArray[3]; window->textRow = (slot + 1 - window->scrollY) * 15; window->textColumn = 26; if ((in = _saveFileMan->openForLoading(genSaveName(readVariable(55))))) { in->read(name, 100); delete in; } len = 0; while (name[len]) { byte chr = name[len]; window->textColumn += getFeebleFontSize(chr); len++; } windowPutChar(window, 0x7f); while (!shouldQuit()) { _keyPressed.reset(); delay(1); if (_keyPressed.ascii == 0 || _keyPressed.ascii >= 127) continue; window->textColumn -= getFeebleFontSize(127); name[len] = 0; windowBackSpace(_windowArray[3]); if (_keyPressed.keycode == Common::KEYCODE_ESCAPE) { _variableArray[55] = 27; break; } if (_keyPressed.keycode == Common::KEYCODE_KP_ENTER || _keyPressed.keycode == Common::KEYCODE_RETURN) { if (!saveGame(readVariable(55), name)) _variableArray[55] = (int16)0xFFFF; else _variableArray[55] = 0; break; } if (_keyPressed.keycode == Common::KEYCODE_BACKSPACE && len != 0) { len--; byte chr = name[len]; window->textColumn -= getFeebleFontSize(chr); name[len] = 0; windowBackSpace(_windowArray[3]); } if (_keyPressed.ascii >= 32 && window->textColumn + 26 <= window->width) { name[len++] = _keyPressed.ascii; windowPutChar(_windowArray[3], _keyPressed.ascii); } windowPutChar(window, 0x7f); } }
void AGOSEngine::o_sub() { // 44: sub uint var = getVarWrapper(); writeVariable(var, readVariable(var) - getVarOrWord()); }
void AGOSEngine::o_addf() { // 45: add f uint var = getVarWrapper(); writeVariable(var, readVariable(var) + getNextVarContents()); }
void AGOSEngine::o_mul() { // 47: mul uint var = getVarWrapper(); writeVariable(var, readVariable(var) * getVarOrWord()); }
void AGOSEngine::o_subf() { // 46: sub f uint var = getVarWrapper(); writeVariable(var, readVariable(var) - getNextVarContents()); }
void AGOSEngine::o_mulf() { // 49: mul f uint var = getVarWrapper(); writeVariable(var, readVariable(var) * getNextVarContents()); }