I am trying to implement smooth Contact model https://simbody.github.io/3.7.0/classSi ... Force.html and compute GRF after setting the states (q and qdot) and check whether my computed value is consistent with the value reported from OpenSim HuntCrossley Contact model. I am able to get vertical direction close (y-axis) but not in the tangential direction (x and z). I think the way I calculate contact point velocity might be incorrect. However, I cannot figure out what exactly is causing the error. I am attaching my code here. Please let me know what did I do wrong. Although tanh function is used, I don't think this is the reason that causing the difference.
I been trying to debug this for 2 days but still no clue...
Code: Select all
double radius = 0.032; //radius of the contact sphere These parameters are the same in the opensim model
double groundHeight = 0.005;//height of the contact plane
double stiffness = 1000000; //stifness of both sphere and plane
double dissipation = 0.1;
double us = 0.9; //staticFriction
double ud = 0.8; //dynamicFriction
double uv = 0;//viscousFriction
double vt = 1;//transition velocity
double cf = 1e-5;
double bd = 300;
double bv = 50;
SimTK::Vec3 normal(0, 1, 0); //horizontal ground, upward normal vector
Array<double> Fc_R1 = OsimModel.getForceSet().get("Foot_Ground_R1").getRecordValues(defaultStates);
//--------------Start computing simulated GRF--------------------------------//
OsimModel.getSimbodyEngine().getPosition(defaultStates, calcn_r, LocationR1, GlobalR1); // getting sphere origin in global frame
double indentation = groundHeight - (GlobalR1[1] - radius);// computing penatration depth
Vec3 Global_contact_point = GlobalR1;
Global_contact_point[1] = GlobalR1[1] - radius + 1 / 2 * indentation; //adjusting origin of the sphere to actual contact point
Vec3 Local_contact_point;
OsimModel.getSimbodyEngine().transformPosition(defaultStates, ground, Global_contact_point, calcn_r, Local_contact_point);// Transform global contact point to local frame
Vec3 Global_contact_point_velocity;
OsimModel.getSimbodyEngine().getVelocity(defaultStates, calcn_r, Local_contact_point, Global_contact_point_velocity); //find global frame velocity of the contact point
Global_contact_point_velocity = -1*Global_contact_point_velocity;
double vnormal = dot(Global_contact_point_velocity, normal); //getting y directional velocity
Vec3 vtangent = Global_contact_point_velocity - vnormal*normal; //getting tangent velocity vector. I THINK THIS VELOCITY IS WRONG BUT DONT KNOW WHY
// Calculate the Hunt-Crossley force. pretty much the same code in Simbody SmoothSphereHalfSpaceForce
// ----------------smooth version---------------------//
double k = (1. / 2.)*std::pow(stiffness, 2. / 3.);
double fh_pos = (4. / 3.)*k*std::sqrt(radius*k)*
std::pow(std::sqrt(indentation*indentation + cf), 3. / 2.);
double fh_smooth = fh_pos*(1. / 2. + (1. / 2.)*std::tanh(bd*indentation));
double c = dissipation;
double fhc_pos = fh_smooth*(1. + (3. / 2.)*c*vnormal);
double fhc_smooth = fhc_pos*(1. / 2. + (1. / 2.)*std::tanh(
bv*(vnormal + (2. / (3.*c)))));
Vec3 force = fhc_smooth*normal;
// Calculate the friction force.
double aux = vtangent.normSqr() + cf;
double vslip = pow(aux, 1. / 2.);
double vrel = vslip / vt;
double ff = fhc_smooth*(std::min(vrel, Real(1))*(ud + 2 * (us - ud) / (1 + vrel*vrel)) + uv*vslip);
force += ff*(vtangent) / vslip;
cout << "Indentation: " << indentation << endl;
cout << "Global_contact_point_velocity: " << Global_contact_point_velocity[0] << "," << Global_contact_point_velocity[1] << "," << Global_contact_point_velocity[2] << "." << endl;
cout << "Global position of sphere origin: " << endl;
cout << GlobalR1[0] << "," << GlobalR1[1] << "," << GlobalR1[2] << endl;
cout << "Global position of contact point: " << endl;
cout << Global_contact_point[0] << "," << Global_contact_point[1] << "," << Global_contact_point[2] << endl;
cout << "Local position of sphere origin: " << endl;
cout << LocationR1[0] << "," << LocationR1[1] << "," << LocationR1[2] << endl;
cout << "Local position of contact point: " << endl;
cout << Local_contact_point[0] << "," << Local_contact_point[1] << "," << Local_contact_point[2] << endl;
cout << "GRF R1 records: " << Fc_R1[6] << ", " << Fc_R1[7] << ", " << Fc_R1[8] << ". " << endl; // print force acting on the sphere
cout << "Hunt Crossley Contact Force smoothed: " << force[0] << "," << force[1] << "," << force[2] << endl; // printing simulated GRF
cout << "difference: " << force[0] - Fc_R1[6] << "," << force[1] - Fc_R1[7] << "," << force[2] - Fc_R1[8] << endl;// printing the difference between the two
cout << "----------------------------------------------------------------------------------------------" << endl;
Code: Select all
Indentation: 0.015491
Global_contact_point_velocity: -3.02458,2.68598,0.492124.
Global position of sphere origin:
-0.34785,0.021509,0.20513
Global position of contact point:
-0.34785,-0.010491,0.20513
Local position of sphere origin:
0.00190116,-0.021859,-0.0038263
Local position of contact point:
-0.00130414,-0.0535383,-0.000640511
GRF R1 records: -184.643, 228.122, 29.1069.
Hunt Crossley Contact Force smoothed: -190.157,235.164,30.94
difference: -5.51335,7.04205,1.83316