/** creates vehicle at desired position @param pos struct Position with desired position */ void SchlangeVelocity::create(const osg::Matrix& pose){ Schlange::create(pose); //*****************joint definition*********** for ( int n = 0; n < conf.segmNumber-1; n++ ) { Pos p1(objects[n]->getPosition()); Pos p2(objects[n]->getPosition()); UniversalJoint* j = new UniversalJoint(objects[n], objects[n+1], (objects[n]->getPosition() + objects[n+1]->getPosition())/2, Axis(0,0,1)* pose, Axis(0,1,0)* pose); j->init(odeHandle, osgHandle, true, conf.segmDia * 1.02); // setting stops at universal joints j->setParam(dParamLoStop, -conf.jointLimit*1.5); j->setParam(dParamHiStop, conf.jointLimit*1.5); j->setParam(dParamLoStop2, -conf.jointLimit*1.5); j->setParam(dParamHiStop2, conf.jointLimit*1.5); // making stops bouncy j->setParam (dParamBounce, 0.9 ); j->setParam (dParamBounce2, 0.9 ); // universal joints.push_back(j); } }
/** 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; };