// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented // as a unit quaternion. void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat) { using std::atan2; using std::asin; using std::sqrt; // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion. float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()), 1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y())); float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x())); float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()), 1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z())); //hacky globals ROLL = roll; PITCH = pitch; YAW = yaw; // Convert the floating point angles in radians to a scale from 0 to 20. roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18); pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18); yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18); if (CURRENTPOSE == myo::Pose::waveIn) { WAVEINCOUNTER += 2; } if (CURRENTPOSE == myo::Pose::fingersSpread) { FINGSERSPREADCOUNTER += 2; } if (WAVEINCOUNTER != 1) { WAVEINCOUNTER -= 1; } }
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented // as a unit quaternion. void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat) { // Calculate the normalized quaternion. float norm = sqrtf(quat.x() * quat.x() + quat.y() * quat.y() + quat.z() * quat.z() + quat.w() * quat.w()); myo::Quaternion<float> normalized(quat.x() / norm, quat.y() / norm, quat.z() / norm, quat.w() / norm); // Calculate pitch and yaw from the normalized quaternion. float pitch = asinf(2.0f * (normalized.w() * normalized.y() - normalized.z() * normalized.x())); float yaw = atan2f(2.0f * (normalized.w() * normalized.z() + normalized.x() * normalized.y()), 1.0f - 2.0f * (normalized.y() * normalized.y() + normalized.z() * normalized.z())); // Calculate the change in pitch and yaw since the last update. float deltaPitch = pitch - _previousPitch; float deltaYaw = yaw - _previousYaw; // Calculate the amount by which to move the cursor. // The value SENSITIVITY_X and SENSITIVITY_Y control how sensitive the cursor is // to movements made by the Myo in the x- and y-directions, respectively. int dx = deltaYaw * SENSITIVITY_X; int dy = deltaPitch * SENSITIVITY_Y; if (cameraMode) { // Actually move the cursor. POINT cursorPos; bool result = GetCursorPos(&cursorPos); SetCursorPos(cursorPos.x - dx, cursorPos.y - dy); } // Store the new pitch and yaw values for next time. _previousPitch = pitch; _previousYaw = yaw; }
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented // as a unit quaternion. void MyoDataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat) { using std::atan2; using std::asin; using std::sqrt; // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion. float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()), 1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y())); float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x())); float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()), 1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z())); // Convert the floating point angles in radians to a scale from 0 to 20. float roll_f = ((roll + (float)M_PI)/(M_PI * 2.0f) * 18); float pitch_f = ((pitch + (float)M_PI/2.0f)/M_PI * 18); float yaw_f = ((yaw + (float)M_PI)/(M_PI * 2.0f) * 18); int roll_w = static_cast<int>(roll_f); int pitch_w = static_cast<int>(pitch_f); int yaw_w = static_cast<int>(yaw_f); MyoData &data = knownMyosData[myo]; data.roll = roll_f/18.0*100.0; data.pitch = pitch_f/18.0*100.0; data.yaw = yaw_f/18.0*100.0; sendOrientation(myo, data.roll, data.pitch, data.yaw); }
void DataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat) { using std::atan2; using std::asin; using std::sqrt; using std::max; using std::min; controlSystem = controller->getControlSystem(); systemUnlocked = controller->getSystemStatus(); if (controlSystem==2 && systemUnlocked){ roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()), 1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y())); roll_w = static_cast<int>((roll + (float)M_PI/2.0f)/M_PI * 18); controller->setRoll(roll_w); } else { pitch = asin(max(-1.0f, min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x())))); pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18); controller->setPitch(pitch_w); //cout << "SPEED: " << controller->longSpeed[pitch_w] << endl; } if (controlSystem == 0 && systemUnlocked){ yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()), 1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z())); yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18); controller->setYaw(yaw_w); } }
void DeviceCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat) { Device * device = findDevice(myo); if ( device ) { using std::atan2; using std::asin; using std::sqrt; // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion. float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()), 1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y())); float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x())); float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()), 1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z())); device->q = quat;//set(quat.x(), quat.y(), quat.z(), quat.w()); device->roll = roll; device->pitch = pitch; device->yaw = yaw; // Convert the floating point angles in radians to a scale from 0 to 20. device->roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18); device->pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18); device->yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18); float gyoro_g = sqrt(device->gyro.x()*device->gyro.x() + device->gyro.y()*device->gyro.y() + device->gyro.z()*device->gyro.z()); float g = sqrt(device->accel.x()*device->accel.x() + device->accel.y()*device->accel.y() + device->accel.z()*device->accel.z()); // cout << gyoro_g << endl; // cout << g << endl; if ( gyoro_g <= 0.2 ) device->gravity = g; // Quaternion4f q; // q = quat; // //.set(quat.x(), quat.z(), quat.y(), quat.w()); // Vector3f linear_accel; // ofMatrix4x4 mat; // mat.translate(ofVec3f(0,device->gravity,0)); // mat.rotate(q); // ofVec3f trans = mat.getTranslation(); // // linear_accel = device->getAccel(); // linear_accel.x = linear_accel.x - trans.x; // linear_accel.y = linear_accel.y - trans.z; // linear_accel.z = linear_accel.z - trans.y; // // device->linear_accel.set(linear_accel); // cout << device->getAccel() << endl; // cout << mat.getTranslation() << endl; // cout << linear_accel << endl; } }
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented // as a unit quaternion. void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat) { using std::atan2; using std::asin; using std::sqrt; // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion. float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()), 1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y())); float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x())); float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()), 1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z())); // Convert the floating point angles in radians to a scale from 0 to 20. roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18); pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18); yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18); float drop = 1.1; roll = ((roll + (float)M_PI)/(M_PI * 2.0f) * 18); pitch = ((pitch + (float)M_PI/2.0f)/M_PI * 18); if(pitch > 10) { if(pitch < 20) { pitch *= (drop*(20-pitch)); } } else { if(pitch != 10) { pitch *= (drop*(10-pitch)); } } yaw = ((yaw + (float)M_PI)/(M_PI * 2.0f) * 18); if(yaw > 10.0000) { if(yaw < 20.0000) { yaw *= (drop*(20-yaw)); } else { yaw = 19.9999; } } else { if(yaw != 10.0000) { yaw *= (drop*(10-yaw)); } } if(color_input == true) { float x = (GetSystemMetrics(SM_CXSCREEN)); float y = (GetSystemMetrics(SM_CYSCREEN)); SetCursorPos((((((20)-yaw))/(20))*x), ((pitch/(20))*y)); } }
char DataCollector::quat_to_note(myo::Quaternion<float> q){ /* A- X .51 Y -.16 Z 0.85 B- X .43 Y -.36 Z .81 C- X .26 Y -.45 Z .69 D- X .40 Y .12 Z .89 E- X .30 Y .04 Z .76 F- X .32 Y .39 Z .77 G- X .27 Y .35 Z .60 */ myo::Vector3<float> v(q.x(), q.y(), q.z()); if (angle_between(v, a_vec) <=30){ return 'A'; } else if (angle_between(v, b_vec) <= 30){ return 'B'; } else if (angle_between(v, c_vec) <= 30){ return 'C'; } else if (angle_between(v, d_vec) <= 30){ return 'D'; } else if (angle_between(v, e_vec) <= 30){ return 'E'; } else if (angle_between(v, f_vec) <= 30){ return 'F'; } else if (angle_between(v, g_vec) <= 30){ return 'G'; } else return 'X'; }
void vrpn_Tracker_ThalmicLabsMyo::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& rotation) { if (myo != _myo) return; if (!d_connection) { return; } vrpn_gettimeofday(&_timestamp, NULL); double dt = vrpn_TimevalDurationSeconds(_timestamp, vrpn_Button::timestamp); vrpn_Tracker::timestamp = _timestamp; vrpn_Analog::timestamp = _timestamp; d_quat[0] = rotation.x(); d_quat[1] = rotation.y(); d_quat[2] = rotation.z(); d_quat[3] = rotation.w(); // do the same as analog, with euler angles (maybe offset from when OnArmSync?) q_vec_type euler; q_to_euler(euler, d_quat); channel[ANALOG_ROTATION_X] = euler[Q_ROLL]; channel[ANALOG_ROTATION_Y] = euler[Q_PITCH]; channel[ANALOG_ROTATION_Z] = euler[Q_YAW]; char msgbuf[1000]; 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, "Thalmic Lab's myo tracker: can't write message: tossing\n"); } _analogChanged = true; }
void DataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat) { using std::atan2; using std::asin; using std::sqrt; // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion. float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()), 1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y())); float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x())); float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()), 1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z())); // Convert the floating point angles in radians to a scale from 0 to 20. roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 100); pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 100); yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 100); }
void myouse::MyouseListener::onOrientationData(myo::Myo * myo, uint64_t timestamp, const myo::Quaternion<float> & rotation) { using std::atan2; using std::asin; using std::sin; double newRoll = atan2(2.0f * (rotation.w() * rotation.x() + rotation.y() * rotation.z()), 1.0f - 2.0f * (rotation.x() * rotation.x() + rotation.y() * rotation.y())); double newPitch = asin(2.0f * (rotation.w() * rotation.y() - rotation.z() * rotation.x())); double newYaw = atan2(2.0f * (rotation.w() * rotation.z() + rotation.x() * rotation.y()), 1.0f - 2.0f * (rotation.y() * rotation.y() + rotation.z() * rotation.z())); double roll = newRoll - rollOffset; double pitch = newPitch - pitchOffset; double yaw = newYaw - yawOffset; if (xDir == myo::xDirectionTowardElbow) pitch *= -1; if (isScrolling) { scroll(-pitch * SCROLL_SPEED); } else { int x = SCREEN_WIDTH * (0.5 - X_SPEED * yaw); int y = SCREEN_HEIGHT * (0.5 + Y_SPEED * pitch); bool dragging = leftDown || rightDown || middleDown; float dist = sqrt((x - lastX) * (x - lastX) + (y - lastY) * (y - lastY)); if (!dragging || dist > DRAG_THRESHOLD) { moveMouse(x, y); lastX = x; lastY = y; } } rawRoll = newRoll; rawPitch = newPitch; rawYaw = newYaw; }
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented // as a unit quaternion. void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat) { using std::atan2; using std::asin; using std::sqrt; using std::max; using std::min; // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion. float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()), 1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y())); float pitch = asin(max(-1.0f, min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x())))); float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()), 1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z())); //M_PI doesnt work // Convert the floating point angles in radians to a scale from 0 to 18. roll_w = static_cast<int>((roll + (float)3.14) / (3.14 * 2.0f) * 18); pitch_w = static_cast<int>((pitch + (float)3.14 / 2.0f) / 3.14 * 18); yaw_w = static_cast<int>((yaw + (float)3.14) / (3.14 * 2.0f) * 18); }
jobject getJavaQuaternion(JNIEnv * env, myo::Quaternion<float> quaternion) { jclass quaternionClass = env->FindClass("net/johnluetke/myojni/jni/Quaternion"); jmethodID quaternionConstructor = env->GetMethodID(quaternionClass, "<init>", "(DDDD)V"); jobject javaQuaternion = env->NewObject(quaternionClass, quaternionConstructor, quaternion.x(), quaternion.y(), quaternion.z(), quaternion.w()); return javaQuaternion; }
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented // as a unit quaternion. void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat) { using std::atan2; using std::asin; using std::sqrt; std::cout<<"orientation fucntion called"<<std::endl; // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion. float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()), 1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y())); float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x())); float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()), 1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z())); osc::OutboundPacketStream p(buffer, OUTPUT_BUFFER_SIZE); p << osc::BeginMessage("/myo/orientation") << MAC << quat.x() << quat.y() << quat.z() << quat.w() << roll << pitch << yaw << osc::EndMessage; transmitSocket->Send(p.Data(), p.Size()); // Convert the floating point angles in radians to a scale from 0 to 20. roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18); pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18); yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18); }
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented as a unit quaternion. // This function takes the position values from the quaternion and assign new values to roll_w, pitch_w and yaw_w every time new data are provided by the Myo. void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat) { using std::atan2; using std::asin; using std::sqrt; using std::max; using std::min; // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion. float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()), 1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y())); // Convert the floating point angles in radians to a clockwise scale from 1 to 127 (good for PureData) with the zero angle in π. // Need to change the format of the angle? DO IT HERE! roll_tmp = static_cast<int>( ( roll * 180/M_PI) ); // the value of 200 ensures a positive value for the angle. }
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented // as a unit quaternion. void Filter::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat) { quatX = quat.x(); //Return the x-component of this quaternion's vector quatY = quat.y(); //Return the y -component of this quaternion's vector quatZ = quat.z(); //Return the z-component of this quaternion's vector quatW = quat.w(); //Return the w-component (scalar) of this quaternion's vector using std::atan2; using std::asin; using std::sqrt; using std::cos; using std::sin; using std::max; using std::min; roll = -1 * atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()), 1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z())); pitch = -1 * atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()), 1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y())); yaw = -1 * asin(max(-1.0f, min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x())))); }
void MyoDataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat) { using std::atan2; using std::asin; using std::sqrt; // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion. float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()), 1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y())); float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x())); float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()), 1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z())); MyoFrameOrientationData & orient_data = m_orientation_data; orient_data.timestamp = timestamp; orient_data.quaternion = ofQuaternion( quat.x(), quat.y(), quat.z(), quat.w() ); orient_data.euler_roll = roll; orient_data.euler_pitch = pitch; orient_data.euler_yaw = yaw; }
float QuaternionToPitch(const myo::Quaternion<float>& quat) { return std::asin(std::max( -1.0f, std::min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x())))); }
// Orientaion void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat) { const int count = 4; float values[count] = { quat.x(), quat.y(), quat.z(), quat.w() }; this->addEventData(myo, timestamp, "orientation", new VectorEventData(values, count)); }
void WifiDataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat) { using std::atan2; using std::asin; using std::sqrt; using std::max; using std::min; ClientInfo* clientinfo = new ClientInfo(); clientinfo->ID = 0; clientinfo->message = new std::string("message"); clientinfo->name = "Client"; // Calculate Euler angles (roll and pitch) from the unit quaternion. float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()), 1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y())); float pitch = asin(max(-1.0f, min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x())))); //Transforms the radians to degrees angles. roll_w = (int)(roll * 180 / M_PI); pitch_w = (int)(pitch * 180 / M_PI); //To use in the robot servo motor, roll_w mut be between 45 and 135 degrees //Gives an interval of 30 degrees to stabilize at 0 degrees. //The 0 degree on the servo motor is at 90 degrees. if (roll_w >= -15 && roll_w <= 15) { roll_w = 90; } else if (roll_w < -15) { //roll_w min value is 45. if (roll_w < -60) roll_w = 45; else roll_w += 105; } else if (roll_w > 15) { //roll_w max value is 135 if (roll_w > 60) roll_w = 135; else roll_w += 75; } //The robot has 6 diffirent speeds //Gives an interval of 30 degrees to stabilize at 0 degrees. // Parking state if (pitch_w >= -15 && pitch_w <= 15) { pitch_w = 0; } //Reverse speeds else if (pitch_w < -15) { if (pitch_w >= -35) pitch_w = 4; else if (pitch_w >= -55) pitch_w = 5; else pitch_w = 6; } //Foward speeds else if (pitch_w > 15) { if (pitch_w <= 35) pitch_w = 1; else if (pitch_w <= 55) pitch_w = 2; else pitch_w = 3; } clientinfo->servo = roll_w; clientinfo->speed = pitch_w; client->sendToServer(clientinfo); delete clientinfo; }
float QuaternionToRoll(const myo::Quaternion<float>& quat) { return std::atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()), 1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y())); }