Example #1
0
dReal ServoMotor::getTorque() {
	// code from Jeff Shim

	osg::Vec3 torque1(fback_.t1[0], fback_.t1[1], fback_.t1[2] );
	osg::Vec3 torque2(fback_.t2[0], fback_.t2[1], fback_.t2[2] );
	osg::Vec3 force1(fback_.f1[0], fback_.f1[1], fback_.f1[2] );
	osg::Vec3 force2(fback_.f2[0], fback_.f2[1], fback_.f2[2] );

	const double* p1 = dBodyGetPosition( dJointGetBody(joint_->getJoint(),0) );
	const double* p2 = dBodyGetPosition( dJointGetBody(joint_->getJoint(),1) );

	osg::Vec3 pos1(p1[0], p1[1], p1[2]);
	osg::Vec3 pos2(p2[0], p2[1], p2[2]);


	dVector3 odeAnchor;
	dJointGetHingeAnchor ( joint_->getJoint(), odeAnchor );
	osg::Vec3 anchor(odeAnchor[0], odeAnchor[1], odeAnchor[2]);


	osg::Vec3 ftorque1 = torque1 - (force1^(pos1-anchor));// torq by motor = total torq - constraint torq
	osg::Vec3 ftorque2 = torque2 - (force2^(pos2-anchor));// opposite direction - use if this is necessary

	dVector3 odeAxis;
	dJointGetHingeAxis ( joint_->getJoint(), odeAxis);
	osg::Vec3 axis(odeAxis[0], odeAxis[1], odeAxis[2] );
	axis.normalize();

	double torque =  ftorque1 * axis;

	//printf ("torque: % 1.10f\n", torque);
	return torque;
}
Example #2
0
void
dxJointHinge::setRelativeValues()
{
    dVector3 vec;
    dJointGetHingeAnchor(this, vec);
    setAnchors( this, vec[0], vec[1], vec[2], anchor1, anchor2 );

    dJointGetHingeAxis(this, vec);
    setAxes( this,  vec[0], vec[1], vec[2], axis1, axis2 );
    computeInitialRelativeRotation();
}
Example #3
0
static void get_phys_joint_anchor(dJointID j, float *v)
{
    dVector3 V = { 0, 0, 0 };

    switch (dJointGetType(j))
    {
    case dJointTypeBall:      dJointGetBallAnchor     (j, V); break;
    case dJointTypeHinge:     dJointGetHingeAnchor    (j, V); break;
    case dJointTypeHinge2:    dJointGetHinge2Anchor   (j, V); break;
    case dJointTypeUniversal: dJointGetUniversalAnchor(j, V); break;
    default: break;
    }

    v[0] = (float) V[0];
    v[1] = (float) V[1];
    v[2] = (float) V[2];
}
Example #4
0
/**
*@brief 位置更新の関数
*/
void ODEJointObj::UpdateJointPosition()
{
	ms->mu->lock();
	if(JointType == 0)
	{
		dVector3 P;
		dJointGetSliderAxis(joint, P);

	}
	else if(JointType == 2)
	{
		dVector3 P;
		dJointGetHingeAnchor(joint, P);
		SetPosition(bscale_x*P[0] + offx, bscale_y*P[1] + offy, bscale_z*P[2] + offz);
		dJointGetHingeAxis(joint, P);

	}
	ms->mu->unlock();
}
Example #5
0
//! Refer the current anchor position in world coordinate
// added by noma on 2012-02-27
Vector3d HingeJoint::getCurrentAnchorPosition()
{
	dReal tmpPos[4];
	dJointGetHingeAnchor(m_joint, tmpPos);
	return Vector3d(tmpPos[0],tmpPos[1],tmpPos[2]);
}
Example #6
0
soyatomsPosition* soy_joints_hinge_get_anchor (soyjointsHinge* self) {
	soyatomsPosition* result;
	dxVector3* _tmp0_;
	dxVector3* v;
	struct dxJoint* _tmp1_;
	dxVector3* _tmp2_;
	soyatomsPosition* _tmp3_;
	soyatomsPosition* _tmp4_;
	soyatomsPosition* value;
	gboolean _tmp5_ = FALSE;
	gboolean _tmp6_ = FALSE;
	gboolean _tmp7_ = FALSE;
	soyatomsPosition* _tmp8_;
	gboolean _tmp14_;
	gboolean _tmp20_;
	gboolean _tmp26_;
	g_return_val_if_fail (self != NULL, NULL);
	_tmp0_ = dvector3_new ();
	v = _tmp0_;
	_tmp1_ = ((soyjointsJoint*) self)->joint;
	_tmp2_ = v;
	dJointGetHingeAnchor ((struct dxJoint*) _tmp1_, _tmp2_);
	_tmp3_ = self->priv->_anchor_obj;
	_tmp4_ = _g_object_ref0 (_tmp3_);
	value = _tmp4_;
	_tmp8_ = value;
	if (_tmp8_ == NULL) {
		_tmp7_ = TRUE;
	} else {
		dxVector3* _tmp9_;
		dReal _tmp10_;
		soyatomsPosition* _tmp11_;
		gfloat _tmp12_;
		gfloat _tmp13_;
		_tmp9_ = v;
		_tmp10_ = _tmp9_->x;
		_tmp11_ = value;
		_tmp12_ = soy_atoms_position_get_x (_tmp11_);
		_tmp13_ = _tmp12_;
		_tmp7_ = ((gfloat) _tmp10_) != _tmp13_;
	}
	_tmp14_ = _tmp7_;
	if (_tmp14_) {
		_tmp6_ = TRUE;
	} else {
		dxVector3* _tmp15_;
		dReal _tmp16_;
		soyatomsPosition* _tmp17_;
		gfloat _tmp18_;
		gfloat _tmp19_;
		_tmp15_ = v;
		_tmp16_ = _tmp15_->y;
		_tmp17_ = value;
		_tmp18_ = soy_atoms_position_get_y (_tmp17_);
		_tmp19_ = _tmp18_;
		_tmp6_ = ((gfloat) _tmp16_) != _tmp19_;
	}
	_tmp20_ = _tmp6_;
	if (_tmp20_) {
		_tmp5_ = TRUE;
	} else {
		dxVector3* _tmp21_;
		dReal _tmp22_;
		soyatomsPosition* _tmp23_;
		gfloat _tmp24_;
		gfloat _tmp25_;
		_tmp21_ = v;
		_tmp22_ = _tmp21_->z;
		_tmp23_ = value;
		_tmp24_ = soy_atoms_position_get_z (_tmp23_);
		_tmp25_ = _tmp24_;
		_tmp5_ = ((gfloat) _tmp22_) != _tmp25_;
	}
	_tmp26_ = _tmp5_;
	if (_tmp26_) {
		soyatomsPosition* _tmp27_;
		dxVector3* _tmp31_;
		dReal _tmp32_;
		dxVector3* _tmp33_;
		dReal _tmp34_;
		dxVector3* _tmp35_;
		dReal _tmp36_;
		soyatomsPosition* _tmp37_;
		soyatomsPosition* _tmp38_;
		soyatomsPosition* _tmp39_;
		soyatomsPosition* _tmp40_;
		_tmp27_ = value;
		if (_tmp27_ != NULL) {
			soyatomsPosition* _tmp28_;
			guint _tmp29_ = 0U;
			soyatomsPosition* _tmp30_;
			_tmp28_ = self->priv->_anchor_obj;
			g_signal_parse_name ("on-set", SOY_ATOMS_TYPE_POSITION, &_tmp29_, NULL, FALSE);
			g_signal_handlers_disconnect_matched (_tmp28_, G_SIGNAL_MATCH_ID | G_SIGNAL_MATCH_FUNC | G_SIGNAL_MATCH_DATA, _tmp29_, 0, NULL, (GCallback) __soy_joints_hinge_anchor_set_soy_atoms_position_on_set, self);
			_tmp30_ = self->priv->_anchor_obj;
			g_object_weak_unref ((GObject*) _tmp30_, __soy_joints_hinge_anchor_weak_gweak_notify, self);
		}
		_tmp31_ = v;
		_tmp32_ = _tmp31_->x;
		_tmp33_ = v;
		_tmp34_ = _tmp33_->y;
		_tmp35_ = v;
		_tmp36_ = _tmp35_->z;
		_tmp37_ = soy_atoms_position_new ((gfloat) _tmp32_, (gfloat) _tmp34_, (gfloat) _tmp36_);
		_g_object_unref0 (value);
		value = _tmp37_;
		_tmp38_ = value;
		g_signal_connect_object (_tmp38_, "on-set", (GCallback) __soy_joints_hinge_anchor_set_soy_atoms_position_on_set, self, 0);
		_tmp39_ = value;
		g_object_weak_ref ((GObject*) _tmp39_, __soy_joints_hinge_anchor_weak_gweak_notify, self);
		_tmp40_ = value;
		self->priv->_anchor_obj = _tmp40_;
	}
	result = value;
	_dvector3_free0 (v);
	return result;
}
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
    }
}
Example #8
0
soyatomsPosition* soy_joints_hinge_get_anchor (soyjointsHinge* self) {
	soyatomsPosition* result;
	dxVector3* v = NULL;
	dxVector3* _tmp0_ = NULL;
	struct dxJoint* _tmp1_ = NULL;
	dxVector3* _tmp2_ = NULL;
	soyatomsPosition* value = NULL;
	soyatomsPosition* _tmp3_ = NULL;
	soyatomsPosition* _tmp4_ = NULL;
	gboolean _tmp5_ = FALSE;
	gboolean _tmp6_ = FALSE;
	gboolean _tmp7_ = FALSE;
	soyatomsPosition* _tmp8_ = NULL;
	gboolean _tmp14_ = FALSE;
	gboolean _tmp20_ = FALSE;
	gboolean _tmp26_ = FALSE;
#line 62 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	g_return_val_if_fail (self != NULL, NULL);
#line 63 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	_tmp0_ = dvector3_new ();
#line 63 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	v = _tmp0_;
#line 64 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	_tmp1_ = ((soyjointsJoint*) self)->joint;
#line 64 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	_tmp2_ = v;
#line 64 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	dJointGetHingeAnchor ((struct dxJoint*) _tmp1_, _tmp2_);
#line 65 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	_tmp3_ = self->priv->_anchor_obj;
#line 65 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	_tmp4_ = _g_object_ref0 (_tmp3_);
#line 65 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	value = _tmp4_;
#line 66 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	_tmp8_ = value;
#line 66 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	if (_tmp8_ == NULL) {
#line 66 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp7_ = TRUE;
#line 513 "Hinge.c"
	} else {
		dxVector3* _tmp9_ = NULL;
		dReal _tmp10_ = 0.0;
		soyatomsPosition* _tmp11_ = NULL;
		gfloat _tmp12_ = 0.0F;
		gfloat _tmp13_ = 0.0F;
#line 66 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp9_ = v;
#line 66 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp10_ = _tmp9_->x;
#line 66 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp11_ = value;
#line 66 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp12_ = soy_atoms_position_get_x (_tmp11_);
#line 66 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp13_ = _tmp12_;
#line 66 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp7_ = ((gfloat) _tmp10_) != _tmp13_;
#line 532 "Hinge.c"
	}
