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");
}
}
}
