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