#line 66 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	_tmp14_ = _tmp7_;
#line 66 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	if (_tmp14_) {
#line 66 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp6_ = TRUE;
#line 540 "Hinge.c"
	} else {
		dxVector3* _tmp15_ = NULL;
		dReal _tmp16_ = 0.0;
		soyatomsPosition* _tmp17_ = NULL;
		gfloat _tmp18_ = 0.0F;
		gfloat _tmp19_ = 0.0F;
#line 67 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp15_ = v;
#line 67 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp16_ = _tmp15_->y;
#line 67 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp17_ = value;
#line 67 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp18_ = soy_atoms_position_get_y (_tmp17_);
#line 67 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp19_ = _tmp18_;
#line 67 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp6_ = ((gfloat) _tmp16_) != _tmp19_;
#line 559 "Hinge.c"
	}
#line 66 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	_tmp20_ = _tmp6_;
#line 66 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	if (_tmp20_) {
#line 66 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp5_ = TRUE;
#line 567 "Hinge.c"
	} else {
		dxVector3* _tmp21_ = NULL;
		dReal _tmp22_ = 0.0;
		soyatomsPosition* _tmp23_ = NULL;
		gfloat _tmp24_ = 0.0F;
		gfloat _tmp25_ = 0.0F;
#line 67 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp21_ = v;
#line 67 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp22_ = _tmp21_->z;
#line 67 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp23_ = value;
#line 67 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp24_ = soy_atoms_position_get_z (_tmp23_);
#line 67 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp25_ = _tmp24_;
#line 67 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp5_ = ((gfloat) _tmp22_) != _tmp25_;
#line 586 "Hinge.c"
	}
