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; };