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;
}
}
}
/**
* Initialize the whole plugins. Gets the monitoredContact, closes the mechanincal
* @param world_
* @param space_
* @param contactJointGroup_
*/
void webots_physics_init(dWorldID world_, dSpaceID space_, dJointGroupID contactJointGroup_) {
#ifdef PHYSICS_DEBUG_OUT
time_t rawtime;
struct tm * timeinfo;
time ( &rawtime );
timeinfo = localtime ( &rawtime );
DEBUG_OUT("Start Initialization at "<<asctime (timeinfo))
#endif
s_data= new amarsi::PhysicsPluginData();
s_data->world=world_;
s_data->space=space_;
s_data->contactJointGroup=contactJointGroup_;
s_data->setMotionRestrictor(dWebotsGetBodyFromDEF("AMARSI"));
doStuff("LEFT_FORE",amarsi::LEFT_FORE);
doStuff("RIGHT_FORE",amarsi::RIGHT_FORE);
doStuff("LEFT_HIND",amarsi::LEFT_HIND);
doStuff("RIGHT_HIND",amarsi::RIGHT_HIND);
s_data->addResettableBody(dWebotsGetBodyFromDEF("AMARSI"));
s_data->addResettableBody(dWebotsGetBodyFromDEF("MOTORS_GROUP"));
s_data->addResettableBody(dWebotsGetBodyFromDEF("FR4_BODY_PARTS"));
s_data->saveState();
s_data->addDeniedContactBody(getBodyFromDEF("MOTORS_GROUP"));
s_data->addFloorElement("GROUND");
s_data->addFloorElement("RAMP");
s_data->addFloorElement("PODIUM");
s_data->addFloorElement("SLOPE");
s_data->addFloorElement("HEIGHTMAP");
s_data->setMainBody("MOTORS_GROUP");
initAllAmarsiMessages();
Message::set_emitter_receiver(EmitterReceiverPhysics::create());
Message::receive_enable(2);
DEBUG_OUT("Finish Initialization.")
// toe_erp=0.04;
// toe_crf=0.001;
}
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 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;
}
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);
}
