Primitive* PlattfussSchlange::createSegment(int index, const OdeHandle& odeHandle){
Primitive* p;
/////////// MIDDLE SEGMENT (BODY)
if ( index*2 == conf.segmNumber-1) {
//p = new Box(conf.segmLength*1.5,conf.segmLength*1.5, conf.segmLength*.6);
//p = new Capsule(conf.segmDia*.8/*2.8*/ , conf.segmLength*1);
// p = new Capsule(conf.segmLength/2 , conf.segmLength*2);
p = new Sphere(conf.segmLength*.8);
p->setTexture("Images/wood.rgb");
p->init(odeHandle, conf.segmMass*2, osgHandle);
// p->setPose( osg::Matrix::rotate(M_PI/2, 0, 1, 0)*osg::Matrix::translate( conf.segmDia, 0, 0) );
} /////// FEED
else if( (index == 0) | (index== conf.segmNumber-1)) {
// p = new Capsule(conf.segmDia*.8/*2.8*/ , conf.segmLength*1);
// p = new Sphere(conf.segmLength/2*2);
p = new Box(1.8*conf.segmLength,3*conf.segmLength, conf.segmLength*.3);
p->setTexture("Images/whitemetal_farbig_small.rgb");
p->init(odeHandle, conf.segmMass*3, osgHandle);
} /////// NORMAL SEGMENT
else {
p = SchlangeServo2::createSegment(index, odeHandle);
}
return p;
}
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;
}
Primitive* Schlange::createSegment(int index, const OdeHandle& odeHandle){
Primitive* p;
p = new Capsule(conf.segmDia * 0.8, conf.segmLength);
// if (index==0)
// p = new Box(conf.segmLength*.1,conf.segmLength*.9, conf.segmLength*.6);
p->setTexture("Images/whitemetal_farbig_small.rgb");
p->init(odeHandle, conf.segmMass, osgHandle);
if(index==0)
p->setColor(conf.headColor);
else
p->setColor(conf.bodyColor);
return p;
}
/** creates vehicle at desired position
*/
void DefaultCaterPillar::create(const osg::Matrix& pose) {
if (created) {
destroy();
}
odeHandle.createNewSimpleSpace(parentspace,false);
int half = conf.segmNumber/2;
// linear positioning (snake-like)
for(int n = 0; n < conf.segmNumber; n++) {
Primitive* p = new Box(conf.segmDia/2, conf.segmDia*2, conf.segmLength);
p->setTexture("Images/dusty.rgb");
p->init(odeHandle, conf.segmMass, osgHandle);
p->setPose(osg::Matrix::rotate(M_PI/2, 0, 1, 0) *
osg::Matrix::translate((n-half)*conf.segmLength*0.7, 0, conf.segmDia/2) * // made boxes overlapping for not seeing any gaps (*0.7)
pose);
objects.push_back(p);
}
created=true;
}
void RandomObstacles::spawn(OType type , SType subtype){
OdeHandle handle2 = odeHandle;
OsgHandle osgHandle2;
double density=((double)rand()/RAND_MAX)*(conf.maxDensity - conf.minDensity) + conf.minDensity;
if(subtype == SRandom){
subtype = (SType)(rand()%4);
}
switch (subtype){
case Metal:
handle2.substance.toMetal(1);
osgHandle2 = osgHandle.changeColor(Color(0.5,0.5,0.5));
density = conf.maxDensity;
break;
case Plastic:
handle2.substance.toPlastic(1);
osgHandle2 = osgHandle.changeColor(Color(1,1,1));
break;
case Rubber:
handle2.substance.toRubber(10);
osgHandle2 = osgHandle.changeColor(Color(0.2,0.2,0.2));
break;
case Foam:
default:
handle2.substance.toFoam(15);
osgHandle2 = osgHandle.changeColor(Color(1,1,0));
density = conf.minDensity;
break;
}
Pos dim((double)rand() / RAND_MAX,
(double)rand() / RAND_MAX,
(double)rand() / RAND_MAX);
dim = (dim & (conf.maxSize - conf.minSize)) + conf.minSize;
if(type == ORandom) {
int l = conf.boxRelFreq + conf.sphereRelFreq + conf.capRelFreq;
int r = rand()%l;
if(r<conf.boxRelFreq) type = RandomObstacles::Box;
else if (r<conf.boxRelFreq + conf.sphereRelFreq) type = RandomObstacles::Sphere;
else type = RandomObstacles::Caps;
}
Primitive* o;
switch (type){
case RandomObstacles::Box:
o = new lpzrobots::Box(dim.x(), dim.y(), dim.z());
o->init(handle2, dim.x()* dim.y()* dim.z() * density, osgHandle2);
break;
case RandomObstacles::Sphere:
o = new lpzrobots::Sphere(dim.x()/2.0);
o->init(handle2, 2.0/3.0*M_PI*pow(dim.x(),3)*density , osgHandle2);
break;
case RandomObstacles::Caps:
default:
o = new lpzrobots::Capsule(dim.x()/2.0, dim.z()/2.0);
o->init(handle2, M_PI*sqr(dim.x())*dim.z()/8*density, osgHandle2);
break;
}
Pos pos(random_minusone_to_one(0), random_minusone_to_one(0), 1);
pos = (pos) & conf.area;
pos.z() += (index%3) * conf.area.z()/2;
index++;
o->setPosition(pos * pose);
obst.push_back(o);
};
/** creates vehicle at desired position
@param pos struct Position with desired position
*/
void Hexapod::create( const osg::Matrix& pose ){
if (created) {
destroy();
}
odeHandle.createNewSimpleSpace(parentspace,true);
// color of joint axis and whiskers
OsgHandle osgHandleJ = osgHandle.changeColor(Color(72./255.,16./255.,16./255.));
TwoAxisServoVel* servo;
OneAxisServo* spring;
OneAxisServo* spring2;
FixedJoint* fixedJoint;
// create body
double twidth = conf.size * conf.width ;// 1/1.5;
double theight = conf.size * conf.height; // 1/4;
trunk = new Box(conf.size, twidth, theight);
trunk->setTexture("Images/toy_fur3.jpg");
trunk->init(odeHandle, conf.mass*conf.percentageBodyMass, osgHandle);
osg::Matrix trunkPos = TRANSM(0,0,conf.legLength)*pose;
trunk->setPose(trunkPos);
objects.push_back(trunk);
if(conf.useBigBox){
Primitive* pole;
double poleheight=conf.size*2;
pole = new Box(poleheight, twidth*3.2, theight*1);
bigboxtransform= new Transform(trunk,pole, osg::Matrix::translate(0,0,2*theight));
bigboxtransform->init(odeHandle, 0, osgHandle,Primitive::Geom /*| Primitive::Draw */);
}else{
bigboxtransform=0;
}
osg::Matrix m0 = pose;
if(conf.irSensors == true){
for(int i = -1; i < 2; i+=2){
irbox = new Box(0.1,0.1,0.1);
irbox->setTexture("Images/toy_fur3.jpg");
irbox->init(odeHandle, 0.00001, osgHandle);
irbox->setPose(ROTM(M_PI/4,0,0,1) * TRANSM(i*conf.size/2,0,theight/2)*trunkPos);
objects.push_back(irbox);
fixedJoint = new FixedJoint(trunk,irbox);
fixedJoint->init(odeHandle, osgHandleJ, true, 0.4);
joints.push_back(fixedJoint);
}
for(int i = -1; i < 2; i+=2){
irbox = new Box(0.1,0.1,0.15);
irbox->setTexture("Images/toy_fur3.jpg");
irbox->init(odeHandle, 0.00001, osgHandle);
irbox->setPose(TRANSM(0,i*twidth/2,theight/2 + 0.05)*trunkPos);
objects.push_back(irbox);
fixedJoint = new FixedJoint(trunk,irbox);
fixedJoint->init(odeHandle, osgHandleJ, true, 0.4);
joints.push_back(fixedJoint);
}
irSensorBank.init(odeHandle, osgHandle);
if (conf.irFront){ // add front left and front right infrared sensor to sensorbank if required
IRSensor* sensor = new IRSensor();
irSensorBank.registerSensor(sensor, objects[2],
Matrix::rotate(-1*-M_PI/2, Vec3(0,1,0)) *
Matrix::translate(1*0.05,0,0),
conf.irRangeFront, RaySensor::drawAll);
IRSensor* sensor2 = new IRSensor();
irSensorBank.registerSensor(sensor2, objects[2],
Matrix::rotate(-1*-M_PI/2, Vec3(0,1,0)) *
Matrix::rotate(1*-M_PI/2, Vec3(0,0,1)) *
Matrix::translate(0,-0.05,0),
conf.irRangeFront, RaySensor::drawAll);
}
if (conf.irBack){ // add front left and front right infrared sensor to sensorbank if required
IRSensor* sensor = new IRSensor();
irSensorBank.registerSensor(sensor, objects[1],
Matrix::rotate(1*-M_PI/2, Vec3(0,1,0)) *
Matrix::translate(-1*0.05,0,0),
conf.irRangeBack, RaySensor::drawAll);
IRSensor* sensor2 = new IRSensor();
irSensorBank.registerSensor(sensor2, objects[1],
Matrix::rotate(1*-M_PI/2, Vec3(0,1,0)) *
Matrix::rotate(1*-M_PI/2, Vec3(0,0,1)) *
Matrix::translate(0,0.05,0),
conf.irRangeBack, RaySensor::drawAll);
}
if(conf.irLeft){
IRSensor* sensor = new IRSensor();
irSensorBank.registerSensor(sensor, objects[3],
Matrix::rotate(-1*-M_PI/2, Vec3(0,1,0)) *
Matrix::rotate(-M_PI/2, Vec3(0,0,1)) *
Matrix::translate(0,-0.05,0.05),
conf.irRangeLeft, RaySensor::drawAll);
/* IRSensor* sensor2 = new IRSensor();
irSensorBank.registerSensor(sensor2, objects[3],
Matrix::rotate(-1*-M_PI/2, Vec3(0,1,0)) *
Matrix::rotate(1*-M_PI/2, Vec3(0,0,1)) *
Matrix::translate(0,-0.05,0),
conf.irRangeLeft, RaySensor::drawAll);
*/
}
if(conf.irRight){
IRSensor* sensor = new IRSensor();
irSensorBank.registerSensor(sensor, objects[4],
Matrix::rotate(-1*-M_PI/2, Vec3(0,1,0)) *
Matrix::rotate(M_PI/2, Vec3(0,0,1)) *
Matrix::translate(0,0.05,0.05),
conf.irRangeLeft, RaySensor::drawAll);
/* IRSensor* sensor2 = new IRSensor();
irSensorBank.registerSensor(sensor2, objects[4],
Matrix::rotate(1*-M_PI/2, Vec3(0,1,0)) *
Matrix::rotate(1*-M_PI/2, Vec3(0,0,1)) *
Matrix::translate(0,0.05,0),
conf.irRangeRight, RaySensor::drawAll);
*/
}
}
std::vector<Primitive*> tarsusParts;
// legs (counted from back to front)
double legdist = conf.size*0.9 / (conf.legNumber/2-1);
for ( int n = 0; n < conf.legNumber; n++ ) {
int v = n;
double len1 = conf.legLength*0.5;
double rad1 = conf.legLength/15;
double len2 = conf.legLength*0.5;
double rad2 = conf.legLength/15;
// upper limp
Primitive* coxaThorax;
Pos pos = Pos(-conf.size/(2+0.2) + ((int)n/2) * legdist,
n%2==0 ? - twidth/2 : twidth/2,
conf.legLength - theight/3);
osg::Matrix m = ROTM(M_PI/2,v%2==0 ? -1 : 1,0,0) * TRANSM(pos) * pose;
coxaThorax = new Capsule(rad1, len1);
coxaThorax->setTexture("Images/toy_fur3.jpg");
coxaThorax->init(odeHandle, legmass, osgHandle);
osg::Matrix m1 = TRANSM(0,0,-len1/2)
* ROTM(M_PI,0,0,v%2==0 ? -1 : 1)
* ROTM(2*M_PI,0,v%2==0 ? -1 : 1,0) * m;
coxaThorax->setPose(m1);
thoraxPos.push_back(coxaThorax->getPosition());
thorax.push_back(coxaThorax);
objects.push_back(coxaThorax);
legs.push_back(coxaThorax);
// powered hip joint
Pos nullpos(0,0,0);
UniversalJoint* j
= new UniversalJoint(trunk, coxaThorax, nullpos * m,
ROTM(M_PI,0,0,v%2==0 ? -1 : 1) * Axis(v%2==0 ? -1 : 1,0,0) * m,
ROTM(M_PI,0,0,v%2==0 ? -1 : 1) * Axis(0,1,0) * m);
j->init(odeHandle, osgHandleJ, true, rad1 * 2.1);
joints.push_back(j);
legContactArray[n].joint = j->getJoint();
// values will be set in setParam later
servo = new TwoAxisServoVel(odeHandle, j,-1,1, 1, -1,1,1,0 );
// servo = new UniversalServo(j,-1,1, 1, -1,1,1,0);
hipservos.push_back(servo);
// lower leg
Primitive* tibia;
tibia = new Capsule(rad2, len2);
tibia->setTexture("Images/toy_fur3.jpg");
tibia->init(odeHandle, legmass, osgHandle);
osg::Matrix m2 = TRANSM(0,0,-len2/2) * ROTM(1.5,v%2==0 ? -1 : 1,0,0)
* TRANSM(0,0,-len1/2) * m1;
tibia->setPose(m2);
objects.push_back(tibia);
legs.push_back(tibia);
legContactArray[n].legID = n;
legContactArray[n].geomid = tibia->getGeom();
legContactArray[n].bodyID = tibia->getBody();
if(conf.useTebiaJoints){
// springy knee joint
HingeJoint* k = new HingeJoint(coxaThorax, tibia, Pos(0,0,-len1/2) * m1,
Axis(v%2==0 ? -1 : 1,0,0) * m1);
k->init(odeHandle, osgHandleJ, true, rad1 * 2.1);
// servo used as a spring
spring = new HingeServo(k, -1, 1, 1, 0.01,0); // parameters are set later
tebiasprings.push_back(spring);
joints.push_back(k);
}else{
// fixed knee joint
FixedJoint* k = new FixedJoint(coxaThorax, tibia);
k->init(odeHandle, osgHandleJ, false, rad1 * 2.1);
joints.push_back(k);
}
// lower limp should not collide with body!
odeHandle.addIgnoredPair(trunk,tibia);
if(conf.tarsus){
// New: tarsus
Primitive *tarsus;
double angle = M_PI/12;
double radius = rad2/2;
double length = len2/2;
double mass = legmass/10;
tarsus = new Capsule(radius,length);
tarsus->setTexture("Images/toy_fur3.jpg");
tarsus->init(odeHandle, mass, osgHandle);
osg::Matrix m4;
osg::Matrix m3 =
ROTM(-angle,v%2==0 ? -1 : 1,0,0) *
TRANSM(0,0,-len2/2) *
m2;
if(v < 2){
m4 = ROTM(v%2==0 ? angle : -angle,0,v%2==0 ? -1 : 1,0) * m3;
}else if( v > 3){
m4 = ROTM(v%2==0 ? -angle : angle,0,v%2==0 ? -1 : 1,0) * m3;
}else{
m4 = m3;
}
m4 = TRANSM(0,0,-length/2) * m4;
tarsus->setPose(m4);
tarsusParts.push_back(tarsus);
objects.push_back(tarsus);
if(conf.useTarsusJoints){
// springy joint
HingeJoint* k = new HingeJoint(tibia, tarsus, Pos(0,0,-len2/2) * m2,
Axis(v%2==0 ? -1 : 1,0,0) * m2);
k->init(odeHandle, osgHandleJ, true, rad1 * 2.1);
// servo used as a spring
// spring2 = new HingeServo(k, -1, 1, 1, 0.01,0); // parameters are set later
spring2 = new OneAxisServoVelocityControlled(odeHandle,k, -1, 1, 1, 0.01); // parameters are set later
tarsussprings.push_back(spring2);
joints.push_back(k);
}else{
FixedJoint* k = new FixedJoint(tibia, tarsus);
k->init(odeHandle, osgHandleJ, true, rad1 * 2.1);
joints.push_back(k);
}
Primitive *section = tarsus;
for(int i = 1; i < conf.numTarsusSections; i++){
double lengthS = length/1.5;
double radiusS = radius/1.5;
section = new Capsule(radiusS,lengthS);
section->setTexture("Images/toy_fur3.jpg");
section->init(odeHandle, mass/2, osgHandle);
osg::Matrix m5;
if(v < 2){
m5 = TRANSM(0,0,-lengthS/2) *
ROTM(v%2==0 ? angle : -angle,0,v%2==0 ? -1 : 1,0) *
ROTM(v%2==0 ? angle : -angle,1,0,0) *
TRANSM(0,0,-length/2) *
m4;
}else if(v > 3){
m5 = TRANSM(0,0,-lengthS/2) *
ROTM(v%2==0 ? -angle : angle,0,v%2==0 ? -1 : 1,0) *
ROTM(v%2==0 ? angle : -angle,1,0,0) *
TRANSM(0,0,-length/2) *
m4;
}else{
m5 = TRANSM(0,0,-lengthS/2) *
ROTM(v%2==0 ? angle : -angle,1,0,0) *
TRANSM(0,0,-length/2) *
m4;
}
section->setPose(m5);
objects.push_back(section);
tarsusParts.push_back(section);
FixedJoint* fj = new FixedJoint(tarsusParts[i-1], tarsusParts[i]);
fj->init(odeHandle, osgHandleJ, true, rad1 * 2.1);
joints.push_back(fj);
m4 = m5;
}
std::cout<< "legContactArray[" << n << "].legID = " << n << std::endl;
legContactArray[n].legID = n;
legContactArray[n].geomid = section->getGeom();
legContactArray[n].bodyID = section->getBody();
tarsusParts.clear();
}
}
// New: wiskers
for ( int n = -1; n < 2; n+=2 ) {
double l1 = conf.legLength*0.5;
double t1 = conf.legLength/30;
Primitive* whisker;
Pos pos = Pos(conf.size/(2)+t1,
n*twidth/4,
conf.legLength + theight/5);
osg::Matrix m = ROTM(M_PI/10, n,0,0) * ROTM(M_PI/2+M_PI/10, 0,-1,0) * TRANSM(pos) * pose;
whisker = new Capsule(t1, l1);
whisker->init(odeHandle, legmass/10, osgHandleJ);
osg::Matrix m1 = TRANSM(0,0,-l1/2) * m;
whisker->setPose(m1);
objects.push_back(whisker);
//FixedJoint* k = new FixedJoint(trunk, whisker);
//k->init(odeHandle, osgHandle, false, 0);
HingeJoint* k = new HingeJoint(trunk, whisker, Pos(0,0,0) * m,
Axis(1,0,0) * m);
k->init(odeHandle, osgHandleJ, true, t1 * 2.1);
// servo used as a spring
spring = new HingeServo(k, -M_PI/6, M_PI/6, .1, 0.01,0);
whiskersprings.push_back(spring);
joints.push_back(k);
Primitive* whisker2;
whisker2 = new Capsule(t1/2, l1);
whisker2->init(odeHandle, legmass/10, osgHandleJ);
osg::Matrix m2 = TRANSM(0,0,-l1/2)
* ROTM(M_PI/10, n,0,0)
* ROTM(M_PI/10, 0,1,0) * TRANSM(0,0,-l1/2) * m1;
whisker2->setPose(m2);
objects.push_back(whisker2);
// k = new FixedJoint(whisker, whisker2);
// k->init(odeHandle, osgHandleJ, false, 0);
k = new HingeJoint(whisker, whisker2, Pos(0,0,-l1/2) * m1,
Axis(0,1,0) * m1);
k->init(odeHandle, osgHandleJ, true, t1 * 2.1);
// servo used as a spring
spring = new HingeServo(k, -M_PI/6, M_PI/6, .05, 0.01,0);
whiskersprings.push_back(spring);
joints.push_back(k);
}
notifyOnChange("dummy"); // apply all parameters.
created=true;
};
