StickyObj::StickyObj(const char * string){
name = string;
geomID = dWebotsGetGeomFromDEF(name);
if(geomID){
if(DEBUG)dWebotsConsolePrintf("%s geom has been found !! ", name);
bodyID = dGeomGetBody(geomID);
dMass dmass;
dBodyGetMass(bodyID, &dmass);
mass = dmass.mass;
}else{
dWebotsConsolePrintf("ERROR %s geom has NOT been found !! ERROR ", name);
}
linkJoint = dBodyGetJoint(bodyID,0);
dJointSetFeedback(linkJoint, &linkJointFeedback);
adhesiveForce = Vector3D(0.0,0.0,0.0);
adheringPoints = 0;
surfaceArea = 0.0;
collidingPoints = 0;
state = 0;
elapsedDetachingTimer = 0;
elapsedAttachingTimer = 0;
rho = 1;
}
void StickyObj::printName(int attach){
if(attach){
dWebotsConsolePrintf("Name: %s attached", name);
}else{
dWebotsConsolePrintf("Name: %s detached", name);
}
}
void StickyObj::printInfos(){
const char * states[4] = {
"detached",
"attached",
"detaching",
"attaching"
};
dWebotsConsolePrintf("** Name: %s ** State: %s -- Area: %f -- AdhePoints: %d, -- rho: %f", name, states[state], surfaceArea,adheringPoints,rho);
//if(isAttached()){
//dWebotsConsolePrintf("AdhesionForce: %f %f %f ", adhesiveForce.getX(), adhesiveForce.getY(), adhesiveForce.getZ());
dWebotsConsolePrintf("JointForce: %f %f %f -- JointTorque: %f %f %f",linkJointForce.getX(),linkJointForce.getY(),linkJointForce.getZ(),linkJointTorque.getX(),linkJointTorque.getY(),linkJointTorque.getZ());
dWebotsConsolePrintf("XLeft --> SigmZ: %f TauX: %f TauY: %f --> VonMises: %f\n",sheerAndStress[0][BOUND/2].getX(), sheerAndStress[0][BOUND/2].getY(),sheerAndStress[0][BOUND/2].getZ() ,vonMisesFunction(sheerAndStress[0][BOUND/2]) );
/*
dWebotsConsolePrintf("XMiddle--> SigmZ: %f TauX: %f TauY: %f --> VonMises: %f",sheerAndStress[BOUND/2][BOUND/2].getX(), sheerAndStress[BOUND/2][BOUND/2].getY(),sheerAndStress[BOUND/2][BOUND/2].getZ() ,vonMisesFunction(sheerAndStress[BOUND/2][BOUND/2]) );
dWebotsConsolePrintf("XRight --> SigmZ: %f TauX: %f TauY: %f --> VonMises: %f\n",sheerAndStress[BOUND-1][BOUND/2].getX(), sheerAndStress[BOUND-1][BOUND/2].getY(),sheerAndStress[BOUND-1][BOUND/2].getZ() ,vonMisesFunction(sheerAndStress[BOUND-1][BOUND/2]) );
//dWebotsConsolePrintf("VonMises XLeft %f XMiddle %f XRight %f",vonMisesFunction(sheerAndStress[1][BOUND/2]), vonMisesFunction(sheerAndStress[BOUND/2][BOUND/2]),vonMisesFunction(sheerAndStress[BOUND-2][BOUND/2]));
dWebotsConsolePrintf("YUp --> SigmZ: %f TauX: %f TauY: %f --> VonMises: %f",sheerAndStress[BOUND/2][0].getX(), sheerAndStress[BOUND/2][0].getY(),sheerAndStress[BOUND/2][0].getZ() ,vonMisesFunction(sheerAndStress[BOUND/2][0]) );
dWebotsConsolePrintf("XMiddle--> SigmZ: %f TauX: %f TauY: %f --> VonMises: %f",sheerAndStress[BOUND/2][BOUND/2].getX(), sheerAndStress[BOUND/2][BOUND/2].getY(),sheerAndStress[BOUND/2][BOUND/2].getZ() ,vonMisesFunction(sheerAndStress[BOUND/2][BOUND/2]) );
dWebotsConsolePrintf("YDown --> SigmZ: %f TauX: %f TauY: %f --> VonMises: %f\n",sheerAndStress[BOUND/2][BOUND-1].getX(), sheerAndStress[BOUND/2][BOUND-1].getY(),sheerAndStress[BOUND/2][BOUND-1].getZ() ,vonMisesFunction(sheerAndStress[BOUND/2][BOUND-1]) );
*/
//}
}
// this function is called by Webots after dWorldStep()
void webots_physics_step_end() {
if (nContacts == 0) {
dWebotsConsolePrintf("no contact\n");
return;
}
int i;
for (i = 0; i < nContacts; i++) {
// printf force and torque that each contact joint
// applies on the box's body
dWebotsConsolePrintf("contact: %d:\n", i);
print_vec3("f1", feedbacks[i].f1);
print_vec3("t1", feedbacks[i].t1);
print_vec3("f2", feedbacks[i].f2);
print_vec3("t2", feedbacks[i].t2);
dWebotsConsolePrintf("\n");
}
}
/*
* Test if adhesions points should unattach due to (pulling) forces (vs AdhesionForce)
* Two different case : if stress pushing or stress pulling
* Update the number of points attached (and by effet de bord, the AdhesionForce)
* Return 1 if detached , else 0
*/
int StickyObj::testAdhesionEfficiency(){
//For each adhesionPoint:
// if (pull force + stress)*func(sheer) > Fadhesion --> then detach this point
dReal lengX = absdReal(2*collidingPointPos[0].getX()); // approximate the width of the conctact surfac with one point (should be done with all points ..)
dReal lengZ = absdReal(2*collidingPointPos[0].getZ()); // approximate the height of the conctact surfac with one point (should be done with all points ..)
dReal stepX = lengX/BOUND;
dReal stepZ = lengZ/BOUND;
dReal dispX = -lengX/2+stepX/2;
dReal dispZ = -lengZ/2+stepZ/2;
rho= adheringPoints/MAX_ADHESIONPOINTS;
dReal sigmaVonMisesThreshold = 200*ADHESION_FORCE_FACTOR*surfaceArea*rho; // = 1000 exp data from Webots
dReal thetaVonMisesThreshold = 50*ADHESION_FORCE_FACTOR*surfaceArea*rho; // guessed value
dReal mu = 0.5;
int adhePoints = MAX_ADHESIONPOINTS;
dReal max =0.0;
dReal maxTemp;
int i;
int j;
for(i=0; i< BOUND; i++){
for(j=0; j<BOUND;j++){
sheerAndStress[i][j] = computeSheerAndStress(dispX+stepX*i ,dispZ+stepZ*j);
maxTemp = 0.01*vonMisesFunction(sheerAndStress[i][j]);
if(sheerAndStress[i][j].getX() > 0){
if(maxTemp > sigmaVonMisesThreshold){
adhePoints--;
}
}else{
if(maxTemp > mu*-1*sheerAndStress[i][j].getX()*rho + thetaVonMisesThreshold){
adhePoints--;
}
}
if(maxTemp > max){
max = maxTemp;
}
}
}
adheringPoints = adhePoints;
if(adheringPoints == 0){
dWebotsConsolePrintf("Detached due to forces");
printInfos();
detach();
dJointDestroy(adhesionJoint);
return 1;
}
return 0;
}
// convenience function to print a 3d vector
void print_vec3(const char *msg, const dVector3 v) {
dWebotsConsolePrintf("%s: %g %g %g\n", msg, v[0], v[1], v[2]);
}
// convenience function to find ODE body by its DEF name in the .wbt file
dBodyID getBody(const char *def) {
dBodyID body = dWebotsGetBodyFromDEF(def);
if (!body)
dWebotsConsolePrintf("Warning: did not find body with DEF name: %s", def);
return body;
}
// convenience function to find ODE geometry by its DEF name in the .wbt file
dGeomID getGeom(const char *def) {
dGeomID geom = dWebotsGetGeomFromDEF(def);
if (!geom)
dWebotsConsolePrintf("Warning: did not find geometry with DEF name: %s", def);
return geom;
}