/* ******************************************************************************************** */ void FT::updateExternal () { // Attempt to get the current reading. If failed, return the last value Vector6d raw; if(!getRaw(raw)) return; // Supplement raw with the offset Vector6d ideal = raw + offset; // Otherwise, compute the external reading as the difference between expected reading due to // gripper weight and the current ideal reading, in the world frame error(ideal, lastExternal, true); }
// // SourceTokenizerC::doCommand_define // void SourceTokenizerC::doCommand_define(SourceTokenC *) { SourceTokenC::Reference name = getRaw(); doAssert(name, SourceTokenC::TT_NAM); if (inStack.back()->peek() == '(') { MacroData data; doAssert(getRaw(), SourceTokenC::TT_PAREN_O); SourceTokenC::Reference arg = getRaw(); if (arg->type != SourceTokenC::TT_PAREN_C) for (;;) { if (arg->type == SourceTokenC::TT_DOT3) { data.first.push_back(std::string()); doAssert((arg = getRaw()), SourceTokenC::TT_PAREN_C); break; } doAssert(arg, SourceTokenC::TT_NAM); data.first.push_back(arg->data); if ((arg = getRaw())->type == SourceTokenC::TT_PAREN_C) break; doAssert(arg, SourceTokenC::TT_COMMA); arg = getRaw(); } inStack.back()->disableQuote(); for (char c; (c = inStack.back()->get()) != '\n';) data.second += c; inStack.back()->enableQuote(); inStack.back()->unget('\n'); if (isSkip()) return; addMacro(name->data, data); } else { std::string data; inStack.back()->disableQuote(); for (char c; (c = inStack.back()->get()) != '\n';) data += c; inStack.back()->enableQuote(); inStack.back()->unget('\n'); if (isSkip()) return; addDefine(name->data, data); } }
boost::optional<Document> DocumentSourceProject::getNext() { pExpCtx->checkForInterrupt(); boost::optional<Document> input = pSource->getNext(); if (!input) return boost::none; /* create the result document */ const size_t sizeHint = pEO->getSizeHint(); MutableDocument out (sizeHint); out.copyMetaDataFrom(*input); /* Use the ExpressionObject to create the base result. If we're excluding fields at the top level, leave out the _id if it is found, because we took care of it above. */ _variables->setRoot(*input); pEO->addToDocument(out, *input, _variables.get()); _variables->clearRoot(); #if defined(_DEBUG) if (!_simpleProjection.getSpec().isEmpty()) { // Make sure we return the same results as Projection class BSONObj inputBson = input->toBson(); BSONObj outputBson = out.peek().toBson(); BSONObj projected = _simpleProjection.transform(inputBson); if (projected != outputBson) { log() << "$project applied incorrectly: " << getRaw() << endl; log() << "input: " << inputBson << endl; log() << "out: " << outputBson << endl; log() << "projected: " << projected << endl; verify(false); // exits in _DEBUG builds } } #endif return out.freeze(); }
/* ******************************************************************************************** */ FT::FT (GripperType type, somatic_d_t* daemon_cx_, dynamics::SkeletonDynamics* r_, Side side_) { // Sanity check the input and set the local bariables assert((r_ != NULL) && "Can not initialize the f/t sensors without robot kinematics"); robot = r_; daemon_cx = daemon_cx_; side = side_; // Open the data channel chan = new ach_channel_t(); somatic_d_channel_open(daemon_cx, chan, (side == LEFT) ? "llwa_ft" : "rlwa_ft", NULL); // Set the gripper mass and center of mass based on the type: the com for ext, schunk and robotiq // are 0.026, 0.0683, 0.065 (?) gripperMass = 0.169; //< f/t extension mass (will be there regardless of type) if(type == GRIPPER_TYPE_ROBOTIQ) { gripperMass += 2.3; gripperCoM = Vector3d(0.0, 0.0, 0.09); } else if (type == GRIPPER_TYPE_SCHUNK) { gripperMass += 1.6; gripperCoM = Vector3d(0.0, -0.000, 0.091); } else gripperCoM = Vector3d(0.0, -0.000, 0.013); // Average one second's worth of FT readings and average to get a good initial reading double time_ft_av_start = aa_tm_timespec2sec(aa_tm_now()); int num_data = 0; Vector6d data = Vector6d::Zero(), temp; while((num_data < 100) || (aa_tm_timespec2sec(aa_tm_now()) - time_ft_av_start < 1.0)) { num_data++; bool gotReading = false; while(!gotReading) gotReading = getRaw(temp); data += temp; } data /= (double)num_data; // Compute the offset between what the reading should be and what it is assuming no external // forces error(data, offset, false); }
intrusive_ptr<Document> DocumentSourceProject::getCurrent() { intrusive_ptr<Document> pInDocument(pSource->getCurrent()); /* create the result document */ const size_t sizeHint = pEO->getSizeHint(); intrusive_ptr<Document> pResultDocument(Document::create(sizeHint)); /* Use the ExpressionObject to create the base result. If we're excluding fields at the top level, leave out the _id if it is found, because we took care of it above. */ pEO->addToDocument(pResultDocument, pInDocument, /*root=*/pInDocument); #if defined(_DEBUG) if (!_simpleProjection.getSpec().isEmpty()) { // Make sure we return the same results as Projection class BSONObjBuilder inputBuilder; pSource->getCurrent()->toBson(&inputBuilder); BSONObj input = inputBuilder.done(); BSONObjBuilder outputBuilder; pResultDocument->toBson(&outputBuilder); BSONObj output = outputBuilder.done(); BSONObj projected = _simpleProjection.transform(input); if (projected != output) { log() << "$project applied incorrectly: " << getRaw() << endl; log() << "input: " << input << endl; log() << "out: " << output << endl; log() << "projected: " << projected << endl; verify(false); // exits in _DEBUG builds } } #endif return pResultDocument; }
// // SourceTokenizerC::getExpand // SourceTokenC::Reference SourceTokenizerC::getExpand() { SourceTokenC::Reference tok = getRaw(); if (canExpand && tok->type == SourceTokenC::TT_NAM) { if (hasMacro(tok->data)) { SourceTokenC::Reference tmpTok = getExpand(); // Macro invocation! if (tmpTok->type == SourceTokenC::TT_PAREN_O) { expandMacro(tok); return getExpand(); } unget(tmpTok); } if (hasDefine(tok->data)) { expandDefine(tok); return getExpand(); } if (tok->data == "__FILE__") return SourceTokenC::create(tok->pos, tok->pos.filename, SourceTokenC::TT_STR); if (tok->data == "__LINE__") { std::ostringstream oss; oss << tok->pos.line; return SourceTokenC::create(tok->pos, oss.str(), SourceTokenC::TT_INT); } } return tok; }
// // SourceTokenizerC::getRaw // SourceTokenC::Reference SourceTokenizerC::getRaw() { if (!ungetStack.empty()) { canExpand = false; SourceTokenC::Reference tok = ungetStack.back(); ungetStack.pop_back(); return tok; } else try { canExpand = true; SourceTokenC::Reference tok(new SourceTokenC); tok->readToken(inStack.back()); return tok; } catch (SourceStream::EndOfStream &e) { if (inStack.size() == 1) throw; delete inStack.back(); inStack.pop_back(); return getRaw(); } }
// // SourceTokenizerC::doCommand_include // void SourceTokenizerC::doCommand_include(SourceTokenC *) { SourceTokenC::Reference inc = getRaw(); std::string filename; unsigned flags = SourceStream::ST_C; if(inc->type == SourceTokenC::TT_STR) filename = inc->data; else if(inc->type == SourceTokenC::TT_CMP_LT) { flags |= SourceStream::STF_NOUSER; for(char c; (c = inStack.back()->get()) != '>';) { if(c == '\n') Error(inc->pos, "unterminated include"); filename += c; } } else Error(inc->pos, "expected TT_STR or TT_CMP_LT"); doAssert(peekRaw(), SourceTokenC::TT_ENDL); if (isSkip()) return; try { inStack.push_back(new SourceStream(filename, flags)); ungetStack.push_back(static_cast<SourceTokenC::Reference>( new SourceTokenC(SourcePosition::builtin(), SourceTokenC::TT_ENDL))); } catch (std::exception const &) { Error(inc->pos, "file not found: %s", filename.c_str()); } }
std::string Hdf::getLastValue() const { HDF *hdf = getRaw(); return hdf_get_last_value(hdf); }
////////////////////////////////////////////////////////// // last string info std::string Hdf::getLastFullName() const { HDF *hdf = getRaw(); return hdf_get_last_fullname(hdf); }
void Light_D1::getBroadbandIr (uint16_t *broadband, uint16_t *ir) { bool valid = false; /* If Auto gain disabled get a single reading and continue */ if(!autoGain) { getRaw (broadband, ir); return; } /* Read data until we find a valid range */ bool _agcCheck = false; do { uint16_t _b, _ir; uint16_t _hi, _lo; tsl2561IntegrationTime_t _it = integrationTime; /* Get the hi/low threshold for the current integration time */ switch(_it) { case TSL2561_INTEGRATIONTIME_13MS: _hi = TSL2561_AGC_THI_13MS; _lo = TSL2561_AGC_TLO_13MS; break; case TSL2561_INTEGRATIONTIME_101MS: _hi = TSL2561_AGC_THI_101MS; _lo = TSL2561_AGC_TLO_101MS; break; default: _hi = TSL2561_AGC_THI_402MS; _lo = TSL2561_AGC_TLO_402MS; break; } getRaw(&_b, &_ir); /* Run an auto-gain check if we haven't already done so ... */ if (!_agcCheck) { if ((_b < _lo) && (gain == TSL2561_GAIN_1X)) { /* Increase the gain and try again */ setGain(TSL2561_GAIN_16X); /* Drop the previous conversion results */ getRaw(&_b, &_ir); /* Set a flag to indicate we've adjusted the gain */ _agcCheck = true; } else if ((_b > _hi) && (gain == TSL2561_GAIN_16X)) { /* Drop gain to 1x and try again */ setGain(TSL2561_GAIN_1X); /* Drop the previous conversion results */ getRaw(&_b, &_ir); /* Set a flag to indicate we've adjusted the gain */ _agcCheck = true; } else { /* Nothing to look at here, keep moving .... Reading is either valid, or we're already at the chips limits */ *broadband = _b; *ir = _ir; valid = true; } } else { /* If we've already adjusted the gain once, just return the new results. This avoids endless loops where a value is at one extreme pre-gain, and the the other extreme post-gain */ *broadband = _b; *ir = _ir; valid = true; } } while (!valid); }
/*! \brief Calibrate using the self test operation. Average the values using bias mode to obtain the scale factors. \param gain [in] Gain setting for the sensor. See data sheet. \param n_samples [in] Number of samples to average together while applying the positive and negative bias. \return Returns false if any of the following occurs: # Invalid input parameters. (gain>7 or n_samples=0). # Id registers are wrong for the compiled device. Unfortunately, we can't distinguish between HMC5843 and HMC5883L. # Calibration saturates during the positive or negative bias on any of the readings. # Readings are outside of the expected range for bias current. */ bool HMC58X3::calibrate(unsigned char gain, unsigned int n_samples) { int16_t xyz[3]; // 16 bit integer values for each axis. int32_t xyz_total[3]={0,0,0}; // 32 bit totals so they won't overflow. bool bret=true; // Function return value. Will return false if the wrong identifier is returned, saturation is detected or response is out of range to self test bias. char id[3]; // Three identification registers should return 'H43'. int32_t low_limit, high_limit; /* Make sure we are talking to the correct device. Hard to believe Honeywell didn't change the identifier. */ if ((8>gain) && (0<n_samples)) // Notice this allows gain setting of 7 which the data sheet warns against. { getID(id); if (('H' == id[0]) && ('4' == id[1]) && ('3' == id[2])) { /* Use the positive bias current to impose a known field on each axis. This field depends on the device and the axis. */ writeReg(HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR=0x010 + MS1,MS0 set to pos bias /* Note that the very first measurement after a gain change maintains the same gain as the previous setting. The new gain setting is effective from the second measurement and on. */ setGain(gain); setMode(1); // Change to single measurement mode. getRaw(&xyz[0],&xyz[1],&xyz[2]); // Get the raw values and ignore since this reading may use previous gain. for (unsigned int i=0; i<n_samples; i++) { setMode(1); getRaw(&xyz[0],&xyz[1],&xyz[2]); // Get the raw values in case the scales have already been changed. UARTprintf("%d %d %d \n", xyz[0], xyz[1], xyz[2]); /* Since the measurements are noisy, they should be averaged rather than taking the max. */ xyz_total[0]+=xyz[0]; xyz_total[1]+=xyz[1]; xyz_total[2]+=xyz[2]; /* Detect saturation. */ if (-(1<<12) >= min(xyz[0],min(xyz[1],xyz[2]))) { DEBUG_PRINT("HMC58x3 Self test saturated. Increase range."); bret=false; break; // Breaks out of the for loop. No sense in continuing if we saturated. } } /* Apply the negative bias. (Same gain) */ UARTprintf("%d %d %d \n", xyz_total[0], xyz_total[1], xyz_total[2]); writeReg(HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR=0x010 + MS1,MS0 set to negative bias. for (unsigned int i=0; i<n_samples; i++) { setMode(1); getRaw(&xyz[0],&xyz[1],&xyz[2]); // Get the raw values in case the scales have already been changed. UARTprintf("%d %d %d \n", xyz[0], xyz[1], xyz[2]); /* Since the measurements are noisy, they should be averaged. */ xyz_total[0]-=xyz[0]; xyz_total[1]-=xyz[1]; xyz_total[2]-=xyz[2]; /* Detect saturation. */ if (-(1<<12) >= min(xyz[0],min(xyz[1],xyz[2]))) { DEBUG_PRINT("HMC58x3 Self test saturated. Increase range."); bret=false; break; // Breaks out of the for loop. No sense in continuing if we saturated. } } /* Compare the values against the expected self test bias gauss. Notice, the same limits are applied to all axis. */ UARTprintf("%d %d %d \n", xyz_total[0], xyz_total[1], xyz_total[2]); low_limit =SELF_TEST_LOW_LIMIT *counts_per_milligauss[gain]*2*n_samples; high_limit=SELF_TEST_HIGH_LIMIT*counts_per_milligauss[gain]*2*n_samples; UARTprintf("lowl=%d highl=%d\n", low_limit, high_limit); if ((true==bret) && (low_limit <= xyz_total[0]) && (high_limit >= xyz_total[0]) && (low_limit <= xyz_total[1]) && (high_limit >= xyz_total[1]) && (low_limit <= xyz_total[2]) && (high_limit >= xyz_total[2]) ) { /* Successful calibration. Normalize the scale factors so all axis return the same range of values for the bias field. Factor of 2 is from summation of total of n_samples from both positive and negative bias. */ x_scale=(counts_per_milligauss[gain]*(HMC58X3_X_SELF_TEST_GAUSS*2))/(xyz_total[0]/n_samples); y_scale=(counts_per_milligauss[gain]*(HMC58X3_Y_SELF_TEST_GAUSS*2))/(xyz_total[1]/n_samples); z_scale=(counts_per_milligauss[gain]*(HMC58X3_Z_SELF_TEST_GAUSS*2))/(xyz_total[2]/n_samples); }else { DEBUG_PRINT("HMC58x3 Self test out of range."); bret=false; } writeReg(HMC58X3_R_CONFA, 0x010); // set RegA/DOR back to default. }else { #if defined(ISHMC5843) DEBUG_PRINT("HMC5843 failed id check."); #else DEBUG_PRINT("HMC5883L failed id check."); #endif bret=false; } }else { /* Bad input parameters. */ DEBUG_PRINT("HMC58x3 Bad parameters."); bret=false; } return(bret); } // calibrate().
int GFFFile::getRaw(std::string label, Struct &sct, MemSeg &memseg) { memseg.mem = getRaw(label, sct, memseg.size); return errcode; }
int GFFFile::getRaw(uint32 n, Struct &sct, MemSeg &memseg) { memseg.mem = getRaw(n, sct, memseg.size); return errcode; }
long long DistinctProjection::getLongLong()const { return Convert::convertToLongLong( getRaw()); }
void Hdf::fromString(const char *input) { CheckNeoError(hdf_read_string(getRaw(), (char*)input)); }
std::string Hdf::getName(bool markVisited /* = true */) const { HDF *hdf = getRaw(); char *name = hdf_obj_name(hdf); if (markVisited) hdf_set_visited(hdf, 1); return name ? name : ""; }
const char *Hdf::configGet(const char *defValue /* = NULL */) const { HDF *hdf = getRaw(); const char *v = hdf_obj_value(hdf); hdf_set_visited(hdf, 1); return v ? v : defValue; }
double DistinctProjection::getDouble()const { return Convert::convertToDouble( getRaw()); }
void Hdf::append(const char *filename) { assert(filename && *filename); CheckNeoError(hdf_read_file(getRaw(), (char*)filename)); }
void Hdf::copy(const Hdf &hdf) { CheckNeoError(hdf_copy(getRaw(), nullptr, hdf.getRaw())); }
bool DistinctProjection::getBool()const { return Convert::convertToBool( getRaw()); }
void Hdf::setVisited(bool visited /* = true */) { hdf_set_visited(getRaw(), visited ? 1 : 0); for (Hdf hdf = firstChild(false); hdf.exists(); hdf = hdf.next(false)) { hdf.setVisited(visited); } }
//--------------------------------------------------------------------- // 各種アクセサ(生情報からデータを切り出す) //--------------------------------------------------------------------- std::string kjm::eventlog_record::SourceName() { char* cp = (char *)getRaw(); cp += sizeof(EVENTLOGRECORD); return std::string( cp ); }
void Hdf::set(const char *value) { CheckNeoError(hdf_set_value(getRaw(), nullptr, (char*)value)); }
// // SourceTokenizerC::peekRaw // SourceTokenC::Reference SourceTokenizerC::peekRaw() { SourceTokenC::Reference tok = getRaw(); ungetStack.push_back(tok); return tok; }
void Hdf::remove(const char *name) const { assert(name && *name); CheckNeoError(hdf_remove_tree(getRaw(), name)); }
uint8 *GFFFile::getRaw(std::string label, Struct &sct, uint32 &size) { return getRaw(getIdxByLabel(label, sct), sct, size); }
void Hdf::write(const char *filename) const { CheckNeoError(hdf_write_file(getRaw(), filename)); }
//get the action target point in the images reference frame bool VisuoThread::getTarget(Value &type, Bottle &options) { bool ok=false; Bottle &bNewTarget=options.addList(); bNewTarget.addString("target"); Bottle &bTarget=bNewTarget.addList(); Bottle &bNewStereo=bTarget.addList(); bNewStereo.addString("stereo"); Bottle &bStereo=bNewStereo.addList(); if(type.isList()) { Bottle *list=type.asList(); if(list->size()>2) { if(list->get(0).asString()=="right") { bStereo.addDouble(0.0); bStereo.addDouble(0.0); bStereo.addDouble(list->get(1).asDouble()); bStereo.addDouble(list->get(2).asDouble()); } else if(list->get(0).asString()=="left") { bStereo.addDouble(list->get(1).asDouble()); bStereo.addDouble(list->get(2).asDouble()); bStereo.addDouble(0.0); bStereo.addDouble(0.0); } else if(list->get(0).asString()=="cartesian") { Bottle &bNewCartesian=bTarget.addList(); bNewCartesian.addString("cartesian"); Bottle &bCartesian=bNewCartesian.addList(); for(int i=1; i<list->size(); i++) bCartesian.addDouble(list->get(i).asDouble()); } else { Bottle &bNewCartesian=bTarget.addList(); bNewCartesian.addString("cartesian"); Bottle &bCartesian=bNewCartesian.addList(); for(int i=0; i<list->size(); i++) bCartesian.addDouble(list->get(i).asDouble()); } ok=true; } else if(list->size()==2) { bStereo.addDouble(list->get(0).asDouble()); bStereo.addDouble(list->get(1).asDouble()); bStereo.addDouble(0.0); bStereo.addDouble(0.0); ok=true; } } else switch(type.asVocab()) { case PARAM_FIXATION: { ok=getFixation(bStereo); break; } case PARAM_MOTION: { ok=getMotion(bStereo); break; } case PARAM_TRACK: { ok=getTrack(bStereo); break; } case PARAM_RAW: { ok=getRaw(bStereo); break; } default: { Bottle &bName=bTarget.addList(); bName.addString("name"); bName.addString(type.asString().c_str()); getObject(type.asString().c_str(),bStereo); break; } } return ok; }