#line 66 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	_tmp26_ = _tmp5_;
#line 66 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	if (_tmp26_) {
#line 592 "Hinge.c"
		soyatomsPosition* _tmp27_ = NULL;
		dxVector3* _tmp31_ = NULL;
		dReal _tmp32_ = 0.0;
		dxVector3* _tmp33_ = NULL;
		dReal _tmp34_ = 0.0;
		dxVector3* _tmp35_ = NULL;
		dReal _tmp36_ = 0.0;
		soyatomsPosition* _tmp37_ = NULL;
		soyatomsPosition* _tmp38_ = NULL;
		soyatomsPosition* _tmp39_ = NULL;
		soyatomsPosition* _tmp40_ = NULL;
#line 68 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp27_ = value;
#line 68 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		if (_tmp27_ != NULL) {
#line 608 "Hinge.c"
			soyatomsPosition* _tmp28_ = NULL;
			guint _tmp29_ = 0U;
			soyatomsPosition* _tmp30_ = NULL;
#line 69 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
			_tmp28_ = self->priv->_anchor_obj;
#line 69 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
			g_signal_parse_name ("on-set", SOY_ATOMS_TYPE_POSITION, &_tmp29_, NULL, FALSE);
#line 69 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
			g_signal_handlers_disconnect_matched (_tmp28_, G_SIGNAL_MATCH_ID | G_SIGNAL_MATCH_FUNC | G_SIGNAL_MATCH_DATA, _tmp29_, 0, NULL, (GCallback) __soy_joints_hinge_anchor_set_soy_atoms_position_on_set, self);
