bool XMLBoundingShape::init(const lpzrobots::OdeHandle& _odeHandle, const lpzrobots::OsgHandle& osgHandle,
double scale, char mode) {
odeHandle = OdeHandle(_odeHandle);
parentSpace = odeHandle.space;
odeHandle.createNewSimpleSpace(parentSpace,true);
char primitiveMode = mode & ~Primitive::Body; // never create any body for the primitives (TODO: compound body)
if (!(mode & Primitive::Body)) {
attachedToParentBody = false;
}
for EACHCHILDNODE(boundingBoxNode, node) {
if (node->getNodeType() == DOMNode::ELEMENT_NODE) {
Primitive* primitive = 0;
if (XMLHelper::matchesName(node,XMLDefinitions::boxNode))
primitive = new Box(VALOFNODE(node, XMLDefinitions::lengthAtt) * scale, VALOFNODE(node, XMLDefinitions::widthAtt) * scale, VALOFNODE(node, XMLDefinitions::heightAtt) * scale);
else if(XMLHelper::matchesName(node,XMLDefinitions::sphereNode))
primitive = new Sphere(VALOFNODE(node,XMLDefinitions::radiusAtt));
else if(XMLHelper::matchesName(node,XMLDefinitions::cylinderNode))
primitive = new Cylinder(VALOFNODE(node,XMLDefinitions::radiusAtt) * scale, VALOFNODE(node,XMLDefinitions::heightAtt) * scale);
else if(XMLHelper::matchesName(node,XMLDefinitions::capsuleNode))
primitive = new Capsule(VALOFNODE(node,XMLDefinitions::radiusAtt) * scale, VALOFNODE(node,XMLDefinitions::heightAtt) * scale);
if (primitive!=0) {
XMLPrimitiveFactory::setMaterial(boundingBoxNode, primitive);
XMLPrimitiveFactory::setMaterial(node,primitive);
const Vec3 rot = XMLHelper::getRotation(node);
const Vec3 pos = XMLHelper::getPosition(node);
if (mode & Primitive::Body) { // use Transforms to attach the Primitives to the body
std::cout << "BoundingShape body mode!" << std::endl;
Primitive* Trans = new lpzrobots::Transform(parent, primitive, osgRotate(rot[0]*M_PI/180.0f,rot[1]*M_PI/180.0f,rot[2]*M_PI/180.0f)
*osg::Matrix::translate(scale*pos[0],scale*pos[1],scale*pos[2]));
Trans->init(odeHandle, 0, osgHandle.changeColor(Color(1.0,0,0,0.3)),primitiveMode);
}
else {
std::cout << "BoundingShape geom only mode!" << std::endl;
primitive->init(odeHandle, 0, osgHandle.changeColor(Color(1.0,0,0,0.3)), primitiveMode);
boundingPrimitiveList.push_back(primitive);
boundingPrimitivePoseList.push_back(osgRotate(rot[0]*M_PI/180.0f,rot[1]*M_PI/180.0f,rot[2]*M_PI/180.0f)
*osg::Matrix::translate(scale*pos[0],scale*pos[1],scale*pos[2]));
}
active = true;
std::cout << "Primitive for BoundingShape created!" << std::endl;
}
}
}
return active;
}
/**
* starting function (executed once at the beginning of the simulation loop)
*/
virtual void start(const lpzrobots::OdeHandle& odeHandle,
const lpzrobots::OsgHandle& osgHandle,
lpzrobots::GlobalData& global) {
// set initial camera position
setCameraHomePos(
lpzrobots::Pos(-0.0114359, 6.66848, 0.922832),
lpzrobots::Pos(178.866, -7.43884, 0));
// set simulation parameters
global.odeConfig.setParam("controlinterval", 30);
global.odeConfig.setParam("simstepsize", 0.001);
// add playground
lpzrobots::Playground* playground
= new lpzrobots::Playground(odeHandle, osgHandle,
osg::Vec3(10, 0.2, 0.3));
playground->setTexture(0,0,lpzrobots::TextureDescr("Images/wall_bw.jpg",-1.5,-3));
playground->setPosition(osg::Vec3(0,0,.0));
global.obstacles.push_back(playground);
// Add Hexabot robot
HEXABOT::HexabotConf hexabotConf = lpzrobots::Hexabot::getDefaultConf();
lpzrobots::OdeHandle rodeHandle = odeHandle;
rodeHandle.substance = lpzrobots::Substance(3.0, 0.0, 50.0, 0.8);
hexabot = new lpzrobots::Hexabot(
rodeHandle,
osgHandle.changeColor(lpzrobots::Color(1, 1, 1)),
hexabotConf, "Hexabot");
// put hexabot a little bit in the air
hexabot->place(osg::Matrix::translate(.0, .0, 1.));
controller = new TripodGait18DOF();
// create wiring
One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise());
// create agent and init it with controller, robot and wiring
lpzrobots::OdeAgent* agent = new lpzrobots::OdeAgent(global);
agent->init(controller, hexabot, wiring);
// create a fixed joint to hold the robot in the air at the beginning
robotfixator = new lpzrobots::FixedJoint(
hexabot->getMainPrimitive(),
global.environment);
robotfixator->init(odeHandle, osgHandle, false);
// inform global variable over everything that happened:
global.configs.push_back(hexabot);
global.agents.push_back(agent);
global.configs.push_back(controller);
std::cout << "\n\n"
<< "################################\n"
<< "# Press x to free hexabot! #\n"
<< "################################\n"
<< "\n\n" << std::endl;
}
/**
* starting function (executed once at the beginning of the simulation loop)
*/
virtual void start(const lpzrobots::OdeHandle& odeHandle,
const lpzrobots::OsgHandle& osgHandle,
lpzrobots::GlobalData& global) {
// set initial camera position
setCameraHomePos(
lpzrobots::Pos(-0.0114359, 6.66848, 0.922832),
lpzrobots::Pos(178.866, -7.43884, 0));
// set simulation parameters
global.odeConfig.setParam("controlinterval", 10);
global.odeConfig.setParam("simstepsize", 0.01);
// Add amosII robot
lpzrobots::AmosIIConf myAmosIIConf = lpzrobots::AmosII::getDefaultConf();
myAmosIIConf.rubberFeet = true;
amos = new lpzrobots::AmosII(
odeHandle,
osgHandle.changeColor(lpzrobots::Color(1, 1, 1)),
myAmosIIConf, "AmosII");
// put amos a little bit in the air
amos->place(osg::Matrix::translate(.0, .0, 0.5));
controller = new ExampleController();
// create wiring
One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise());
// create agent and init it with controller, robot and wiring
lpzrobots::OdeAgent* agent = new lpzrobots::OdeAgent(global);
agent->init(controller, amos, wiring);
// add playground
lpzrobots::Playground* playground
= new lpzrobots::Playground(odeHandle, osgHandle,
osg::Vec3(10, 0.2, 0.3));
playground->setPosition(osg::Vec3(0,0,0));
global.obstacles.push_back(playground);
// create a fixed joint to hold the robot in the air at the beginning
robotfixator = new lpzrobots::FixedJoint(
amos->getMainPrimitive(),
global.environment);
robotfixator->init(odeHandle, osgHandle, false);
// inform global variable over everything that happened:
global.configs.push_back(amos);
global.agents.push_back(agent);
global.configs.push_back(controller);
std::cout << "\n\n"
<< "################################\n"
<< "# Press x to free amosII! #\n"
<< "################################\n"
<< "\n\n" << std::endl;
}
/**
* starting function (executed once at the beginning of the simulation loop)
*/
virtual void start(const lpzrobots::OdeHandle& odeHandle,
const lpzrobots::OsgHandle& osgHandle,
lpzrobots::GlobalData& global) {
// set initial camera position
setCameraHomePos(
lpzrobots::Pos(-0.0114359, 6.66848, 0.922832),
lpzrobots::Pos(178.866, -7.43884, 0));
// set simulation parameters
global.odeConfig.setParam("controlinterval", 10);
global.odeConfig.setParam("simstepsize", 0.01);
global.odeConfig.setParam("noise", 0.3);
// add playground
lpzrobots::Playground* playground
= new lpzrobots::Playground(odeHandle, osgHandle,
osg::Vec3(10, 0.2, 0.3));
playground->setTexture(0,0,lpzrobots::TextureDescr("Images/wall_bw.jpg",-1.5,-3));
playground->setPosition(osg::Vec3(0,0,.0));
global.obstacles.push_back(playground);
//add obstacle
PassiveBox* b1;
double length = 3;
double width = 0.2;
double height = 1.0;
bool obstacle_active = false;
if(obstacle_active)
{
b1 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(length, width, height /*size*/));
//b1->setTexture("dusty.rgb");
b1->setColor(Color(1,0,0));
//b1->setPosition(osg::Vec3(/*-4.5+*/i*4.5,3+i,0)); // Fixed robot position
//osg::Matrix pose;
// pose.setTrans(osg::Vec3(/*-4.5+*/i*4.5,3+i,0));
//b1->setPose(osg::Matrix::rotate(0.5*(M_PI/2), 0,0, 1) * osg::Matrix::translate(/*-4.5+*/i*4.5,3+i,0.5) /* pose*/);
b1->setPose(osg::Matrix::rotate(/*-0.5*(M_PI/4)*/ (M_PI/2), 0,0, 1) * osg::Matrix::translate(2.0, 0.0,0.0) /* pose*/);
global.obstacles.push_back(b1);
lpzrobots::FixedJoint* fixator = new lpzrobots::FixedJoint(b1->getMainPrimitive(), global.environment);
fixator->init(odeHandle, osgHandle);
}
//----------create a sphere as the target by Ren-----------------------------
//the first sphere
lpzrobots::PassiveSphere* s1 = new lpzrobots::PassiveSphere(odeHandle, osgHandle, 0.1);
s1->setPosition(osg::Vec3(3.0, 0.0, 0.1));
s1->setTexture("Images/dusty.rgb");
s1->setColor(lpzrobots::Color(1,0,0));
obst.push_back(s1);
global.obstacles.push_back(s1);
lpzrobots::FixedJoint* fixator1 = new lpzrobots::FixedJoint(s1->getMainPrimitive(), global.environment);
fixator1->init(odeHandle, osgHandle);
//the second sphere
lpzrobots::PassiveSphere* s2 = new lpzrobots::PassiveSphere(odeHandle, osgHandle, 0.1);
s2->setPosition(osg::Vec3(0.0, 3.0, 0.1));
s2->setTexture("Images/dusty.rgb");
s2->setColor(lpzrobots::Color(0,1,0));
obst.push_back(s2);
global.obstacles.push_back(s2);
lpzrobots::FixedJoint* fixator2 = new lpzrobots::FixedJoint(s2->getMainPrimitive(), global.environment);
fixator2->init(odeHandle, osgHandle);
//the third sphere
lpzrobots::PassiveSphere* s3 = new lpzrobots::PassiveSphere(odeHandle, osgHandle, 0.1);
s3->setPosition(osg::Vec3(0.0, -3.0, 0.1));
s3->setTexture("Images/dusty.rgb");
s3->setColor(lpzrobots::Color(0,0,1));
obst.push_back(s3);
global.obstacles.push_back(s3);
lpzrobots::FixedJoint* fixator3 = new lpzrobots::FixedJoint(s3->getMainPrimitive(), global.environment);
fixator3->init(odeHandle, osgHandle);
//----------create a sphere as the target by Ren-----------------------------
// Add amosII robot
dungbeetleConf myAmosIIConf = dungbeetle::getDefaultConf(1.0 /*_scale*/,1 /*_useShoulder*/,1 /*_useFoot*/,1 /*_useBack*/);
myAmosIIConf.rubberFeet = true;
lpzrobots::OdeHandle rodeHandle = odeHandle;
rodeHandle.substance = lpzrobots::Substance(3.0, 0.0, 50.0, 0.8);
//------------------- Link the sphere to the Goal Sensor by Ren---------------
for(unsigned int i = 0; i<obst.size(); i++)
{
myAmosIIConf.GoalSensor_references.push_back(obst.at(i)->getMainPrimitive());
}
//------------------- Link the sphere to the Goal Sensor by Ren---------------
amos = new dungbeetle(
rodeHandle,
osgHandle.changeColor(lpzrobots::Color(1, 1, 1)),
myAmosIIConf, "AmosII");
// define the usage of the individual legs
amos->setLegPosUsage(amos->L0, amos->LEG);
amos->setLegPosUsage(amos->L1, amos->LEG);
amos->setLegPosUsage(amos->L2, amos->LEG);
amos->setLegPosUsage(amos->R0, amos->LEG);
amos->setLegPosUsage(amos->R1, amos->LEG);
amos->setLegPosUsage(amos->R2, amos->LEG);
// put amos a little bit in the air
amos->place(osg::Matrix::translate(.0, .0, 0.5));
controller = new EmptyController();
// create wiring
One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise());
// create agent and init it with controller, robot and wiring
lpzrobots::OdeAgent* agent = new lpzrobots::OdeAgent(global);
agent->init(controller, amos, wiring);
// create a fixed joint to hold the robot in the air at the beginning
robotfixator = new lpzrobots::FixedJoint(
amos->getMainPrimitive(),
global.environment);
robotfixator->init(odeHandle, osgHandle, false);
// inform global variable over everything that happened:
global.configs.push_back(amos);
global.agents.push_back(agent);
global.configs.push_back(controller);
std::cout << "\n\n"
<< "################################\n"
<< "# Press x to free amosII! #\n"
<< "################################\n"
<< "\n\n" << std::endl;
}
/**
* starting function (executed once at the beginning of the simulation loop)
*/
virtual void start(const lpzrobots::OdeHandle& odeHandle,
const lpzrobots::OsgHandle& osgHandle,
lpzrobots::GlobalData& global) {
// set initial camera position
setCameraHomePos(
lpzrobots::Pos(-0.0114359, 6.66848, 0.922832),
lpzrobots::Pos(178.866, -7.43884, 0));
// set simulation parameters
global.odeConfig.setParam("controlinterval", 10);
global.odeConfig.setParam("simstepsize", 0.01);
global.odeConfig.setParam("noise", sim_noise);
// declare terrain params
double terrain_roughness;
double terrain_slip;
double terrain_hardness;
double terrain_elasticity;
double terrain_height;
Color terrain_color;
int terrain_type = terrain_type_g;
// define individual terrain parameters here
switch(terrain_type) {
case 1: // concrete
setTitle("concrete");
terrain_roughness = 10.0;
terrain_slip = 0.0;
terrain_hardness = 100.0;
terrain_elasticity = 0.0;
terrain_height = 0.0;
terrain_color = Color(156.0/255.0,159.0/255.0,166.0/255.0);
break;
case 2: // mud
setTitle("mud");
terrain_roughness = 0.5;
terrain_slip = 5.0;
terrain_hardness = 0.5;
terrain_elasticity = 0.5;
terrain_height = 0.02;
terrain_color = Color(100.0/255.0,100.0/255.0,100.0/255.0);
break;
case 3: // ice
setTitle("ice");
terrain_roughness = 0.0;
terrain_slip = 100.0;
terrain_hardness = 100.0;
terrain_elasticity = 0.0;
terrain_height = 0.0;
terrain_color = Color(215.0/255.0,227.0/255.0,255.0/255.0);
break;
case 4: // sand
setTitle("sand");
terrain_roughness = 1.0;
terrain_slip = 0.1;
terrain_hardness = 30.0;
terrain_elasticity = 0.0;
terrain_height = 0.02;
terrain_color = Color(242.0/255.0,238.0/255.0,124.0/255.0);
break;
case 5: // gravel
setTitle("gravel");
terrain_roughness = 7.0;
terrain_slip = 0.1;
terrain_hardness = 100.0;
terrain_elasticity = 0.0;
terrain_height = 0.03;
terrain_color = Color(115.0/255.0,127.0/255.0,156.0/255.0);
break;
case 6: // grass
setTitle("grass");
terrain_roughness = 5.0;
terrain_slip = 0.0;
terrain_hardness = 30.0;
terrain_elasticity = 0.6;
terrain_height = 0.05;
terrain_color = Color(35.0/255.0,150.0/255.0,20.0/255.0);
break;
case 7: // swamp
setTitle("swamp");
terrain_roughness = 0.0;
terrain_slip = 5.0;
terrain_hardness = 0.0;
terrain_elasticity = 0.0;
terrain_height = 0.1;
terrain_color = Color(50.0/255.0,75.0/255.0,50.0/255.0);
break;
case 8: // rock
setTitle("rock");
terrain_roughness = 10.0;
terrain_slip = 0.0;
terrain_hardness = 100.0;
terrain_elasticity = 0.0;
terrain_height = 0.1;
terrain_color = Color(110.0/255.0,90.0/255.0,60.0/255.0);
break;
case 9: // tiles
setTitle("tiles");
terrain_roughness = 5.0;
terrain_slip = 30.0;
terrain_hardness = 100.0;
terrain_elasticity = 0.0;
terrain_height = 0.0;
terrain_color = Color(250.0/255.0,200.0/255.0,150.0/255.0);
break;
case 10: // snow
setTitle("snow");
terrain_roughness = 0.0;
terrain_slip = 80.0;
terrain_hardness = 20.0;
terrain_elasticity = 0.0;
terrain_height = 0.02;
terrain_color = Color(255.0/255.0,255.0/255.0,255.0/255.0);
break;
case 11: // rubber
setTitle("rubber");
terrain_roughness = 8.0;
terrain_slip = 0.0;
terrain_hardness = 80.0;
terrain_elasticity = 2.0;
terrain_height = 0.0;
terrain_color = Color(0.0/255.0,0.0/255.0,0.0/255.0);
break;
case 12: // carpet
setTitle("carpet");
terrain_roughness = 3.0;
terrain_slip = 0.0;
terrain_hardness = 40.0;
terrain_elasticity = 0.3;
terrain_height = 0.02;
terrain_color = Color(135.0/255.0,100.0/255.0,150.0/255.0);
break;
case 13: // wood
setTitle("wood");
terrain_roughness = 6.0;
terrain_slip = 0.0;
terrain_hardness = 80.0;
terrain_elasticity = 0.2;
terrain_height = 0.02;
terrain_color = Color(90.0/255.0,65.0/255.0,0.0/255.0);
break;
case 14: // plastic
setTitle("plastic");
terrain_roughness = 1.0;
terrain_slip = 2.0;
terrain_hardness = 60.0;
terrain_elasticity = 0.5;
terrain_height = 0.0;
terrain_color = Color(150.0/255.0,250.0/255.0,190.0/255.0);
break;
case 15: // foam
setTitle("foam");
terrain_roughness = 5.0;
terrain_slip = 0.0;
terrain_hardness = 0.0;
terrain_elasticity = 0.7;
terrain_height = 0.05;
terrain_color = Color(220.0/255.0,230.0/255.0,150.0/255.0);
break;
default:
setTitle("default");
terrain_roughness = 10.0;
terrain_slip = 0.0;
terrain_hardness = 100.0;
terrain_elasticity = 0.0;
terrain_height = 0.0;
terrain_color = Color(255.0/255.0,255.0/255.0,255.0/255.0);
}
if (terrain_noise) {
// Adding noise
terrain_roughness += fRand(-10.0*tn_std_prcnt, 10.0*tn_std_prcnt);
terrain_slip += fRand(-10.0*tn_std_prcnt, 10.0*tn_std_prcnt);
terrain_hardness += fRand(-100.0*tn_std_prcnt, 100.0*tn_std_prcnt);
terrain_elasticity += fRand(-2.0*tn_std_prcnt, 2.0*tn_std_prcnt);
terrain_height += fRand(-0.1*tn_std_prcnt, 0.1*tn_std_prcnt);
}
// limits
terrain_roughness = max(0.0, terrain_roughness);
terrain_slip = max(0.0, terrain_slip);
terrain_hardness = max(0.0, terrain_hardness);
terrain_elasticity = max(0.0, terrain_elasticity);
terrain_height = max(0.0, terrain_height);
cout << terrain_roughness << ", " << terrain_slip << ", " << terrain_hardness << ", " << terrain_elasticity << ", " << terrain_height << endl;
//**************Change Material substance*********//
//Substance roughterrainSubstance(1.0,0.0,/*100.0 friction*/100.0,0.0); //(roughness,slip,hardness,elasticity)
Substance roughterrainSubstance(terrain_roughness, terrain_slip, terrain_hardness, terrain_elasticity); //(roughness(0:smooth/1:rough), slip(0:friction/100:sliding), hardness(0:soft/100:hard), elasticity(0:hard/1:elastic))
OdeHandle oodeHandle = odeHandle;
oodeHandle.substance = roughterrainSubstance;
//**************Change Material substance*********//
TerrainGround* terrainground =
new TerrainGround(oodeHandle, osgHandle.changeColor(terrain_color),
"rough1.ppm","", /*AREA-SIZE-X->*/25, /*AREA-SIZE-Y->*/25, /*HEIGHT->*/terrain_height);
terrainground->setPose(osg::Matrix::translate(0, 0, 0.005));
global.obstacles.push_back(terrainground);
/* ########################## Add amosII robot ########################## */
lpzrobots::AmosIIConf myAmosIIConf = lpzrobots::AmosII::getDefaultConf(1.0 /*_scale*/,1 /*_useShoulder*/,1 /*_useFoot*/,1 /*_useBack*/);
myAmosIIConf.rubberFeet = true;
lpzrobots::OdeHandle rodeHandle = odeHandle;
rodeHandle.substance = lpzrobots::Substance(3.0, 0.0, 50.0, 0.8);
amos = new lpzrobots::AmosII(
rodeHandle,
osgHandle.changeColor(lpzrobots::Color(1, 1, 1)),
myAmosIIConf, "AmosII");
// define the usage of the individual legs
amos->setLegPosUsage(amos->L0, amos->LEG);
amos->setLegPosUsage(amos->L1, amos->LEG);
amos->setLegPosUsage(amos->L2, amos->LEG);
amos->setLegPosUsage(amos->R0, amos->LEG);
amos->setLegPosUsage(amos->R1, amos->LEG);
amos->setLegPosUsage(amos->R2, amos->LEG);
// put amos on a random position in the map
double init_pos_x = fRand(0.0, 5.0);
double init_pos_y = fRand(0.0, 5.0);
cout << init_pos_x << "," << init_pos_y;
amos->place(osg::Matrix::translate(init_pos_x, init_pos_y, 0.1));
controller = new EmptyController();
// create wiring
One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise());
// create agent and init it with controller, robot and wiring
lpzrobots::OdeAgent* agent = new lpzrobots::OdeAgent(global);
agent->init(controller, amos, wiring);
// create a fixed joint to hold the robot in the air at the beginning
robotfixator = new lpzrobots::FixedJoint(
amos->getMainPrimitive(),
global.environment);
robotfixator->init(odeHandle, osgHandle, false);
// inform global variable over everything that happened:
global.configs.push_back(amos);
global.agents.push_back(agent);
global.configs.push_back(controller);
// remove robotfixator
if (robotfixator) {
delete robotfixator;
robotfixator = NULL;
}
}
