// plugin function called by Webots at the beginning of the simulation
void webots_physics_init(dWorldID w, dSpaceID s, dJointGroupID j) {
pthread_mutex_init(&mutex, NULL); // needed to run with multi-threaded version of ODE
// get ODE handles to .wbt objects
floor_geom = dWebotsGetGeomFromDEF("FLOOR");
box_geom = dWebotsGetGeomFromDEF("OBJECT");
// get box's body (the floor does not have a body)
box_body = dGeomGetBody(box_geom);
}
CollisionRobot (std::string robot_def)
{
if (robot_def[0] == 'B')
team = BLUE_TEAM;
else
team = YELLOW_TEAM;
robot_body = dWebotsGetBodyFromDEF(robot_def.c_str());
if (robot_body != NULL)
{
int n_wheels = 0;
//get the wheels for this robot definition from the world tree
while (true)
{
std::stringstream s;
s << n_wheels;
std::string wheel_def = robot_def + "." + WHEEL_DEF + s.str ();
dGeomID wheel_geom_id = dWebotsGetGeomFromDEF(wheel_def.c_str());
dBodyID wheel_body_id = dWebotsGetBodyFromDEF(wheel_def.c_str());
if (wheel_geom_id != NULL && wheel_body_id != NULL)
{
robot_wheels_geom.push_back (wheel_geom_id);
robot_wheels_body.push_back (wheel_body_id);
n_wheels++;
}
else
break;
}
}
}
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;
}
/**
* Returns a dBody id from its DEF name.
* @param def DEF name of teh desired dBody
* @return a pointer to the dBody if found NULL otherwise
*/
dBodyID getBodyFromDEF(const char *def) {
//std::cout<<"Try to get body "<<def<<std::endl;
dGeomID geom = dWebotsGetGeomFromDEF(def);
if (! geom) {
printf("Fatal error: did not find dGeom for DEF %s", def);
s_data->disablePhysics();
return NULL;
}
dBodyID body;
if (dGeomIsSpace(geom)){
body = dGeomGetBody(dSpaceGetGeom((dSpaceID)geom, 0));
std::cerr<<"I don't know why I have to do this"<<std::endl;
s_data->disablePhysics();
}else
body = dGeomGetBody(geom);
if (! body) {
printf("Fatal error: did not find dBody for DEF %s", def);
s_data->disablePhysics();
return NULL;
}
return body;
}
void
webots_physics_init (dWorldID w, dSpaceID s, dJointGroupID j)
{
world = w;
space = s;
contact_joint_group = j;
//get blue team robots
for (uint i = 0; i < MAX_TEAM_SIZE; i++)
{
std::stringstream s;
s << (int)i;
std::string robot_name = BLUE + s.str ();
CollisionRobot robot (robot_name);
//get all the robots being created that have proper names and wheels
if (robot.robot_body != NULL && robot.getNWheels () > 0)
blue_team.push_back (robot);
}
//get yellow team robots
for (uint i = 0; i < MAX_TEAM_SIZE; i++)
{
std::stringstream s;
s << (int)i;
std::string robot_name = YELLOW + s.str ();
CollisionRobot robot (robot_name);
//get all the robots being created that have proper names and wheels
if (robot.robot_body != NULL && robot.getNWheels () > 0)
yellow_team.push_back (robot);
}
ground_body = dWebotsGetBodyFromDEF("FIELD");
ground_geom = dWebotsGetGeomFromDEF("FIELD");
}
// 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;
}
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);
}
