Exemple #1
0
/* ******************************************************************************************** */
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);
}
Exemple #2
0
//
// 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();
    }
Exemple #4
0
/* ******************************************************************************************** */
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;
    }
Exemple #6
0
//
// 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;
}
Exemple #7
0
//
// 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();
   }
}
Exemple #8
0
//
// 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());
   }
}
Exemple #9
0
std::string Hdf::getLastValue() const {
  HDF *hdf = getRaw();
  return hdf_get_last_value(hdf);
}
Exemple #10
0
//////////////////////////////////////////////////////////
// last string info
std::string Hdf::getLastFullName() const {
  HDF *hdf = getRaw();
  return hdf_get_last_fullname(hdf);
}
Exemple #11
0
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);
}
Exemple #12
0
/*!
    \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().
Exemple #13
0
int GFFFile::getRaw(std::string label, Struct &sct, MemSeg &memseg)
{
	memseg.mem = getRaw(label, sct, memseg.size);
	return errcode;
}
Exemple #14
0
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());
}
Exemple #16
0
void Hdf::fromString(const char *input) {
  CheckNeoError(hdf_read_string(getRaw(), (char*)input));
}
Exemple #17
0
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 : "";
}
Exemple #18
0
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());
}
Exemple #20
0
void Hdf::append(const char *filename) {
  assert(filename && *filename);
  CheckNeoError(hdf_read_file(getRaw(), (char*)filename));
}
Exemple #21
0
void Hdf::copy(const Hdf &hdf) {
  CheckNeoError(hdf_copy(getRaw(), nullptr, hdf.getRaw()));
}
bool                
DistinctProjection::getBool()const
{
    return Convert::convertToBool( getRaw());
}
Exemple #23
0
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);
  }
}
Exemple #24
0
//---------------------------------------------------------------------
// 各種アクセサ(生情報からデータを切り出す)
//---------------------------------------------------------------------
std::string kjm::eventlog_record::SourceName() {
	char* cp = (char *)getRaw();
	cp += sizeof(EVENTLOGRECORD);

	return std::string( cp );
}
Exemple #25
0
void Hdf::set(const char *value) {
  CheckNeoError(hdf_set_value(getRaw(), nullptr, (char*)value));
}
Exemple #26
0
//
// SourceTokenizerC::peekRaw
//
SourceTokenC::Reference SourceTokenizerC::peekRaw()
{
   SourceTokenC::Reference tok = getRaw();
   ungetStack.push_back(tok);
   return tok;
}
Exemple #27
0
void Hdf::remove(const char *name) const {
  assert(name && *name);
  CheckNeoError(hdf_remove_tree(getRaw(), name));
}
Exemple #28
0
uint8 *GFFFile::getRaw(std::string label, Struct &sct, uint32 &size)
{
	return getRaw(getIdxByLabel(label, sct), sct, size);
}
Exemple #29
0
void Hdf::write(const char *filename) const {
  CheckNeoError(hdf_write_file(getRaw(), filename));
}
Exemple #30
0
//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;
}