/* ******************************************************************************************** */
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);
}
/* ******************************************************************************************** */
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;
}
int display( struct aa_rx_win *win, void *cx_, struct aa_sdl_display_params *params )
{
(void)win;
struct display_cx *cx = (struct display_cx *)cx_;
const struct timespec *now = aa_sdl_display_params_get_time_now(params);
const struct timespec *last = aa_sdl_display_params_get_time_last(params);
const struct aa_rx_sg *scenegraph = cx->scenegraph;
size_t n = aa_rx_sg_frame_count(scenegraph);
size_t m = aa_rx_sg_config_count(scenegraph);
double q[m];
AA_MEM_ZERO(q,m);
int first = aa_sdl_display_params_is_first(params);
if( ! first ) {
double dt = aa_tm_timespec2sec( aa_tm_sub(*now, *last) );
cx->q += dt * 45 * (M_PI/180);
}
q[ cx->i_q ] = cx->q;
double TF_rel[7*n];
double TF_abs[7*n];
aa_rx_sg_tf(scenegraph, m, q,
n,
TF_rel, 7,
TF_abs, 7 );
aa_rx_win_display_sg_tf( cx->win, params, scenegraph,
n, TF_abs, 7 );
int col = aa_rx_cl_check( cx->cl, n, TF_abs, 7, NULL );
printf("in collision: %s\n",
col ? "yes" : "no" );
return 0;
}
