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
}
