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