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;
}
int _tmain(int argc, _TCHAR* argv[])
{
// Create a remote to track output
vrpn_Tracker_Remote *remote = new vrpn_Tracker_Remote("[email protected]");
remote->register_change_handler(NULL, tracker_handler);
remote->shutup = true;
for(int i = 0; i < iSKELETON_NUM; i++)
{
for(int j = 0; j < 3; j++)
position[i][j] =0;
for(int j = 0; j < 4; j++)
orientation[i][j] =0;
}
double eulerOrientation[3]={0,0,0};
// Endless loop to print the changing values
while (true)
{
// Update the joint positions
remote->mainloop();
// Clear console
system("cls");
for (int i=0; i<3/*iSKELETON_NUM*/; i++)
{
// Convert orientation quaternion to euler angles
q_to_euler(eulerOrientation, orientation[i]);
// Output updated data
printf("\nJoint: %i\n", i);
printf("Position: \n X: %f\n Y: %f\n Z: %f\n", position[i][0], position[i][1], position[i][2]);
printf("Orientation: \n Yaw: %f\n Pitch: %f\n Roll: %f\n", eulerOrientation[0], eulerOrientation[1], eulerOrientation[2]);
}
//std::cout << std::endl << std::endl;
_sleep(500);
}
return 0;
}
extern void quat_to_euler(rotation_euler* euler, rotation_quat* quaternion)
{
// rotation_quat -> q_vec
q_type quat_temp;
quat_temp[Q_X] = quaternion->qx;
quat_temp[Q_Y] = quaternion->qy;
quat_temp[Q_Z] = quaternion->qz;
quat_temp[Q_W] = quaternion->qw;
q_vec_type euler_temp;
q_to_euler(euler_temp, quat_temp);
// q_vec_type -> rotation_euler
euler->roll = euler_temp[Q_ROLL];
euler->pitch = euler_temp[Q_PITCH];
euler->yaw = euler_temp[Q_YAW];
}
int vrpn_Tracker_DTrack::dtrack2vrpn_body(int id, const char* str_dtrack, int id_dtrack,
const float* loc, const float* rot, struct timeval timestamp)
{
d_sensor = id;
// position (plus converting to unit meter):
pos[0] = loc[0] / 1000.;
pos[1] = loc[1] / 1000.;
pos[2] = loc[2] / 1000.;
// orientation:
q_matrix_type destMatrix; // if this is not good, just build the matrix and do a matrixToQuat
destMatrix[0][0] = rot[0];
destMatrix[0][1] = rot[1];
destMatrix[0][2] = rot[2];
destMatrix[0][3] = 0.0;
destMatrix[1][0] = rot[3];
destMatrix[1][1] = rot[4];
destMatrix[1][2] = rot[5];
destMatrix[1][3] = 0.0;
destMatrix[2][0] = rot[6];
destMatrix[2][1] = rot[7];
destMatrix[2][2] = rot[8];
destMatrix[2][3] = 0.0;
destMatrix[3][0] = 0.0;
destMatrix[3][1] = 0.0;
destMatrix[3][2] = 0.0;
destMatrix[3][3] = 1.0;
q_from_row_matrix(d_quat, destMatrix);
// pack and deliver tracker report:
if(d_connection){
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, "vrpn_Tracker_DTrack: cannot write message: tossing.\n");
}
}
// tracing:
if(tracing && ((tracing_frames % 10) == 0)){
q_vec_type yawPitchRoll;
q_to_euler(yawPitchRoll, d_quat);
printf("body id (DTrack vrpn): %s%d %d pos (x y z): %.4f %.4f %.4f euler (y p r): %.3f %.3f %.3f\n",
str_dtrack, id_dtrack, id, pos[0], pos[1], pos[2],
Q_RAD_TO_DEG(yawPitchRoll[0]), Q_RAD_TO_DEG(yawPitchRoll[1]), Q_RAD_TO_DEG(yawPitchRoll[2]));
}
return 0;
}
int main(int argc, char *argv[])
{
unsigned i;
q_matrix_type row_matrix;
double sx, sy, sz; /* Scale in X, Y, and Z */
q_xyz_quat_type q;
q_vec_type euler;
const char *infile_name = NULL;
FILE *infile = NULL;
char line[1025];
/* Check the command-line arguments. */
if (argc != 2) {
Usage(argv[0]);
} else {
infile_name = argv[1];
}
/* Open and read the file */
/* Transpose the matrix while it is being read into a row matrix. */
if ( (infile = fopen(infile_name, "r")) == NULL) {
fprintf(stderr,"Cannot open %s for reading\n", infile_name);
return -2;
}
line[1024] = '\0'; /* Ensure null-termination */
/* Print the comment line. */
if (fgets(line, 1024, infile) == NULL) {
fprintf(stderr, "Could not read comment line from %s\n", infile_name);
return -3;
}
printf("Comment: %s\n", line);
/* Read and parse the first matrix line. */
if (fgets(line, 1024, infile) == NULL) {
fprintf(stderr, "Could not read matrix line 1 from %s\n", infile_name);
return -3;
}
if (sscanf(line, "%lf%lf%lf%lf", &row_matrix[0][0], &row_matrix[1][0], &row_matrix[2][0], &row_matrix[3][0]) != 4) {
fprintf(stderr, "Could not parse matrix line 1 from %s\n", infile_name);
return -3;
}
/* Read and parse the second matrix line. */
if (fgets(line, 1024, infile) == NULL) {
fprintf(stderr, "Could not read matrix line 2 from %s\n", infile_name);
return -3;
}
if (sscanf(line, "%lf%lf%lf%lf", &row_matrix[0][1], &row_matrix[1][1], &row_matrix[2][1], &row_matrix[3][1]) != 4) {
fprintf(stderr, "Could not parse matrix line 2 from %s\n", infile_name);
return -3;
}
/* Read and parse the third matrix line. */
if (fgets(line, 1024, infile) == NULL) {
fprintf(stderr, "Could not read matrix line 3 from %s\n", infile_name);
return -3;
}
if (sscanf(line, "%lf%lf%lf%lf", &row_matrix[0][2], &row_matrix[1][2], &row_matrix[2][2], &row_matrix[3][2]) != 4) {
fprintf(stderr, "Could not parse matrix line 3 from %s\n", infile_name);
return -3;
}
/* Read and parse the fourth matrix line. */
if (fgets(line, 1024, infile) == NULL) {
fprintf(stderr, "Could not read matrix line 4 from %s\n", infile_name);
return -3;
}
if (sscanf(line, "%lf%lf%lf%lf", &row_matrix[0][3], &row_matrix[1][3], &row_matrix[2][3], &row_matrix[3][3]) != 4) {
fprintf(stderr, "Could not parse matrix line 4 from %s\n", infile_name);
return -3;
}
printf("Row Matrix directly:\n");
q_print_matrix(row_matrix);
/* The conversion routines cannot handle non-unit scaling. Compute the
* scale of each row in the matrix, print the scales, then divide each
* row to normalize before we do the rest of the math. */
sx = sqrt(row_matrix[0][0]*row_matrix[0][0] +
row_matrix[0][1]*row_matrix[0][1] +
row_matrix[0][2]*row_matrix[0][2]);
sy = sqrt(row_matrix[1][0]*row_matrix[1][0] +
row_matrix[1][1]*row_matrix[1][1] +
row_matrix[1][2]*row_matrix[1][2]);
sz = sqrt(row_matrix[2][0]*row_matrix[2][0] +
row_matrix[2][1]*row_matrix[2][1] +
row_matrix[2][2]*row_matrix[2][2]);
printf("Scale:\n %lg, %lg, %lg\n", sx, sy, sz);
for (i = 0; i < 3; i++) {
row_matrix[0][i] /= sx;
row_matrix[1][i] /= sy;
row_matrix[2][i] /= sz;
}
/* Convert to pos/quat, then Euler and print. */
q_row_matrix_to_xyz_quat(&q, row_matrix);
q_to_euler(euler, q.quat);
printf("Translation:\n %lf, %lf, %lf\n", q.xyz[0], q.xyz[1], q.xyz[2]);
printf("Euler angles (degrees):\n %lg, %lg, %lg\n",
euler[2] * DEGREES_PER_RADIAN, /* Rotation about X is stored in the third component. */
euler[1] * DEGREES_PER_RADIAN,
euler[0] * DEGREES_PER_RADIAN);
} /* main */
