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