#line 70 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
			_tmp30_ = self->priv->_anchor_obj;
#line 70 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
			g_object_weak_unref ((GObject*) _tmp30_, __soy_joints_hinge_anchor_weak_gweak_notify, self);
#line 622 "Hinge.c"
		}
#line 71 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp31_ = v;
#line 71 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp32_ = _tmp31_->x;
#line 71 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp33_ = v;
#line 71 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp34_ = _tmp33_->y;
#line 71 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp35_ = v;
#line 71 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp36_ = _tmp35_->z;
#line 71 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp37_ = soy_atoms_position_new ((gfloat) _tmp32_, (gfloat) _tmp34_, (gfloat) _tmp36_);
#line 71 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_g_object_unref0 (value);
#line 71 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		value = _tmp37_;
#line 74 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp38_ = value;
#line 74 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		g_signal_connect_object (_tmp38_, "on-set", (GCallback) __soy_joints_hinge_anchor_set_soy_atoms_position_on_set, self, 0);
#line 75 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp39_ = value;
#line 75 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		g_object_weak_ref ((GObject*) _tmp39_, __soy_joints_hinge_anchor_weak_gweak_notify, self);
#line 76 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		_tmp40_ = value;
#line 76 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
		self->priv->_anchor_obj = _tmp40_;
#line 654 "Hinge.c"
	}
#line 77 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	result = value;
#line 77 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	_dvector3_free0 (v);
#line 77 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	return result;
#line 662 "Hinge.c"
}
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);
}