int LocalNonsmoothNewtonSolver(FrictionContactProblem* localproblem, double * R, int *iparam, double *dparam) { double mu = localproblem->mu[0]; double * qLocal = localproblem->q; double * MLocal = localproblem->M->matrix0; double Tol = dparam[0]; double itermax = iparam[0]; int i, j, k, inew; // store the increment double dR[3] = {0., 0., 0.}; // store the value fo the function double F[3] = {0., 0., 0.}; // Store the (sub)-gradient of the function double A[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.}; double B[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.}; // Value of AW+B double AWplusB[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.}; // Compute values of Rho (should be here ?) double rho[3] = {1., 1., 1.}; #ifdef OPTI_RHO computerho(localproblem, rho); #endif // compute the velocity double velocity[3] = {0., 0., 0.}; for (i = 0; i < 3; i++) velocity[i] = MLocal[i + 0 * 3] * R[0] + qLocal[i] + MLocal[i + 1 * 3] * R[1] + + MLocal[i + 2 * 3] * R[2] ; for (inew = 0 ; inew < itermax ; ++inew) // Newton iteration { //Update function and gradient Function(R, velocity, mu, rho, F, A, B); #ifndef AC_Generated #ifndef NDEBUG double Fg[3] = {0., 0., 0.}; double Ag[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.}; double Bg[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.}; double AWpB[9]; assert(*rho > 0. && *(rho + 1) > 0. && *(rho + 2) > 0.); #ifdef AC_STD frictionContact3D_AlartCurnierFunctionGenerated(R, velocity, mu, rho, Fg, Ag, Bg); #endif #ifdef AC_JeanMoreau frictionContact3D_AlartCurnierJeanMoreauFunctionGenerated(R, velocity, mu, rho, Fg, Ag, Bg); #endif sub3(F, Fg); sub3x3(A, Ag); sub3x3(B, Bg); assert(hypot3(Fg) <= 1e-7); assert(hypot9(Ag) <= 1e-7); assert(hypot9(Bg) <= 1e-7); cpy3x3(A, Ag); cpy3x3(B, Bg); mm3x3(A, MLocal, AWpB); add3x3(B, AWpB); #endif #endif // compute -(A MLocal +B) for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) { AWplusB[i + 3 * j] = 0.0; for (k = 0; k < 3; k++) { AWplusB[i + 3 * j] -= A[i + 3 * k] * MLocal[k + j * 3]; } AWplusB[i + 3 * j] -= B[i + 3 * j]; } } #ifdef AC_STD #ifndef NDEBUG scal3x3(-1., AWpB); sub3x3(AWplusB, AWpB); assert(hypot9(AWpB) <= 1e-7); #endif #endif // Solve the linear system solv3x3(AWplusB, dR, F); // upate iterates R[0] += dR[0]; R[1] += dR[1]; R[2] += dR[2]; // compute new residue for (i = 0; i < 3; i++) velocity[i] = MLocal[i + 0 * 3] * R[0] + qLocal[i] + MLocal[i + 1 * 3] * R[1] + + MLocal[i + 2 * 3] * R[2] ; Function(R, velocity, mu, rho, F, NULL, NULL); dparam[1] = 0.5 * (F[0] * F[0] + F[1] * F[1] + F[2] * F[2]) / (1.0 + sqrt(R[0] * R[0] + R[1] * R[1] + R[2] * R[2])) ; // improve with relative tolerance /* dparam[2] =0.0; FrictionContact3D_unitary_compute_and_add_error( R , velocity,mu, &(dparam[2]));*/ if (verbose > 1) printf("----------------------------------- LocalNewtonSolver number of iteration = %i error = %.10e \n", inew, dparam[1]); if (dparam[1] < Tol) { /* printf("----------------------------------- LocalNewtonSolver number of iteration = %i error = %.10e \t error2 = %.10e \n",inew,dparam[1], dparam[2]); */ return 0; } }// End of the Newton iteration /* printf("----------------------------------- LocalNewtonSolver number of iteration = %i error = %.10e \t error2 = %.10e \n",inew,dparam[1], dparam[2]); */ return 1; }
int main() { DECL_TIMER(T0); DECL_TIMER(T1); int info = 0; int r = -1; FILE* file = fopen("./data/ACinputs.dat", "r"); unsigned int dim = 0; double* reactions; double* velocities; double *mus; double *rhos; r = fscanf(file, "%d\n", &dim); assert(r > 0); if (r <= 0) return(r); reactions = (double *) malloc(3 * dim * sizeof(double)); velocities = (double *) malloc(3 * dim * sizeof(double)); mus = (double *) malloc(dim * sizeof(double)); rhos = (double *) malloc(3 * dim * sizeof(double)); for (unsigned int i = 0; i < dim * 3 ; ++i) { r = fscanf(file, "%lf\n", &reactions[i]); assert(r > 0); }; for (unsigned int i = 0; i < dim * 3 ; ++i) { r = fscanf(file, "%lf\n", &velocities[i]); assert(r > 0); }; for (unsigned int k = 0; k < dim ; ++k) { r = fscanf(file, "%lf\n", &mus[k]); assert(r > 0); }; for (unsigned int i = 0; i < dim * 3 ; ++i) { r = fscanf(file, "%lf\n", &rhos[i]); assert(r > 0); }; double F1[3], A1[9], B1[9], F2[3], A2[9], B2[9]; for (unsigned int k = 0; k < dim; ++k) { double* p; p = F1; OP3(*p++ = NAN); p = F2; OP3(*p++ = NAN); p = A1; OP3X3(*p++ = NAN); p = B1; OP3X3(*p++ = NAN); p = B2; OP3X3(*p++ = NAN); p = A2; OP3X3(*p++ = NAN); START_TIMER(T0); DO(computeAlartCurnierSTDOld(&reactions[k * 3], &velocities[k * 3], mus[k], &rhos[k * 3], F1, A1, B1)); STOP_TIMER(T0); START_TIMER(T1); DO(computeAlartCurnierSTD(&reactions[k * 3], &velocities[k * 3], mus[k], &rhos[k * 3], F1, A1, B1)); STOP_TIMER(T1); PRINT_ELAPSED(T0); PRINT_ELAPSED(T1); #ifdef WITH_TIMERS printf("T1/T0 = %g\n", ELAPSED(T1) / ELAPSED(T0)); #endif p = F1; OP3(info |= isnan(*p++)); assert(!info); p = A1; OP3X3(info |= isnan(*p++)); assert(!info); p = B1; OP3X3(info |= isnan(*p++)); assert(!info); frictionContact3D_AlartCurnierFunctionGenerated(&reactions[k * 3], &velocities[k * 3], mus[k], &rhos[k * 3], F2, A2, B2); p = F1; OP3(info |= isnan(*p++)); assert(!info); p = A1; OP3X3(info |= isnan(*p++)); assert(!info); p = B1; OP3X3(info |= isnan(*p++)); assert(!info); sub3(F1, F2); sub3x3(A1, A2); sub3x3(B1, B2); #define EPS 1e-6 p = F2; OP3(info |= !(*p++ < EPS)); assert(!info); p = A2; OP3X3(info |= !(*p++ < EPS)); assert(!info); p = B2; OP3X3(info |= !(*p++ < EPS)); assert(!info); } free(reactions); free(velocities); free(mus); free(rhos); fclose(file); return (info); }
void kf(float prevCov[3][3], algp1_msgs::Pose2DWithCovariance action, algp1_msgs::Pose2DWithCovariance measure){ bool kill = false; if(kalmanIteration > 30){ //Base case death kill = true; } //The initial state prediction goes here float X_o [3]={ 1, 1, 0 }; float Z_n [3]; Z_n[0] = sense_model_.pose2d.x; Z_n[1] = sense_model_.pose2d.y; Z_n[2] = sense_model_.pose2d.theta; float A [3][3] = {{ 1, 0, 0}, { 0, 1, 0}, { 0, 0, 1}}; float A_t [3][3] = {{ 1, 0, 0}, { 0, 1, 0}, { 0, 0, 1}}; float H [3][3] = {{ 1, 0, 0}, //plz make this stay I so everything below works { 0, 1, 0}, { 0, 0, 1}}; float P [3][3]; for(int i = 0; i < 3; i++){ for(int j = 0; j < 3; j++){ P[i][j] = prevCov[i][j]; } } float Q [3][3] = {{ 0.01, 0, 0}, { 0, 0.01, 0}, { 0, 0, 0.01}}; float R [3][3] = {{ 1, 0, 0}, { 0, 1, 0}, { 0, 0, 1}}; float I [3][3] = {{ 1, 0, 0}, { 0, 1, 0}, { 0, 0, 1}}; //------ State prediction (from velocity model) float X_p[2]; X_p[0] = action.pose2d.x; X_p[1] = action.pose2d.y; X_p[2] = action.pose2d.theta; //------ Covariance Prediction float APnm1[3][3]; multiply3x3(APnm1, A, P); float APnm1At[3][3]; multiply3x3(APnm1At, APnm1, A_t); float P_p[3][3]; add3x3(P_p, APnm1At, Q); //------Innovation float Y_tilde[3]; sub3x1(Y_tilde, Z_n, X_p); //NOTE: In a fully implemented kalman filter where H is not [I], this step is incorrect. //------Innovation Covariance float HP_p[3][3]; multiply3x3(HP_p, H, P_p); float HP_pHt[3][3]; multiply3x3(HP_pHt, HP_p, H); //NOTE: In this case I assume H = H^T float S[3][3]; add3x3(S, HP_pHt, R); //------Kalman Gain float S_inv[3][3]; try{ InvMatrix(S, S_inv); } catch(const char* errorMessage){ std::cout << errorMessage << "\n"; } float P_pHt [3][3]; multiply3x3(P_pHt, P_p, H);//NOTE: In this case I assume H = H^T float K[3][3]; multiply3x3(K, P_pHt, S_inv); //------State update (yay!) float Ky[3]; multiply3x1and3x3(Ky, K, Y_tilde); float X_n[3]; //New state! add3x1(X_n, X_p, Ky); //------Covariance update (yay yay!) float KH[3][3]; multiply3x3(KH, K, H); float IminusKH[3][3]; sub3x3(IminusKH, I, KH); float P_n[3][3]; multiply3x3(P_n, IminusKH, P_p); //Publish state update algp1_msgs::Pose2DWithCovariance state; geometry_msgs::PoseStamped stamped; state.pose2d.x = X_n[0]; state.pose2d.y = X_n[1]; state.pose2d.theta = X_n[2]; stamped.pose.position.x = X_n[0]; stamped.pose.position.y = X_n[1]; stamped.header.seq = 0; stamped.header.stamp = ros::Time::now(); stamped.header.frame_id = "map_static"; state.covariance[0] = P_n[0][0]; state.covariance[1] = P_n[0][1]; state.covariance[2] = P_n[0][2]; state.covariance[3] = P_n[1][0]; state.covariance[4] = P_n[1][1]; state.covariance[5] = P_n[1][2]; state.covariance[6] = P_n[2][0]; state.covariance[7] = P_n[2][1]; state.covariance[8] = P_n[2][2]; std::cout << "xn"<< std::endl; for (int foo = 0; foo < 3; foo++) { std::cout << X_n[foo] << " "; std::cout << "\n"; } std::cout << std::endl; std::cout << "pn"<< std::endl; print3x3(P_n); pub_state_.publish(state); pub_stamp_.publish(stamped); ROS_INFO("pub'd"); kalmanIteration++; ros::spinOnce(); ros::Duration(.7).sleep(); if(!kill) kf(P_n, vel_model_, sense_model_); //keep repeating until base case has been met }