void NeuronPlugin::InputDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData, const std::vector<NeuronPlugin::NeuronJoint>& joints, const std::vector<NeuronPlugin::NeuronJoint>& prevJoints) { for (size_t i = 0; i < joints.size(); i++) { int poseIndex = neuronJointIndexToPoseIndex((NeuronJointIndex)i); glm::vec3 linearVel, angularVel; const glm::vec3& pos = joints[i].pos; const glm::vec3& rotEuler = joints[i].euler; if (Vectors::ZERO == pos && Vectors::ZERO == rotEuler) { _poseStateMap[poseIndex] = controller::Pose(); continue; } glm::quat rot = eulerToQuat(rotEuler); if (i < prevJoints.size()) { linearVel = (pos - (prevJoints[i].pos * METERS_PER_CENTIMETER)) / deltaTime; // m/s // quat log imaginary part points along the axis of rotation, with length of one half the angle of rotation. glm::quat d = glm::log(rot * glm::inverse(eulerToQuat(prevJoints[i].euler))); angularVel = glm::vec3(d.x, d.y, d.z) / (0.5f * deltaTime); // radians/s } _poseStateMap[poseIndex] = controller::Pose(pos, rot, linearVel, angularVel); } _poseStateMap[controller::RIGHT_HAND_THUMB1] = controller::Pose(rightHandThumb1DefaultAbsTranslation, glm::quat(), glm::vec3(), glm::vec3()); _poseStateMap[controller::LEFT_HAND_THUMB1] = controller::Pose(leftHandThumb1DefaultAbsTranslation, glm::quat(), glm::vec3(), glm::vec3()); }
void test_qvelToQdot() { //constants const int nf = 5; const int ns = NUMSTATE(nf); const int nv = NUMQVEL(nf); Real statedot[ns]; Real qvel[nv],qvel_cvtback[nv]; VecEuler euler = {DEGTORAD(10),DEGTORAD(20),DEGTORAD(30)}; VecOrient orient; Mat3 R_body_to_world; if (WMRSIM_USE_QUATERNION) eulerToQuat(euler,orient); else copyEuler(euler,orient); orientToRot(orient,R_body_to_world); for (int i=0; i<nv; i++) { qvel[i] = i; } qvelToQdot(nf,qvel,orient,R_body_to_world,statedot); qdotToQvel(nf,statedot,orient,R_body_to_world,qvel_cvtback); //convert back to check //PRINT std::cout << "orient =\n"; printOrient(orient,-1,-1); std::cout << std::endl; std::cout << "R_body_to_world =\n"; printMat3(R_body_to_world,-1,-1); std::cout << std::endl; std::cout << "qvel =\n"; printMatReal(nv,1,qvel,-1,-1); std::cout << std::endl; std::cout << "statedot =\n"; printMatReal(ns,1,statedot,-1,-1); std::cout << std::endl; std::cout << "qvel (converted back) =\n"; printMatReal(nv,1,qvel_cvtback,-1,-1); std::cout << std::endl; }
myQuat_t getPosition(mpu9250_t dev) { imuData_t imuData; if (getIMUData(dev, &imuData)) { if (! validIMUData(imuData)) { puts("invalid IMU data"); if (failureCount++ > 20) { puts("Error in IMU, restarting!"); identifyYourself("IMU failure"); if (! initialiseIMU(&dev)) { puts("Could not initialise IMU! dying..."); exit(1); } } return currentQ; } failureCount = 0; /************************************************************************ * Get Gyro data that records current angular velocity and create an * estimated quaternion representation of orientation using this * velocity, the time interval between the last calculation and the * previous orientation: * change in position = velocity * time, * new position = old position + change in position) ************************************************************************/ // get orientation as deduced by adding gyro measured rotational // velocity (times time interval) to previous orientation. double gx1 = (double)(imuData.gyro.x_axis); double gy1 = (double)(imuData.gyro.y_axis); double gz1 = (double)(imuData.gyro.z_axis); double omega[3] = { gx1, gy1, gz1 }; // this will be in degrees/sec //double omegaMagnitude = fabs(vecLength(omega)); uint32_t dt = imuData.ts - lastIMUData.ts; // dt in microseconds lastIMUData = imuData; myQuat_t gRot = makeQuatFromAngularVelocityTime(omega, dt/1000000.0); myQuat_t gyroDeducedQ = quatMultiply(currentQ, gRot); quatNormalise(&gyroDeducedQ); if (! useGyroscopes) { makeIdentityQuat(&gyroDeducedQ); } if (! (useAccelerometers | useMagnetometers)) { // return now with just the gyro data currentQ = gyroDeducedQ; lastIMUData = imuData; return currentQ; } // get orientation from the gyro as Roll Pitch Yaw (ie Euler angles) double gyroDeducedYPR[3]; quatToEuler(gyroDeducedQ, gyroDeducedYPR); double currentYPR[3]; quatToEuler(currentQ, currentYPR); // Calculate a roll/pich/yaw from the accelerometers and magnetometers. /************************************************************************ * Get Accelerometer data that records gravity direction and create a * 'measured' Quaternion representation of orientation (will only be * pitch and roll, not yaw) ************************************************************************/ // get gravity direction from accelerometer double ax1 = imuData.accel.x_axis / 1024.0; double ay1 = imuData.accel.y_axis / 1024.0; double az1 = imuData.accel.z_axis / 1024.0; double downSensor[3] = { ax1, ay1, az1 }; if (fabs(vecLength(downSensor) - 0.98) > 0.5) { // excessive accelerometer reading, bail out //printf("too much accel: %f ", fabs(vecLength(downSensor))); //dumpVec(downSensor); currentQ = gyroDeducedQ; lastIMUData = imuData; return currentQ; } vecNormalise(downSensor); // The accelerometers can only measure pitch and roll (in the world reference // frame), calculate the pitch and roll values to account for // accelerometer readings, make yaw = 0. double ypr[3]; double downX = downSensor[X_AXIS]; double downY = downSensor[Y_AXIS]; double downZ = downSensor[Z_AXIS]; // add a little X into Z when calculating roll to compensate for // situations when pitching near vertical as Y, Z will be near 0 and // the results will be unstable double fiddledDownZ = sqrt(downZ*downZ + 0.001*downX*downX); if (downZ <= 0) // compensate for loss of sign when squaring Z fiddledDownZ *= -1.0; ypr[ROLL] = atan2(downY, fiddledDownZ); ypr[PITCH] = -atan2(downX, sqrt(downY*downY + downZ*downZ)); ypr[YAW] = 0; // yaw == 0, accelerometer cannot measure yaw if (! useAccelerometers) { // if no accelerometers, use roll and pitch values from gyroscope // derived orientation ypr[PITCH] = gyroDeducedYPR[PITCH]; ypr[ROLL] = gyroDeducedYPR[ROLL]; ypr[YAW] = gyroDeducedYPR[YAW]; } else if (vecLength(downSensor) == 0) { puts("weightless?"); // something wrong! - weightless?? ypr[PITCH] = gyroDeducedYPR[PITCH]; ypr[ROLL] = gyroDeducedYPR[ROLL]; ypr[YAW] = gyroDeducedYPR[YAW]; } if (! useMagnetometers) { ypr[YAW] = gyroDeducedYPR[YAW]; } myQuat_t accelQ = eulerToQuat(ypr); myQuat_t invAccelQ = quatConjugate(accelQ); /************************************************************************ * Get Magnetometer data that records north direction and create a * 'measured' Quaternion representation of orientation (will only be * yaw) ************************************************************************/ // get magnetometer indication of North double mx1 = (double)(imuData.mag.x_axis - magHardCorrection[X_AXIS]); double my1 = (double)(imuData.mag.y_axis - magHardCorrection[Y_AXIS]); double mz1 = (double)(imuData.mag.z_axis - magHardCorrection[Z_AXIS]); mx1 *= magSoftCorrection[X_AXIS]; my1 *= magSoftCorrection[Y_AXIS]; mz1 *= magSoftCorrection[Z_AXIS]; // NB MPU9250 has different XYZ axis for magnetometers than for gyros // and accelerometers so juggle x,y,z here... double magV[3] = { my1, mx1, -mz1 }; vecNormalise(magV); // make quaternion representing mag readings myQuat_t magQ = quatFromValues(0, magV[X_AXIS], magV[Y_AXIS], magV[Z_AXIS]); if (vecLength(magV) == 0) { // something wrong! - no magnetic field?? //puts("not on earth?"); magQ = quatFromValues(1, 0, 0, 0); } // the magnetometers can only measure yaw (in the world reference // frame), adjust the yaw of the roll/pitch/yaw values calculated // earlier to account for the magnetometer readings. if (useMagnetometers) { // pitch and roll the mag direction using accelerometer info to // create a quaternion that represents the IMU as if it was // horizontal (so we can get pure yaw) myQuat_t horizQ = quatMultiply(accelQ, magQ); horizQ = quatMultiply(horizQ, invAccelQ); // calculate pure yaw ypr[YAW] = -atan2(horizQ.y, horizQ.x); } // measuredQ is the orientation as measured using the // accelerometer/magnetometer readings myQuat_t measuredQ = eulerToQuat(ypr); // check measured and deduced orientations are not wildly different (eg // due to a 360d alias flip) and adjust if problem... measuredQ = adjustForCongruence(measuredQ, gyroDeducedQ); /************************************************************************* * The gyro estimated orientation will be smooth and precise over short * time intervals but small errors will accumulate causing it to * drift over time. The 'measured' orientation as obtained from the * magnetometer and accelerometer will be jumpy but will always average * around the correct orientation. * * Take the mag and accel 'measured' orientation and the gyro * 'estimated' orientation and create a new 'current' orientation that * is the estimated orientation corrected slightly towards the measured * orientation. That way we get the smooth precision of the gyros but * eliminate the accumulation of drift errors. ************************************************************************/ if (useGyroscopes) { // the new orientation is the gyro deduced position corrected // towards the accel/mag measured position using interpolation // between quaternions. currentQ = slerp(gyroDeducedQ, measuredQ, 0.02); } else { currentQ = measuredQ; } if (! isQuatValid(currentQ)) { puts("error in quaternion, reseting orientation..."); displayData(imuData); dumpQuat(currentQ); makeIdentityQuat(¤tQ); } } return currentQ; }