void vrpn_Tracker_Crossbow::process_packet(const raw_packet &packet) { // Use the current time for a timestamp. vrpn_gettimeofday(×tamp, NULL); // Clear the position record. memset(pos, 0, sizeof(pos)); // Calculate the current orientation. (We don't know yaw, so report 0.) double pitch = convert_scalar(packet.pitch_angle, 180.0f) * DEGREES_TO_RADIANS; double roll = convert_scalar(packet.roll_angle, 180.0f) * DEGREES_TO_RADIANS; xb_quat_from_euler(d_quat, pitch, roll); // Clear the linear velocity; we don't know it. memset(vel, 0, sizeof(vel)); // Calculate the current angular velocity from yaw rate // It's in degrees per second, so convert to radians per second. q_from_euler(vel_quat, convert_scalar(packet.yaw_rate, 1.5f * ang_accel_range) * DEGREES_TO_RADIANS, 0, 0); vel_quat_dt = 1; // Calculate the current acceleration vector acc[0] = convert_scalar(packet.accel_x, 1.5f * lin_accel_range) * MPSS_PER_G; acc[1] = convert_scalar(packet.accel_y, 1.5f * lin_accel_range) * MPSS_PER_G; acc[2] = convert_scalar(packet.accel_z, 1.5f * lin_accel_range) * MPSS_PER_G; //printf("RawAccel = %hd\tG-Range = %f\tDerivedZ = %f\n", packet.accel_z, lin_accel_range, acc[2]); // The angular acceleration vector is nil. (0001 / 1) acc_quat[0] = acc_quat[1] = acc_quat[2] = 0; acc_quat[3] = acc_quat_dt = 1; }
void vrpn_Tracker_ViewPoint::get_tracker() { // Get information for each eye for (int i = 0; i < 2; i++) { // The sensor d_sensor = i; // Which eye? VPX_EyeType eye; if (d_sensor == 0) eye = EYE_A; else eye = EYE_B; // Get tracker data from the DLL VPX_RealPoint gp, cv, ga; if (useSmoothedData) { // Use smoothed data, when available VPX_GetGazePointSmoothed2(eye, &gp); VPX_GetComponentVelocity2(eye, &cv); // Always smoothed VPX_GetGazeAngleSmoothed2(eye, &ga); } else { // Use raw data VPX_GetGazePoint2(eye, &gp); VPX_GetComponentVelocity2(eye, &cv); // Always smoothed VPX_GetGazeAngle2(eye, &ga); } // Set the tracker position from the gaze point pos[0] = gp.x; pos[1] = gp.y; pos[2] = 0.0; // Set the tracker velocity from the eye velocity vel[0] = cv.x; vel[1] = cv.y; vel[2] = 0.0; // Convert the gaze angle to a quaternion q_from_euler(d_quat, 0.0, Q_DEG_TO_RAD(ga.y), Q_DEG_TO_RAD(ga.x)); // Send the data for this eye send_report(); } }
void vrpn_Tracker_OSVRHackerDevKit::on_data_received(std::size_t bytes, vrpn_uint8 *buffer) { if (bytes != 32 && bytes != 16) { send_text_message(vrpn_TEXT_WARNING) << "Received a report " << bytes << " in length, but expected it to be 32 or 16 bytes. Discarding. " "(May indicate issues with HID!)"; return; } vrpn_uint8 version = vrpn_unbuffer_from_little_endian<vrpn_uint8>(buffer); /// @todo Verify that version is what we expect. vrpn_uint8 msg_seq = vrpn_unbuffer_from_little_endian<vrpn_uint8>(buffer); // Signed, 16-bit, fixed-point numbers in Q1.14 format. typedef vrpn::FixedPoint<1, 14> FixedPointValue; d_quat[Q_X] = FixedPointValue(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer)) .get<vrpn_float64>(); d_quat[Q_Y] = FixedPointValue(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer)) .get<vrpn_float64>(); d_quat[Q_Z] = FixedPointValue(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer)) .get<vrpn_float64>(); d_quat[Q_W] = FixedPointValue(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer)) .get<vrpn_float64>(); vrpn_Tracker::timestamp = _timestamp; { char msgbuf[512]; int len = vrpn_Tracker::encode_to(msgbuf); if (d_connection->pack_message(len, _timestamp, position_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY)) { fprintf(stderr, "vrpn_Tracker_OSVRHackerDevKit: cannot write " "message: tossing\n"); } } if (version >= 2) { // We've got angular velocity in this message too // Signed Q6.9 typedef vrpn::FixedPoint<6, 9> VelFixedPoint; q_vec_type angVel; angVel[0] = VelFixedPoint(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer)) .get<vrpn_float64>(); angVel[1] = VelFixedPoint(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer)) .get<vrpn_float64>(); angVel[2] = VelFixedPoint(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer)) .get<vrpn_float64>(); // Given XYZ radians per second velocity. q_from_euler(vel_quat, angVel[2] * vel_quat_dt, angVel[1] * vel_quat_dt, angVel[0] * vel_quat_dt); char msgbuf[512]; int len = vrpn_Tracker::encode_vel_to(msgbuf); if (d_connection->pack_message(len, _timestamp, velocity_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY)) { fprintf(stderr, "vrpn_Tracker_OSVRHackerDevKit: cannot write " "message: tossing\n"); } } }
// This function will get the reports from the intersense dll, then // put that report into the time, sensor, pos and quat fields, and // finally call send_report to send it. void vrpn_Tracker_InterSense::get_report(void) { #ifdef VRPN_INCLUDE_INTERSENSE q_vec_type angles; ISD_TRACKING_DATA_TYPE data; int i; if(ISD_GetTrackingData(m_Handle,&data)) { for(int station=0;station<ISD_MAX_STATIONS;station++) { if(data.Station[station].NewData == TRUE) { d_sensor = station; //-------------------------------------------------------------------- // If we are doing IS900 timestamps, decode the time, add it to the // time we zeroed the tracker, and update the report time. Remember // to convert the MILLIseconds from the report into MICROseconds and // seconds. //-------------------------------------------------------------------- if (do_is900_timestamps) { vrpn_float32 read_time = data.Station[station].TimeStamp; struct timeval delta_time; // Time since the clock was reset // Convert from the float in MILLIseconds to the struct timeval delta_time.tv_sec = (long)(read_time / 1000); // Integer trunction to seconds read_time -= delta_time.tv_sec * 1000; // Subtract out what we just counted delta_time.tv_usec = (long)(read_time * 1000); // Convert remainder to MICROseconds // Store the current time timestamp = vrpn_TimevalSum(is900_zerotime, delta_time); } else { vrpn_gettimeofday(×tamp, NULL); // Set watchdog now } //-------------------------------------------------------------------- // If this sensor has an IS900 button on it, decode // the button values into the button device and mainloop the button // device so that it will report any changes. Each button is stored // in one bit of the byte, with the lowest-numbered button in the // lowest bit. //-------------------------------------------------------------------- if (is900_buttons[station]) { for (i = 0; i < is900_buttons[station]->number_of_buttons(); i++) { is900_buttons[station]->set_button(i, data.Station[station].ButtonState[i]); } is900_buttons[station]->mainloop(); } //-------------------------------------------------------------------- // If this sensor has an IS900 analog on it, decode the analog values // into the analog device and mainloop the analog device so that it // will report any changes. The first byte holds the unsigned char // representation of left/right. The second holds up/down. For each, // 0 means min (left or rear), 127 means center and 255 means max. //-------------------------------------------------------------------- if (is900_analogs[station]) { // Normalize the values to the range -1 to 1 is900_analogs[station]->setChannelValue(0, (data.Station[station].AnalogData[0] - 127) / 128.0); is900_analogs[station]->setChannelValue(1, (data.Station[station].AnalogData[1] - 127) / 128.0); // Report the new values is900_analogs[station]->report_changes(); is900_analogs[station]->mainloop(); } // Copy the tracker data into our internal storage before sending // (no unit problem as the Position vector is already in meters, see ISD_STATION_STATE_TYPE) // Watch: For some reason, to get consistent rotation and translation axis permutations, // we need non direct mapping. // RMT: Based on a report from Christian Odom, it seems like the Quaternions in the // Isense are QXYZ, whereas in Quatlib (and VRPN) they are XYZQ. Once these // are switched correctly, the positions can be read without strange swapping. pos[0] = data.Station[station].Position[0]; pos[1] = data.Station[station].Position[1]; pos[2] = data.Station[station].Position[2]; if(m_StationInfo[station].AngleFormat == ISD_QUATERNION) { d_quat[0] = data.Station[station].Quaternion[1]; d_quat[1] = data.Station[station].Quaternion[2]; d_quat[2] = data.Station[station].Quaternion[3]; d_quat[3] = data.Station[station].Quaternion[0]; } else { // Just return Euler for now... // nahon@virtools needs to convert to radians angles[0] = VRPN_DEGREES_TO_RADIANS*data.Station[station].Euler[0]; angles[1] = VRPN_DEGREES_TO_RADIANS*data.Station[station].Euler[1]; angles[2] = VRPN_DEGREES_TO_RADIANS*data.Station[station].Euler[2]; q_from_euler(d_quat, angles[0], angles[1], angles[2]); } // have to just send it now status = vrpn_TRACKER_REPORT_READY; // fprintf(stderr, "sending message len %d\n", len); send_report(); //printf("Isense %f, %f, %f\n",pos[0],pos[1],pos[2]); //printf("Isense a:%f, %f, %f : ",angles[0],angles[1],angles[2]); //if the tracker reports a quat, these will be garbage //printf("q: %f, %f, %f, %f\n",d_quat[0],d_quat[1],d_quat[2],d_quat[3]); } } } #endif }
void vrpn_IMU_SimpleCombiner::update_matrix_based_on_values(double time_interval) { //================================================================== // Adjust the orientation based on the rotational velocity, which is // measured in the rotated coordinate system. We need to rotate the // difference vector back to the canonical orientation, apply the // orientation change there, and then rotate back. // Be sure to scale by the time value. q_type forward, inverse; q_copy(forward, d_quat); q_invert(inverse, forward); // Remember that Euler angles in Quatlib have rotation around Z in // the first term. Compute the time-scaled delta transform in // canonical space. q_type delta; q_from_euler(delta, time_interval * d_rotational_vel.values[Q_Z], time_interval * d_rotational_vel.values[Q_Y], time_interval * d_rotational_vel.values[Q_X]); // Bring the delta back into canonical space q_type canonical; q_mult(canonical, delta, inverse); q_mult(canonical, forward, canonical); q_mult(d_quat, canonical, d_quat); //================================================================== // To the extent that the acceleration vector's magnitude is equal // to the expected gravity, rotate the orientation so that the vector // points downward. This is measured in the rotated coordinate system, // so we need to rotate back to canonical orientation and apply // the difference there and then rotate back. The rate of rotation // should be as specified in the gravity-rotation-rate parameter so // we don't swing the head around too quickly but only slowly re-adjust. double accel = sqrt( d_acceleration.values[0] * d_acceleration.values[0] + d_acceleration.values[1] * d_acceleration.values[1] + d_acceleration.values[2] * d_acceleration.values[2] ); double diff = fabs(accel - 9.80665); // Only adjust if we're close enough to the expected gravity // @todo In a more complicated IMU tracker, base this on state and // error estimates from a Kalman or other filter. double scale = 1.0 - diff; if (scale > 0) { // Get a new forward and inverse matrix from the current, just- // rotated matrix. q_copy(forward, d_quat); // Change how fast we adjust based on how close we are to the // expected value of gravity. Then further scale this by the // amount of time since the last estimate. double gravity_scale = scale * d_gravity_restore_rate * time_interval; // Rotate the gravity vector by the estimated transform. // We expect this direction to match the global down (-Y) vector. q_vec_type gravity_global; q_vec_set(gravity_global, d_acceleration.values[0], d_acceleration.values[1], d_acceleration.values[2]); q_vec_normalize(gravity_global, gravity_global); q_xform(gravity_global, forward, gravity_global); //printf(" XXX Gravity: %lg, %lg, %lg\n", gravity_global[0], gravity_global[1], gravity_global[2]); // Determine the rotation needed to take gravity and rotate // it into the direction of -Y. q_vec_type neg_y; q_vec_set(neg_y, 0, -1, 0); q_type rot; q_from_two_vecs(rot, gravity_global, neg_y); // Scale the rotation by the fraction of the orientation we // should remove based on the time that has passed, how well our // gravity vector matches expected, and the specified rate of // correction. static q_type identity = { 0, 0, 0, 1 }; q_type scaled_rot; q_slerp(scaled_rot, identity, rot, gravity_scale); //printf("XXX Scaling gravity vector by %lg\n", gravity_scale); // Rotate by this angle. q_mult(d_quat, scaled_rot, d_quat); //================================================================== // If we are getting compass data, and to the extent that the // acceleration vector's magnitude is equal to the expected gravity, // compute the cross product of the cross product to find the // direction of north perpendicular to down. This is measured in // the rotated coordinate system, so we need to rotate back to the // canonical orientation and do the comparison there. // The fraction of rotation should be as specified in the // magnetometer-rotation-rate parameter so we don't swing the head // around too quickly but only slowly re-adjust. if (d_magnetometer.ana != NULL) { // Get a new forward and inverse matrix from the current, just- // rotated matrix. q_copy(forward, d_quat); // Find the North vector that is perpendicular to gravity by // clearing its Y axis to zero and renormalizing. q_vec_type magnetometer; q_vec_set(magnetometer, d_magnetometer.values[0], d_magnetometer.values[1], d_magnetometer.values[2]); q_vec_type magnetometer_global; q_xform(magnetometer_global, forward, magnetometer); magnetometer_global[Q_Y] = 0; q_vec_type north_global; q_vec_normalize(north_global, magnetometer_global); //printf(" XXX north_global: %lg, %lg, %lg\n", north_global[0], north_global[1], north_global[2]); // Determine the rotation needed to take north and rotate it // into the direction of negative Z. q_vec_type neg_z; q_vec_set(neg_z, 0, 0, -1); q_from_two_vecs(rot, north_global, neg_z); // Change how fast we adjust based on how close we are to the // expected value of gravity. Then further scale this by the // amount of time since the last estimate. double north_rate = scale * d_north_restore_rate * time_interval; // Scale the rotation by the fraction of the orientation we // should remove based on the time that has passed, how well our // gravity vector matches expected, and the specified rate of // correction. static q_type identity = { 0, 0, 0, 1 }; q_slerp(scaled_rot, identity, rot, north_rate); // Rotate by this angle. q_mult(d_quat, scaled_rot, d_quat); } } //================================================================== // Compute and store the velocity information // This will be in the rotated coordinate system, so we need to // rotate back to the identity orientation before storing. // Convert from radians/second into a quaternion rotation as // rotated in a hundredth of a second and set the rotational // velocity dt to a hundredth of a second so that we don't // risk wrapping. // Remember that Euler angles in Quatlib have rotation around Z in // the first term. Compute the time-scaled delta transform in // canonical space. q_from_euler(delta, 1e-2 * d_rotational_vel.values[Q_Z], 1e-2 * d_rotational_vel.values[Q_Y], 1e-2 * d_rotational_vel.values[Q_X]); // Bring the delta back into canonical space q_mult(canonical, delta, inverse); q_mult(vel_quat, forward, canonical); vel_quat_dt = 1e-2; }
void vrpn_Tracker_AnalogFly::update_matrix_based_on_values (double time_interval) { double tx,ty,tz, rx,ry,rz; // Translation (m/s) and rotation (rad/sec) q_matrix_type diffM; // Difference (delta) matrix // For absolute trackers, the interval is treated as "1", so that the // translations and rotations are unscaled; if (d_absolute) { time_interval = 1.0; }; // compute the translation and rotation tx = d_x.value * time_interval; ty = d_y.value * time_interval; tz = d_z.value * time_interval; rx = d_sx.value * time_interval * (2*VRPN_PI); ry = d_sy.value * time_interval * (2*VRPN_PI); rz = d_sz.value * time_interval * (2*VRPN_PI); // Build a rotation matrix, then add in the translation q_euler_to_col_matrix(diffM, rz, ry, rx); diffM[3][0] = tx; diffM[3][1] = ty; diffM[3][2] = tz; // While the clutch is not engaged, we don't move. Record that // the clutch was off so that we know later when it is re-engaged. if (!d_clutch_engaged) { d_clutch_was_off = true; return; } // When the clutch becomes re-engaged, we store the current matrix // multiplied by the inverse of the present differential matrix so that // the first frame of the mouse-hold leaves us in the same location. // For the absolute matrix, this re-engages new motion at the previous // location. if (d_clutch_engaged && d_clutch_was_off) { d_clutch_was_off = false; q_type diff_orient; // This is backwards, because Euler angles have rotation about Z first... q_from_euler(diff_orient, rz, ry, rx); q_xyz_quat_type diff; q_vec_set(diff.xyz, tx, ty, tz); q_copy(diff.quat, diff_orient); q_xyz_quat_type diff_inverse; q_xyz_quat_invert(&diff_inverse, &diff); q_matrix_type di_matrix; q_to_col_matrix(di_matrix, diff_inverse.quat); di_matrix[3][0] = diff_inverse.xyz[0]; di_matrix[3][1] = diff_inverse.xyz[1]; di_matrix[3][2] = diff_inverse.xyz[2]; q_matrix_mult(d_clutchMatrix, di_matrix, d_currentMatrix); } // Apply the matrix. if (d_absolute) { // The difference matrix IS the current matrix. Catenate it // onto the clutch matrix. If there is no clutching happening, // this matrix will always be the identity so this will just // copy the difference matrix. q_matrix_mult(d_currentMatrix, diffM, d_clutchMatrix); } else { // Multiply the current matrix by the difference matrix to update // it to the current time. if (d_worldFrame) { // If using world frame: // 1. Separate out the translation and add to the differential translation tx += d_currentMatrix[3][0]; ty += d_currentMatrix[3][1]; tz += d_currentMatrix[3][2]; diffM[3][0] = 0; diffM[3][1] = 0; diffM[3][2] = 0; d_currentMatrix[3][0] = 0; d_currentMatrix[3][1] = 0; d_currentMatrix[3][2] = 0; // 2. Compose the rotations. q_matrix_mult(d_currentMatrix, d_currentMatrix, diffM); // 3. Put the new translation back in the matrix. d_currentMatrix[3][0] = tx; d_currentMatrix[3][1] = ty; d_currentMatrix[3][2] = tz; } else { q_matrix_mult(d_currentMatrix, diffM, d_currentMatrix); } } // Finally, convert the matrix into a pos/quat // and copy it into the tracker position and quaternion structures. convert_matrix_to_tracker(); }
void vrpn_Tracker_WiimoteHead::_update_2_LED_pose(q_xyz_quat_type & newPose) { if (d_points != 2) { // we simply stop updating our pos+orientation if we lost LED's // TODO: right now if we don't have exactly 2 points we lose the lock d_lock = false; d_flipState = FLIP_UNKNOWN; return; } // TODO right now only handling the 2-LED glasses d_lock = true; double rx, ry, rz; rx = ry = rz = 0; double X0, X1, Y0, Y1; X0 = d_vX[0]; X1 = d_vX[1]; Y0 = d_vY[0]; Y1 = d_vY[1]; if (d_flipState == FLIP_180) { /// If the first report of a new tracking lock indicated that /// our "up" vector had no positive y component, we have the /// points in the wrong order - flip them around. /// This uses the assumption that the first time we see the glasses, /// they ought to be right-side up (a reasonable assumption for /// head tracking) std::swap(X0, X1); std::swap(Y0, Y1); } const double dx = X0 - X1; const double dy = Y0 - Y1; const double dist = sqrt(dx * dx + dy * dy); const double angle = dist * cvtDistToAngle; // Note that this is an approximation, since we don't know the // distance/horizontal position. (I think...) const double headDist = (d_blobDistance / 2.0) / tan(angle); // Translate the distance along z axis, and tilt the head newPose.xyz[2] = headDist; // translate along Z rz = atan2(dy, dx); // rotate around Z // Find the sensor pixel of the line of sight - directly between // the LEDs const double avgX = (X0 + X1) / 2.0; const double avgY = (Y0 + Y1) / 2.0; /// @todo For some unnerving reason, in release builds, avgX tends to become NaN/undefined /// However, any kind of inspection (such as the following, or even a simple cout) /// appears to prevent the issue. This makes me uneasy, but I won't argue with /// what is working. if (wm_isnan(avgX)) { std::cerr << "NaN detected in avgX: X0 = " << X0 << ", X1 = " << X1 << std::endl; return; } if (wm_isnan(avgY)) { std::cerr << "NaN detected in avgY: Y0 = " << Y0 << ", Y1 = " << Y1 << std::endl; return; } // b is the virtual depth in the sensor from a point to the full sensor // used for finding similar triangles to calculate x/y translation const double bHoriz = xResSensor / 2 / tan(fovX / 2); const double bVert = yResSensor / 2 / tan(fovY / 2); // World head displacement (X and Y) from a centered origin at // the calculated distance from the sensor newPose.xyz[0] = headDist * (avgX - xResSensor / 2) / bHoriz; newPose.xyz[1] = headDist * (avgY - yResSensor / 2) / bVert; // set the quat. part of our pose with rotation angles q_from_euler(newPose.quat, rz, ry, rx); // Apply the new pose q_vec_copy(d_currentPose.xyz, newPose.xyz); q_copy(d_currentPose.quat, newPose.quat); }