/* ******************************************************************************************** */
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;
}
