void idle(void) { static int frames; static float time; float ifps; vec3 v; vec4 q0,q1,q2; matrix m; ifps = 1.0 / getfps(); if(!my_pause) angle += ifps * 360.0 / 4.0; v_set(sin(angle * DEG2RAD / 3),cos(angle * DEG2RAD / 7),2,light); v_set(0,0,1,v); q_set(v,psi,q0); v_set(0,1,0,v); q_set(v,phi,q1); q_multiply(q0,q1,q2); q_to_matrix(q2,m); v_set(dist,0,0,camera); v_transform(camera,m,camera); v_add(camera,mesh,camera); frames++; time += ifps; if(time > 1.0) { printf("%d frames %.2f fps\n",frames,(float)frames / time); frames = 0; time = 0; } }
float KChainOrientationBodySchema::TryInverseKinematics(const cart_vec_t position){ int i,j; float cand,rdist=0,tdist=0,k=0.0001; //rotation vs translation weight CVector3_t stack[MAX_LINKS]; CVector3_t tar,newrod,rod,diff,pos; CQuat_t q1,q2,q3,q4,rot; // converting data format v_copy(position.GetArray(),pos); q_complete(position.GetArray()+3,rot); q_inv(rot,q1); for(i=0;i<nb_joints;i++){ GetQuaternion(i,q2); q_multiply(q2,q1,q3); q_copy(q3,q1); } for(j=0;j<50;j++){ #ifdef WITH_LAST_LINK last_link->GetTranslation(rod); #else v_clear(rod); #endif InverseKinematicsStack(pos,stack); for(i=nb_joints-1;i>=0;i--){ GetInverseQuaternion(i,q2); q_multiply(q2,q1,q3); //q1*inv(Ri) if(IsInverted(i)==-1){ v_copy(stack[i],tar); Translate(i,rod,newrod); //getting to joint cand = joints[i]->MinimizePositionAndRotationAngle(tar,newrod,q3,k); cand = AngleInClosestRange(i,cand); joints[i]->SetAngle(-cand);// todo to check if it is - Rotate(i,newrod,rod); // updating "rod" v_sub(tar,rod,diff); } else{ Rotate(i,stack[i+1],tar); //rotating back cand = joints[i]->MinimizePositionAndRotationAngle(tar,rod,q3,k); cand = AngleInClosestRange(i,cand); joints[i]->SetAngle(cand); Rotate(i,rod,newrod); Translate(i,newrod,rod); v_sub(tar,newrod,diff); } GetQuaternion(i,q2); q_multiply(q3,q2,q1); q_multiply(q2,q3,q4); rdist = v_length(q4);//rotation distance, only the first 3 components tdist = v_length(diff);//translation distance // cout<<"rot "<<rdist<<" pos "<<tdist<<" prod: "<<(1-k)*rdist+k*tdist<<endl; if(tdist<tol && rdist<rot_tol){return rdist/rot_tol+tdist;} } q_multiply(rot,q1,q2); q_inv(rot,q3); q_multiply(q2,q3,q1); } return rdist/rot_tol + tdist; }
int main(int argc, char **argv){ init(); short accel[3], gyro[3], sensors[1]; long quat[4]; unsigned long timestamp; unsigned char more[0]; time_t sec, current_time; // set to the time before calibration int running = 0; // set to 1 once the calibration has been done time(&sec); printf("Read system time\n"); while (1){ short status; mpu_get_int_status(&status); if (! (status & MPU_INT_STATUS_DMP) ){ continue; } int fifo_read = dmp_read_fifo(gyro, accel, quat, ×tamp, sensors, more); if (fifo_read != 0) { printf("Error reading fifo.\n"); } if (fifo_read == 0 && sensors[0] && running) { float angles[NOSENTVALS]; float temp_quat[4]; rescale_l(quat, temp_quat, QUAT_SCALE, 4); if (!quat_offset[0]) { // check if the IMU has finished calibrating if(abs(last_quat[1]-temp_quat[1]) < THRESHOLD) { // the IMU has finished calibrating int i; quat_offset[0] = temp_quat[0]; // treat the w value separately as it does not need to be reversed for(i=1;i<4;++i){ quat_offset[i] = -temp_quat[i]; } } else { memcpy(last_quat, temp_quat, 4*sizeof(float)); } } else { q_multiply(quat_offset, temp_quat, angles+9); // multiply the current quaternstion by the offset caputured above to re-zero the values // rescale the gyro and accel values received from the IMU from longs that the // it uses for efficiency to the floats that they actually are and store these values in the angles array rescale_s(gyro, angles+3, GYRO_SCALE, 3); rescale_s(accel, angles+6, ACCEL_SCALE, 3); // turn the quaternation (that is already in angles) into euler angles and store it in the angles array euler(angles+9, angles); printf("Yaw: %+5.1f\tRoll: %+5.1f\tPitch: %+5.1f\n", angles[0]*180.0/PI, angles[1]*180.0/PI, angles[2]*180.0/PI); // send the values in angles over UDP as a string (in udp.c/h) udp_send(angles, 13); } } else { time(¤t_time); // check if more than CALIBRATION_TIME seconds has passed since calibration started if(abs(difftime(sec, current_time)) > CALIBRATION_TIME) { printf("Calibration time up...\n"); // allow all of the calculating, broadcasting and offset code from above to run running = 1; } else printf("Calibrating...\n"); } } }