/* ******************************************************************************************** */
Eigen::VectorXd SpaceNav::updateSpaceNav() {
// grab the current time
double time_now = aa_tm_timespec2sec(aa_tm_now());
// try to get a raw input
Eigen::VectorXd spacenav_input_raw;
bool got_new_input;
got_new_input = this->getSpaceNavRaw(spacenav_input_raw);
// if we get a raw input got_new_inputfully, cache it.
if (got_new_input) {
this->last_spacenav_input = spacenav_input_raw;
this->time_last_input = aa_tm_timespec2sec(aa_tm_now());
}
// if we have a recently cached value, including one that we
// just got in this call, use the cached value instead
if (time_now - this->time_last_input < this->cache_timeout) {
return this->last_spacenav_input;
}
// if our cached value is too old, then the system has become
// unsafe and we instead return a zero input
return Eigen::VectorXd::Zero(6);
}
AA_API void
aa_tick(const char fmt[], ...) {
va_list argp;
va_start( argp, fmt );
vfprintf( stderr, fmt, argp );
va_end( argp );
aa_tick_tock_start = aa_tm_now();
}
/* ******************************************************************************************** */
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);
}
/* ******************************************************************************************** */
SpaceNav::SpaceNav (somatic_d_t* _daemon_cx, char* _chan_name, double _cache_timeout) {
// grab variables
this->daemon_cx = _daemon_cx;
this->cache_timeout = _cache_timeout;
// open our ach channel
somatic_d_channel_open(this->daemon_cx, &this->spacenav_chan, _chan_name, NULL);
// initialize the variables we use for caching state and
// failing safely
time_last_input = aa_tm_timespec2sec(aa_tm_now());
last_spacenav_input = Eigen::VectorXd::Zero(6);
buttons[0] = buttons[1] = 0;
}
AA_API struct timespec
aa_tock(void) {
struct timespec t = aa_tm_sub( aa_tm_now(), aa_tick_tock_start );
fprintf( stderr, "%.6f ms\n", (double)t.tv_sec*1e3 + (double)t.tv_nsec / 1e6 );
return t;
}
