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;
}
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();
}
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];
}
/**
*@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();
}
//! 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]);
}
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
}
}
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);
}