static dJointID find_shared_joint(dBodyID body1, dBodyID body2)
{
dJointID joint = 0;
if (body1 == 0)
{
if (body2 == 0)
return 0;
else
return find_shared_joint(body2, body1);
}
else
{
int i, n = dBodyGetNumJoints(body1);
for (i = 0; i < n; ++i)
if ((joint = dBodyGetJoint(body1, i)))
{
if (dJointGetBody(joint, 0) == body2 ||
dJointGetBody(joint, 1) == body2) return joint;
}
return 0;
}
}
void CPHFracturesHolder::PhTune(dBodyID body)
{
//iterate through all body's joints and set joints feedbacks where is not already set
//contact feedbacks stored in global storage - ContactFeedBacks wich cleared on each step
//breacable joints already has their feedbacks,
//feedbacks for rest noncontact joints stored in m_feedbacks in runtime in this function and
//and killed by destructor
//int dBodyGetNumJoints (dBodyID b);
//dJointID dBodyGetJoint (dBodyID, int index);
//dJointGetType
//dJointTypeContact
int num=dBodyGetNumJoints(body);
for(int i=0;i<num;++i)
{
dJointID joint=dBodyGetJoint(body,i);
if(dJointGetType(joint)==dJointTypeContact)
{
dJointSetFeedback(joint,ContactFeedBacks.add());
}
else
{
CPHJoint* ph_joint=(CPHJoint*)dJointGetData(joint);
if(!(ph_joint&&ph_joint->JointDestroyInfo())) dJointSetFeedback(joint,ContactFeedBacks.add());
//if(!dJointGetFeedback(joint))
//{
// m_feedbacks.push_back(dJointFeedback());
// dJointSetFeedback(joint,&m_feedbacks.back());
//}
}
}
}
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;
}
Joint* internal_getCommonJoint(dBodyID body0, dBodyID body1)
{
// First we need to find the ODE joint connecting the bodies
// (it would be ideal if ODE included this functionality...).
// We only need to check one of the bodies' ODE joints
// because it is assumed here that the two bodies are
// connected, thus they should have a common ODE joint.
int numJoints0 = dBodyGetNumJoints(body0);
dJointID theJoint = 0;
// Loop through body0's ODE joints.
int i = 0;
for (i = 0; i < numJoints0; ++i)
{
dJointID currentJoint = dBodyGetJoint(body0, i);
dBodyID jointBody0 = dJointGetBody(currentJoint, 0);
dBodyID jointBody1 = dJointGetBody(currentJoint, 1);
if ((jointBody0 == body0 && jointBody1 == body1) ||
(jointBody0 == body1 && jointBody1 == body0))
{
// This is the ODE joint connecting the two bodies.
// Note that if the two bodies are connected by multiple
// Joints, the behavior is undefined.
theJoint = currentJoint;
}
}
// Make sure the ODE joint was actually found. This should
// be guaranteed.
assert(theJoint);
// Now return the associated OPAL Joint.
return (Joint*) dJointGetData(theJoint);
}
void BoxObstacle::remove() {
dGeomDestroy(boxGeom_);
if (box_ != 0) {
for(int i=0; i< dBodyGetNumJoints(box_); i++) {
dJointDestroy(dBodyGetJoint(box_, i));
}
dBodyDestroy(box_);
}
}
bool HasContact(dBodyID a)
{
if(a == 0) return false;
int n = dBodyGetNumJoints (a);
for(int i=0;i<n;i++) {
dJointID j=dBodyGetJoint (a,i);
if(dJointGetType(j)==dJointTypeContact) return true;
}
return false;
}
virtual void PhTune(dReal step)
{
InitValues();
int num=dBodyGetNumJoints(m_body);
for(int i=0;i<num;++i)
{
dJointID joint=dBodyGetJoint(m_body,i);
if(dJointGetType(joint)==dJointTypeContact)
{
dJointSetFeedback(joint,ContactFeedBacks.add());
}
}
}
bool HasContact(dBodyID a,dBodyID b)
{
if(a == 0 && b == 0) return false; //two environments
if(a == 0) Swap(a,b);
int n = dBodyGetNumJoints (a);
for(int i=0;i<n;i++) {
dJointID j=dBodyGetJoint (a,i);
if(dJointGetType(j)==dJointTypeContact) {
dBodyID j0=dJointGetBody(j,0);
dBodyID j1=dJointGetBody(j,1);
if(j0 == b || j1 == b) return true;
}
}
return false;
}
virtual void PhDataUpdate(dReal step)
{
int num=dBodyGetNumJoints(m_body);
for(int i=0;i<num;i++)
{
dJointID joint=dBodyGetJoint(m_body,i);
if(dJointGetType(joint)==dJointTypeContact)
{
dJointFeedback* feedback=dJointGetFeedback(joint);
R_ASSERT2(feedback,"Feedback was not set!!!");
dxJoint* b_joint=(dxJoint*) joint;
dBodyID other_body=b_joint->node[1].body;
bool b_body_second=(b_joint->node[1].body==m_body);
dReal* self_force=feedback->f1;
dReal* self_torque=feedback->t1;
dReal* othrers_force=feedback->f2;
dReal* othrers_torque=feedback->t2;
if(b_body_second)
{
other_body=b_joint->node[0].body;
self_force=feedback->f2;
self_torque=feedback->t2;
othrers_force=feedback->f1;
othrers_torque=feedback->t1;
}
save_max(m_max_force_self,_sqrt(dDOT( self_force,self_force)));
save_max(m_max_torque_self,_sqrt(dDOT( self_torque,self_torque)));
save_max(m_max_force_self_y,_abs(self_force[1]));
save_max(m_max_force_self_sd,_sqrt(self_force[0]*self_force[0]+self_force[2]*self_force[2]));
if(other_body)
{
dVector3 shoulder;dVectorSub(shoulder,dJointGetPositionContact(joint),dBodyGetPosition(other_body));
dReal shoulder_lenght=_sqrt(dDOT(shoulder,shoulder));
save_max(m_max_force_others,_sqrt(dDOT( othrers_force,othrers_force)));
if(!fis_zero(shoulder_lenght))
save_max(m_max_torque_others,_sqrt(dDOT( othrers_torque,othrers_torque))/shoulder_lenght);
}
}
}
}
static void drawGeom(dGeomID geomID)
{
// printf("1%lu", (uint64_t)geomID);
int gclass = dGeomGetClass(geomID);
// printf("2\n");
const dReal *pos = NULL;
const dReal *rot = NULL;
bool canDrawJoints = false;
ODEUserObject* userObj = (ODEUserObject*)dGeomGetData(geomID);
if (nullptr != userObj) {
dsSetColorAlpha(1, 1, 1, 1);
// printf("%f %f %f %f\n", userObj->colorVec[0], userObj->colorVec[1], userObj->colorVec[2], userObj->colorVec[3]);
dsSetTexture (userObj->textureNum);
dsSetColorAlpha(userObj->m_colorVec[0], userObj->m_colorVec[1], userObj->m_colorVec[2], userObj->m_colorVec[3]);
} else {
dsSetColorAlpha(1, 1, 0, 1);
dsSetTexture (DS_WOOD);
}
switch (gclass) {
case dSphereClass:
if (nullptr != userObj && userObj->visible) {
pos = dGeomGetPosition(geomID);
rot = dGeomGetRotation(geomID);
dsDrawSphere(pos, rot, dGeomSphereGetRadius(geomID));
}
canDrawJoints = true;
break;
case dBoxClass:
{
if (nullptr != userObj && userObj->visible) {
pos = dGeomGetPosition(geomID);
rot = dGeomGetRotation(geomID);
dVector3 lengths;
dGeomBoxGetLengths(geomID, lengths);
dsDrawBox(pos, rot, lengths);
}
canDrawJoints = true;
break;
}
case dCylinderClass:
{
if (nullptr != userObj && userObj->visible) {
pos = dGeomGetPosition(geomID);
rot = dGeomGetRotation(geomID);
dReal length;
dReal radius;
dGeomCylinderGetParams(geomID, &radius, &length);
dsDrawCylinder(pos, rot, length, radius);
}
canDrawJoints = true;
break;
}
default:
break;
}
// printf("class: %d\n", gclass);
if (canDrawJoints) {
#ifdef DRAW_JOINTS_TOO
dBodyID body = dGeomGetBody(geomID);
int numJoints = dBodyGetNumJoints(body);
for (int i = 0; i < numJoints; ++i)
{
dJointID joint = dBodyGetJoint(body, i);
int jointClass = dJointGetType(joint);
switch (jointClass)
{
case dJointTypeHinge:
{
dVector3 a11;
dBodyID body1 = dJointGetBody(joint, 0);
dBodyID body2 = dJointGetBody(joint, 1);
if (body1 && body2) {
const dReal* bodyPos1 = dBodyGetPosition(body1);
const dReal* bodyPos2 = dBodyGetPosition(body2);
dJointGetHingeAnchor(joint, a11);
dsSetColor(1, 0, 0);
dsDrawLine(a11, bodyPos1);
dsDrawLine(a11, bodyPos2);
dsSetColor(0, 1, 0);
dsDrawLine(bodyPos1, bodyPos2);
}
}
}
}
#endif
}
}
void SParts::calcPosition(Joint *currj, Joint *nextj, const Vector3d &anchorv, const Rotation &R)
{
if (currj) {
DUMP(("currj = %s\n", currj->name()));
}
DUMP(("name = %s\n", name()));
if (nextj) {
DUMP(("nextj = %s\n", nextj->name()));
}
DUMP(("anchorv = (%f, %f, %f)\n", anchorv.x(), anchorv.y(), anchorv.z()));
{
Vector3d v(m_pos.x(), m_pos.y(), m_pos.z());
// Calculate shift vector from positoin/anchor/orientation
if (currj) {
v -= currj->getAnchor();
DUMP(("parent anchor = %s (%f, %f, %f)\n",
currj->name(),
currj->getAnchor().x(),
currj->getAnchor().y(), currj->getAnchor().z()));
}
DUMP(("v1 = (%f, %f, %f)\n", v.x(), v.y(), v.z()));
Rotation rr(R);
if (currj) {
rr *= currj->getRotation();
}
v.rotate(rr);
DUMP(("v2 = (%f, %f, %f)\n", v.x(), v.y(), v.z()));
v += anchorv;
DUMP(("v3 = (%f, %f, %f)\n", v.x(), v.y(), v.z()));
//DUMP(("v4 = (%f, %f, %f)\n", v.x(), v.y(), v.z()));
setPosition(v);
if(m_onGrasp) {
dBodyID body = odeobj().body();
//int num = dBodyGetNumJoints(body);
// get grasp joint
dJointID joint = dBodyGetJoint(body, 0);
// get target body from joint
dBodyID targetBody = dJointGetBody(joint, 1);
// get position of grasp target and parts
const dReal *pos = dBodyGetPosition(body);
const dReal *tpos = dBodyGetPosition(targetBody);
// Set if it is moved
if(pos[0] != tpos[0] || pos[1] != tpos[1] || pos[2] != tpos[2]) {
dBodySetPosition(targetBody, pos[0], pos[1], pos[2]);
}
// get position of grasp target and parts
const dReal *qua = dBodyGetQuaternion(body);
const dReal *tqua = dBodyGetQuaternion(targetBody);
// rotation from initial orientation
dQuaternion rot;
dQMultiply2(rot, qua, m_gini);
// Set if it is rotated
if(rot[0] != tqua[0] || rot[1] != tqua[1] || rot[2] != tqua[2] || rot[3] != tqua[3]) {
dBodySetQuaternion(targetBody, rot);
}
//LOG_MSG(("target2 %d", targetBody));
//LOG_MSG(("(%f, %f, %f)", tpos[0], tpos[1], tpos[2]));
//LOG_MSG(("onGrasp!! num = %d, joint = %d", num));
}
}
Rotation rr(R);
rr *= m_rot;
if (currj) {
rr *= currj->getRotation();
}
setRotation(rr);
if (!nextj) { return; }
for (ChildC::iterator i=m_children.begin(); i!=m_children.end(); i++) {
Child *child = *i;
Rotation R_(R);
if (currj) {
R_ *= currj->getRotation();
}
Vector3d nextv;
nextv = nextj->getAnchor();
if (currj) {
nextv -= currj->getAnchor();
}
nextv.rotate(R_);
nextv += anchorv;
DUMP(("nextv1 : (%f, %f, %f)\n", nextv.x(), nextv.y(), nextv.z()));
// act in a case that multiple JOINTs are connected to one link
if(strcmp(nextj->name(),child->currj->name()) == 0) {
child->nextp->calcPosition(child->currj, child->nextj, nextv, R_);
}
}
}
dJointID PhysicsBody::getJoint(Int32 index) const
{
return dBodyGetJoint(_BodyID, index);
}
//#define DBG_BREAK
bool CPHFracture::Update(CPHElement* element)
{
////itterate through impacts & calculate
dBodyID body=element->get_body();
//const Fvector& v_bodyvel=*((Fvector*)dBodyGetLinearVel(body));
CPHFracturesHolder* holder=element->FracturesHolder();
PH_IMPACT_STORAGE& impacts=holder->Impacts();
Fvector second_part_force,first_part_force,second_part_torque,first_part_torque;
second_part_force.set(0.f,0.f,0.f);
first_part_force.set(0.f,0.f,0.f);
second_part_torque.set(0.f,0.f,0.f);
first_part_torque.set(0.f,0.f,0.f);
//const Fvector& body_local_pos=element->local_mass_Center();
const Fvector& body_global_pos=*(const Fvector*)dBodyGetPosition(body);
Fvector body_to_first, body_to_second;
body_to_first.set(*((const Fvector*)m_firstM.c));//,body_local_pos
body_to_second.set(*((const Fvector*)m_secondM.c));//,body_local_pos
//float body_to_first_smag=body_to_first.square_magnitude();
//float body_to_second_smag=body_to_second.square_magnitude();
int num=dBodyGetNumJoints(body);
for(int i=0;i<num;i++)
{
bool applied_to_second=false;
dJointID joint=dBodyGetJoint(body,i);
dJointFeedback* feedback=dJointGetFeedback(joint);
VERIFY2(feedback,"Feedback was not set!!!");
dxJoint* b_joint=(dxJoint*) joint;
bool b_body_second=(b_joint->node[1].body==body);
Fvector joint_position;
if(dJointGetType(joint)==dJointTypeContact)
{
dxJointContact* c_joint=(dxJointContact*)joint;
dGeomID first_geom=c_joint->contact.geom.g1;
dGeomID second_geom=c_joint->contact.geom.g2;
joint_position.set(*(Fvector*)c_joint->contact.geom.pos);
if(dGeomGetClass(first_geom)==dGeomTransformClass)
{
first_geom=dGeomTransformGetGeom(first_geom);
}
if(dGeomGetClass(second_geom)==dGeomTransformClass)
{
second_geom=dGeomTransformGetGeom(second_geom);
}
dxGeomUserData* UserData;
UserData=dGeomGetUserData(first_geom);
if(UserData)
{
u16 el_position=UserData->element_position;
//define if the contact applied to second part;
if(el_position<element->numberOfGeoms()&&
el_position>=m_start_geom_num&&
el_position<m_end_geom_num&&
first_geom==element->Geom(el_position)->geometry()
) applied_to_second=true;
}
UserData=dGeomGetUserData(second_geom);
if(UserData)
{
u16 el_position=UserData->element_position;
if(el_position<element->numberOfGeoms()&&
el_position>=m_start_geom_num&&
el_position<m_end_geom_num&&
second_geom==element->Geom(el_position)->geometry()
) applied_to_second=true;
}
}
else
{
CPHJoint* J = (CPHJoint*) dJointGetData(joint);
if(!J)continue;//hack..
J->PSecondElement()->InterpolateGlobalPosition(&joint_position);
CODEGeom* root_geom=J->RootGeom();
if(root_geom)
{
u16 el_position=root_geom->element_position();
if(element==J->PFirst_element()&&
el_position<element->numberOfGeoms()&&
el_position>=m_start_geom_num&&
el_position<m_end_geom_num
) applied_to_second=true;
}
}
//accomulate forces applied by joints to first and second parts
Fvector body_to_joint;
body_to_joint.sub(joint_position,body_global_pos);
if(applied_to_second)
{
Fvector shoulder;
shoulder.sub(body_to_joint,body_to_second);
if(b_body_second)
{
Fvector joint_force;
joint_force.set(*(const Fvector*)feedback->f2);
second_part_force.add(joint_force);
Fvector torque;
torque.crossproduct(shoulder,joint_force);
second_part_torque.add(torque);
}
else
{
Fvector joint_force;
joint_force.set(*(const Fvector*)feedback->f1);
second_part_force.add(joint_force);
Fvector torque;
torque.crossproduct(shoulder,joint_force);
second_part_torque.add(torque);
}
}
else
{
Fvector shoulder;
shoulder.sub(body_to_joint,body_to_first);
if(b_body_second)
{
Fvector joint_force;
joint_force.set(*(const Fvector*)feedback->f2);
first_part_force.add(joint_force);
Fvector torque;
torque.crossproduct(shoulder,joint_force);
first_part_torque.add(torque);
}
else
{
Fvector joint_force;
joint_force.set(*(const Fvector*)feedback->f1);
first_part_force.add(joint_force);
Fvector torque;
torque.crossproduct(shoulder,joint_force);
first_part_torque.add(torque);
}
}
}
PH_IMPACT_I i_i=impacts.begin(),i_e=impacts.end();
for(;i_i!=i_e;i_i++)
{
u16 geom = i_i->geom;
if((geom>=m_start_geom_num&&geom<m_end_geom_num))
{
Fvector force;
force.set(i_i->force);
force.mul(ph_console::phRigidBreakWeaponFactor);
Fvector second_to_point;
second_to_point.sub(body_to_second,i_i->point);
//force.mul(30.f);
second_part_force.add(force);
Fvector torque;
torque.crossproduct(second_to_point,force);
second_part_torque.add(torque);
}
else
{
Fvector force;
force.set(i_i->force);
Fvector first_to_point;
first_to_point.sub(body_to_first,i_i->point);
//force.mul(4.f);
first_part_force.add(force);
Fvector torque;
torque.crossproduct(first_to_point,force);
second_part_torque.add(torque);
}
}
Fvector gravity_force;
gravity_force.set(0.f,-ph_world->Gravity()*m_firstM.mass,0.f);
first_part_force.add(gravity_force);
second_part_force.add(gravity_force);
dMatrix3 glI1,glI2,glInvI,tmp;
// compute inertia tensors in global frame
dMULTIPLY2_333 (tmp,body->invI,body->R);
dMULTIPLY0_333 (glInvI,body->R,tmp);
dMULTIPLY2_333 (tmp,m_firstM.I,body->R);
dMULTIPLY0_333 (glI1,body->R,tmp);
dMULTIPLY2_333 (tmp,m_secondM.I,body->R);
dMULTIPLY0_333 (glI2,body->R,tmp);
//both parts have eqiual start angular vel same as have body so we ignore it
//compute breaking torque
///break_torque=glI2*glInvI*first_part_torque-glI1*glInvI*second_part_torque+crossproduct(second_in_bone,second_part_force)-crossproduct(first_in_bone,first_part_force)
Fvector break_torque,vtemp;
dMULTIPLY0_331 ((float*)&break_torque,glInvI,(float*)&first_part_torque);
dMULTIPLY0_331 ((float*)&break_torque,glI2,(float*)&break_torque);
dMULTIPLY0_331 ((float*)&vtemp,glInvI,(float*)&second_part_torque);
dMULTIPLY0_331 ((float*)&vtemp,glI1,(float*)&vtemp);
break_torque.sub(vtemp);
//Fvector first_in_bone,second_in_bone;
//first_in_bone.sub(*((const Fvector*)m_firstM.c),m_pos_in_element);
//second_in_bone.sub(*((const Fvector*)m_secondM.c),m_pos_in_element);
//vtemp.crossproduct(second_in_bone,second_part_force);
//break_torque.add(vtemp);
//vtemp.crossproduct(first_in_bone,first_part_force);
//break_torque.sub(vtemp);
#ifdef DBG_BREAK
float btm_dbg=break_torque.magnitude()*ph_console::phBreakCommonFactor/torque_factor;
#endif
if(break_torque.magnitude()*ph_console::phBreakCommonFactor>m_break_torque*torque_factor)
{
//m_break_torque.set(second_part_torque);
m_pos_in_element.set(second_part_force);
m_break_force=second_part_torque.x;
m_break_torque=second_part_torque.y;
m_add_torque_z=second_part_torque.z;
m_breaked=true;
#ifndef DBG_BREAK
return m_breaked;
#endif
}
Fvector break_force;//=1/(m1+m2)*(F1*m2-F2*m1)+r2xT2/(r2^2)-r1xT1/(r1^2)
break_force.set(first_part_force);
break_force.mul(m_secondM.mass);
vtemp.set(second_part_force);
vtemp.mul(m_firstM.mass);
break_force.sub(vtemp);
break_force.mul(1.f/element->getMass());//element->getMass()//body->mass.mass
//vtemp.crossproduct(second_in_bone,second_part_torque);
//break_force.add(vtemp);
//vtemp.crossproduct(first_in_bone,first_part_torque);
//break_force.sub(vtemp);
float bfm=break_force.magnitude()*ph_console::phBreakCommonFactor;
if(m_break_force<bfm)
{
second_part_force.mul(bfm/m_break_force);
m_pos_in_element.set(second_part_force);
//m_pos_in_element.add(break_force);
m_break_force=second_part_torque.x;
m_break_torque=second_part_torque.y;
m_add_torque_z=second_part_torque.z;
m_breaked=true;
#ifndef DBG_BREAK
return m_breaked;
#endif
}
#ifdef DBG_BREAK
Msg("bone_id %d break_torque - %f(max %f) break_force %f (max %f) breaked %d",m_bone_id,btm_dbg,m_break_torque,bfm,m_break_force,m_breaked);
#endif
return m_breaked;
}
void doStuff(const std::string & prefix,amarsi::Limbs limb){
std::ostringstream name;
name<<prefix<<"_TOE";
dBodyID toeBody=dWebotsGetBodyFromDEF(name.str().c_str());
dGeomID toe=dWebotsGetGeomFromDEF(name.str().c_str());
//std::cerr<<"Get Geom of value "<<toe<<std::endl;
if(!toe || !toeBody){
std::cerr<<"Did not found "<<name<<" "<<toe<<" "<<toeBody<<std::endl;
s_data->disablePhysics();
return;
}
std::ostringstream nameTibia;
nameTibia<<prefix<<"_FRONT_KNEE";
dBodyID tibia = dWebotsGetBodyFromDEF(nameTibia.str().c_str());
if(!tibia){
std::cerr<<"Did not found "<<nameTibia<<std::endl;
s_data->disablePhysics();
return;
}
// std::cerr<<nameTibia<<" "<<tibia<<std::endl;
int numJoint=dBodyGetNumJoints(tibia);
std::ostringstream nameFemur;
nameFemur<<prefix<<"_HIP_SERVO";
dBodyID femur = dWebotsGetBodyFromDEF(nameFemur.str().c_str());
if(!femur){
std::cerr<<"Did not found "<<nameFemur<<std::endl;
s_data->disablePhysics();
return;
}
for(int i=0;i<numJoint;++i){
dJointID j=dBodyGetJoint(tibia,i);
dBodyID b1= dJointGetBody(j,0);
dBodyID b2= dJointGetBody(j,1);
dReal pos[4];
if(b1==femur || b2==femur){
if(b1==femur)
dJointGetHingeAnchor2(j,pos);
else
dJointGetHingeAnchor(j,pos);
dVector3 posRel;
dBodyGetPosRelPoint(tibia,pos[0],pos[1],pos[3],posRel);
// std::cerr<<"pos : "<<posRel[0]<<" "<<posRel[1]<<" "<<posRel[2]<<std::endl;
// s_data->addKneeJoint(j,limb);
}
}
std::ostringstream nameAnkle;
nameAnkle<<prefix<<"_ANKLE"<<std::flush;
dBodyID ankleBody = dWebotsGetBodyFromDEF(nameAnkle.str().c_str());
dGeomID ankle=dWebotsGetGeomFromDEF(nameAnkle.str().c_str());
if(!ankle || !ankleBody){
std::cerr<<"Did not found "<<nameAnkle<<" "<<ankle<<" "
<<ankleBody<<std::endl;
s_data->disablePhysics();
return;
}
insertGeomInMonitored(ankle,limb);
insertGeomInMonitored(toe,limb);
s_data->addResettableBody(femur);
s_data->addResettableBody(tibia);
s_data->addResettableBody(ankleBody);
s_data->addResettableBody(toeBody);
}
