/*!***************************************************************************** ******************************************************************************* \note compute_kinematics \date June 99 \remarks computes kinematic variables ******************************************************************************* Function Parameters: [in]=input,[out]=output none ******************************************************************************/ static void compute_kinematics(void) { int i,j,r,o; static int firsttime = TRUE; static SL_DJstate *temp; static Matrix last_J; static Matrix last_Jbase; if (firsttime) { temp=(SL_DJstate *)my_calloc((unsigned long)(n_dofs+1),sizeof(SL_DJstate),MY_STOP); last_J = my_matrix(1,6*n_endeffs,1,n_dofs); last_Jbase = my_matrix(1,6*n_endeffs,1,2*N_CART); } /* compute the desired link positions */ linkInformationDes(joint_des_state,&base_state,&base_orient,endeff, joint_cog_mpos_des,joint_axis_pos_des,joint_origin_pos_des, link_pos_des,Alink_des,Adof_des); /* the desired endeffector information */ for (i=1; i<=N_CART; ++i) { for (j=1; j<=n_endeffs; ++j) { cart_des_state[j].x[i] = link_pos_des[link2endeffmap[j]][i]; } } /* the desired quaternian of the endeffector */ for (j=1; j<=n_endeffs; ++j) { linkQuat(Alink_des[link2endeffmap[j]],&(cart_des_orient[j])); } /* addititional link information */ linkInformation(joint_state,&base_state,&base_orient,endeff, joint_cog_mpos,joint_axis_pos,joint_origin_pos, link_pos,Alink,Adof); /* create the endeffector information */ for (i=1; i<=N_CART; ++i) { for (j=1; j<=n_endeffs; ++j) { cart_state[j].x[i] = link_pos[link2endeffmap[j]][i]; } } /* the quaternian of the endeffector */ for (j=1; j<=n_endeffs; ++j) { linkQuat(Alink[link2endeffmap[j]],&(cart_orient[j])); } /* the COG position */ compute_cog(); /* the jacobian */ jacobian(link_pos,joint_origin_pos,joint_axis_pos,J); baseJacobian(link_pos,joint_origin_pos,joint_axis_pos,Jbase); jacobian(link_pos_des,joint_origin_pos_des,joint_axis_pos_des,Jdes); baseJacobian(link_pos_des,joint_origin_pos_des,joint_axis_pos_des,Jbasedes); /* numerical time derivative of Jacobian */ if (!firsttime) { mat_sub(J,last_J,dJdt); mat_mult_scalar(dJdt,(double)ros_servo_rate,dJdt); mat_sub(Jbase,last_Jbase,dJbasedt); mat_mult_scalar(dJbasedt,(double)ros_servo_rate,dJbasedt); } mat_equal(J,last_J); mat_equal(Jbase,last_Jbase); /* compute the cartesian velocities and accelerations */ for (j=1; j<=n_endeffs; ++j) { for (i=1; i<=N_CART; ++i) { cart_state[j].xd[i] = 0.0; cart_state[j].xdd[i] = 0.0; cart_orient[j].ad[i] = 0.0; cart_orient[j].add[i] = 0.0; cart_des_state[j].xd[i] = 0.0; cart_des_orient[j].ad[i] = 0.0; /* contributations from the joints */ for (r=1; r<=n_dofs; ++r) { cart_state[j].xd[i] += J[(j-1)*6+i][r] * joint_state[r].thd; cart_orient[j].ad[i] += J[(j-1)*6+i+3][r] * joint_state[r].thd; cart_des_state[j].xd[i] += Jdes[(j-1)*6+i][r] *joint_des_state[r].thd; cart_des_orient[j].ad[i]+= Jdes[(j-1)*6+i+3][r] * joint_des_state[r].thd; cart_state[j].xdd[i] += J[(j-1)*6+i][r] * joint_state[r].thdd + dJdt[(j-1)*6+i][r] * joint_state[r].thd; cart_orient[j].add[i] += J[(j-1)*6+i+3][r] * joint_state[r].thdd + dJdt[(j-1)*6+i+3][r] * joint_state[r].thd; } /* contributations from the base */ for (r=1; r<=N_CART; ++r) { cart_state[j].xd[i] += Jbase[(j-1)*6+i][r] * base_state.xd[r]; cart_orient[j].ad[i] += Jbase[(j-1)*6+i+3][r] * base_state.xd[r]; cart_state[j].xd[i] += Jbase[(j-1)*6+i][3+r] * base_orient.ad[r]; cart_orient[j].ad[i] += Jbase[(j-1)*6+i+3][3+r] * base_orient.ad[r]; cart_des_state[j].xd[i] += Jbasedes[(j-1)*6+i][r] * base_state.xd[r]; cart_des_orient[j].ad[i] += Jbasedes[(j-1)*6+i+3][r] * base_state.xd[r]; cart_des_state[j].xd[i] += Jbasedes[(j-1)*6+i][3+r] * base_orient.ad[r]; cart_des_orient[j].ad[i] += Jbasedes[(j-1)*6+i+3][3+r] * base_orient.ad[r]; cart_state[j].xdd[i] += Jbase[(j-1)*6+i][r] * base_state.xdd[r] + dJbasedt[(j-1)*6+i][r] * base_state.xd[r]; cart_orient[j].add[i] += Jbase[(j-1)*6+i+3][r] * base_state.xdd[r] + dJbasedt[(j-1)*6+i+3][r] * base_state.xd[r]; cart_state[j].xdd[i] += Jbase[(j-1)*6+i][3+r] * base_orient.add[r] + dJbasedt[(j-1)*6+i][3+r] * base_orient.ad[r]; cart_orient[j].add[i] += Jbase[(j-1)*6+i+3][3+r] * base_orient.add[r] + dJbasedt[(j-1)*6+i+3][3+r] * base_orient.ad[r]; } } /* compute quaternion derivatives */ quatDerivatives(&(cart_orient[j])); quatDerivatives(&(cart_des_orient[j])); for (r=1; r<=N_QUAT; ++r) cart_des_orient[j].qdd[r] = 0.0; // we don't have dJdes_dt so far } /* reset first time flag */ firsttime = FALSE; }
void expm(Matrix A, Matrix B, double h) { static int firsttime = TRUE; static Matrix M; static Matrix M2; static Matrix M3; static Matrix D; static Matrix N; if (firsttime) { firsttime = FALSE; M = my_matrix(1,3*N_DOFS,1,3*N_DOFS); M2 = my_matrix(1,3*N_DOFS,1,3*N_DOFS); M3 = my_matrix(1,3*N_DOFS,1,3*N_DOFS); D = my_matrix(1,3*N_DOFS,1,3*N_DOFS); N = my_matrix(1,3*N_DOFS,1,3*N_DOFS); } int norm = 0; double c = 0.5; int q = 6; // uneven p-q order for Pade approx. int p = 1; int i,j,k; // Form the big matrix mat_mult_scalar(A, h, A); mat_equal_size(A, 2*N_DOFS, 2*N_DOFS, M); for (i = 1; i <= 2*N_DOFS; ++i) { for (j = 1; j <= N_DOFS; ++j) { M[i][2*N_DOFS + j] = h * B[i][j]; } } //print_mat("M is:\n", M); // scale M by power of 2 so that its norm < 1/2 norm = (int)(log2(inf_norm(M))) + 2; //printf("Inf norm of M: %f \n", inf_norm(M)); if (norm < 0) norm = 0; mat_mult_scalar(M, 1/pow(2,(double)norm), M); //printf("Norm of M logged, floored and 2 added is: %d\n", norm); //print_mat("M after scaling is:\n", M); for (i = 1; i <= 3*N_DOFS; i++) { for (j = 1; j <= 3*N_DOFS; j++) { N[i][j] = c * M[i][j]; D[i][j] = -c * M[i][j]; } N[i][i] = N[i][i] + 1.0; D[i][i] = D[i][i] + 1.0; } // set M2 equal to M mat_equal(M, M2); // start pade approximation for (k = 2; k <= q; k++) { c = c * (q - k + 1) / (double)(k * (2*q - k + 1)); mat_mult(M,M2,M2); mat_mult_scalar(M2,c,M3); mat_add(N,M3,N); if (p == 1) mat_add(D,M3,D); else mat_sub(D,M3,D); p = -1 * p; } // multiply denominator with nominator i.e. D\E my_inv_ludcmp(D, 3*N_DOFS, D); mat_mult(D,N,N); // undo scaling by repeated squaring for (k = 1; k <= norm; k++) mat_mult(N,N,N); // get the matrices out // read off the entries for (i = 1; i <= 2*N_DOFS; i++) { for (j = 1; j <= 2*N_DOFS; j++) { A[i][j] = N[i][j]; } for (j = 1; j <= N_DOFS; j++) { B[i][j] = N[i][2*N_DOFS + j]; } } }