void ContactSensor::init(const OdeHandle& odeHandle,
const OsgHandle& osgHandle,
Primitive* reference,
bool createSphere,
const osg::Matrix pose,
bool colorObject){
assert(reference);
this->colorObject = colorObject;
this->reference = reference;
value = 0;
lastvalue = -1;
if(createSphere){
sensorBody = new Sphere(size);
transform = new Transform(reference, sensorBody, pose);
origColor = osgHandle.getColor("joint");
transform->init(odeHandle, 0, osgHandle.changeColor(origColor));
transform->substance.setCollisionCallback(contactCollCallbackNoCol,this);
}else{
reference->substance.setCollisionCallback(contactCollCallback,this);
if(reference->getOSGPrimitive())
origColor = reference->getOSGPrimitive()->getColor();
else
colorObject = false;
}
update();
initialised = true;
};
void TmpDisplayItem::init(const OdeHandle& odeHandle, const OsgHandle& osgHandle){
Color mcolor(color);
if(useColorName){
mcolor = osgHandle.getColor(colorname);
mcolor.alpha() = alpha;
}
item->init(osgHandle.changeColor(mcolor), quality);
item->setMatrix(pose);
initialized=true;
}
void TmpPrimitive::init(const OdeHandle& odeHandle, const OsgHandle& osgHandle){
Color mcolor(color);
if(useColorName){
mcolor = osgHandle.getColor(colorname);
mcolor.alpha() = alpha;
}
item->init(odeHandle, mass, osgHandle.changeColor(mcolor), mode);
item->setPose(pose);
initialized=true;
}
void TmpJoint::init(const OdeHandle& odeHandle, const OsgHandle& osgHandle){
Color mcolor(color);
if(useColorName){
mcolor = osgHandle.getColor(colorname);
mcolor.alpha() = alpha;
}
joint->init(odeHandle, osgHandle.changeColor(mcolor),
withVisual, visualSize, ignoreColl);
initialized=true;
}
// starting function (executed once at the beginning of the simulation loop)
void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
setCameraHomePos(Pos(-6.32561, 5.12705, 3.17278), Pos(-130.771, -17.7744, 0));
global.odeConfig.setParam("noise", 0.05);
global.odeConfig.setParam("controlinterval", 2);
global.odeConfig.setParam("cameraspeed", 250);
global.odeConfig.setParam("gravity", -6);
setParam("UseQMPThread", false);
// use Playground as boundary:
AbstractGround* playground =
new Playground(odeHandle, osgHandle, osg::Vec3(8, 0.2, 1), 1);
// // playground->setColor(Color(0,0,0,0.8));
playground->setGroundColor(Color(2,2,2,1));
playground->setPosition(osg::Vec3(0,0,0.05)); // playground positionieren und generieren
global.obstacles.push_back(playground);
Boxpile* boxpile = new Boxpile(odeHandle, osgHandle);
boxpile->setColor("wall");
boxpile->setPose(ROTM(M_PI/5.0,0,0,1)*TRANSM(0, 0,0.2));
global.obstacles.push_back(boxpile);
// global.obstacles.push_back(playground);
// double diam = .90;
// OctaPlayground* playground3 = new OctaPlayground(odeHandle, osgHandle, osg::Vec3(/*Diameter*/4.0*diam, 5,/*Height*/ .3), 12,
// false);
// // playground3->setColor(Color(.0,0.2,1.0,1));
// playground3->setPosition(osg::Vec3(0,0,0)); // playground positionieren und generieren
// global.obstacles.push_back(playground3);
controller=0;
// addParameter("gamma_s",&teacher);
global.configs.push_back(this);
for(int i=0; i< bars; i++) {
PassiveBox* b = new PassiveBox(odeHandle, osgHandle.changeColor(Color(0.,0.,0.)),
osg::Vec3(1,10,0.3+i*.1),10);
b->setPosition(osg::Vec3(10+i*7,0,0));
global.obstacles.push_back(b);
}
/******* H E X A P O D *********/
int numhexapods = 1;
for ( int ii = 0; ii< numhexapods; ii++) {
HexapodConf myHexapodConf = Hexapod::getDefaultConf();
myHexapodConf.coxaPower = 1.5;
myHexapodConf.tebiaPower = 0.8;
myHexapodConf.coxaJointLimitV = .9; // M_PI/8; // angle range for vertical dir. of legs
myHexapodConf.coxaJointLimitH = 1.3; //M_PI/4;
myHexapodConf.tebiaJointLimit = 1.8; // M_PI/4; // +- 45 degree
myHexapodConf.percentageBodyMass = .5;
myHexapodConf.useBigBox = false;
myHexapodConf.tarsus = true;
myHexapodConf.numTarsusSections = 1;
myHexapodConf.useTarsusJoints = true;
// myHexapodConf.numTarsusSections = 2;
OdeHandle rodeHandle = odeHandle;
rodeHandle.substance.toRubber(20);
vehicle = new Hexapod(rodeHandle, osgHandle.changeColor("Green"),
myHexapodConf, "Hexapod_" + std::itos(teacher*10000));
// on the top
vehicle->place(osg::Matrix::rotate(M_PI*1,1,0,0)*osg::Matrix::translate(0,0,1.5+ 2*ii));
// normal position
// vehicle->place(osg::Matrix::translate(0,0,0));
// InvertMotorNStepConf cc = InvertMotorNStep::getDefaultConf();
// cc.cInit=1.0;
// cc.useS=false;
// cc.someInternalParams=true;
// InvertMotorNStep *semox = new InvertMotorNStep(cc);
// semox->setParam("steps", 1);
// semox->setParam("continuity", 0.005);
// semox->setParam("teacher", teacher);
SoMLConf sc = SoML::getDefaultConf();
sc.useHiddenContr=true;
sc.useHiddenModel=false;
sc.someInternalParams=false;
sc.useS=false;
SoML* soml = new SoML(sc);
soml->setParam("epsC",0.105);
soml->setParam("epsA",0.05);
Sox* sox = new Sox(1.2, false);
sox->setParam("epsC",0.105);
sox->setParam("epsA",0.05);
sox->setParam("Logarithmic",1);
SeMoXConf cc = SeMoX::getDefaultConf();
//cc.cInit=.95;
cc.cInit=.99;
cc.modelExt=false;
cc.someInternalParams=true;
SeMoX* semox = new SeMoX(cc);
DerInfConf dc = DerInf::getDefaultConf();
dc.cInit=.599;
dc.someInternalParams=false;
AbstractController* derinf = new DerInf(dc);
derinf->setParam("epsC",0.1);
derinf->setParam("epsA",0.05);
AbstractController* sine = 0;
if(useSineController) {
// sine = new SineController(~0, SineController::Sine);
sine = new SineController(~0, SineController::Impulse);
// // // // // motorpower 20
sine->setParam("period", 30);
sine->setParam("phaseshift", 0.5);
sine->setParam("amplitude", 0.5);
}
semox->setParam("epsC", 0.1);
semox->setParam("epsA", 0.1);
semox->setParam("rootE", 3);
semox->setParam("s4avg", 1);
semox->setParam("gamma_cont", 0.005);
semox->setParam("gamma_teach", teacher);
if(useSineController) {
controller = sine;
} else {
// controller = semox;
controller = sox;
// controller = soml;
// controller = derinf;
}
One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1));
// the feedbackwiring feeds here 75% of the motor actions as inputs and only 25% of real inputs
// AbstractWiring* wiring = new FeedbackWiring(new ColorUniformNoise(0.1),
// FeedbackWiring::Motor, 0.75);
//global.plotoptions.push_back(PlotOption(GuiLogger,Robot,5));
OdeAgent* agent = new OdeAgent(global);
agent->init(controller, vehicle, wiring);
// add an operator to keep robot from falling over
agent->addOperator(new LimitOrientationOperator(Axis(0,0,1), Axis(0,0,1), M_PI*0.5, 30));
if(track) {
TrackRobotConf c = TrackRobot::getDefaultConf();
c.displayTrace = true;
c.scene = "";
c.interval = 1;
c.trackSpeed = false;
c.displayTraceThickness = 0.01;
agent->setTrackOptions(TrackRobot(c));
}
if(tracksegm) {
TrackRobotConf c = TrackRobot::getDefaultConf();
Color col = osgHandle.getColor("joint");
c.displayTrace = true;
c.scene = "segm";
c.interval = 1;
c.displayTraceThickness = 0.02;
col.alpha() = 0.5;
agent->addTracking(5, TrackRobot(c), col);
agent->addTracking(8, TrackRobot(c), col);
}
global.agents.push_back(agent);
global.configs.push_back(agent);
//agent->startMotorBabblingMode(5000);
// this->getHUDSM()->setColor(Color(1.0,1.0,0));
// this->getHUDSM()->setFontsize(18);
// this->getHUDSM()->addMeasure(teacher,"gamma_s",ID,1);
}
}
// starting function (executed once at the beginning of the simulation loop)
void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
setCameraHomePos(Pos(-0.777389, 6.34573, 1.83396), Pos(-170.594, -5.10046, 0));
setCameraMode(Follow);
// initialization
// - set noise to 0.1
// - register file chess.ppm as a texture called chessTexture (used for the wheels)
global.odeConfig.setParam("noise", 0.05);
global.odeConfig.setParam("controlinterval", 1);
global.odeConfig.setParam("realtimefactor", 1);
// global.odeConfig.setParam("gravity", 0);
for(int i=0; i< 4; i++){
PassiveBox* b = new PassiveBox(odeHandle, osgHandle.changeColor(Color(.6, .6, .4)),
osg::Vec3(1,10,0.3+i*0.02),0);
b->setTexture(0,TextureDescr("Images/playfulmachines.rgb",1,1));
b->setPosition(osg::Vec3(-75+i*30,0,0));
global.obstacles.push_back(b);
}
controller=0;
/******* S L I D E R - w H E E L I E *********/
SliderWheelieConf mySliderWheelieConf = SliderWheelie::getDefaultConf();
mySliderWheelieConf.segmNumber = segmnum;
mySliderWheelieConf.motorPower = 5;
mySliderWheelieConf.jointLimitIn = M_PI/3;
// mySliderWheelieConf.frictionGround=0.5;
// mySliderWheelieConf.segmLength=1.4;
mySliderWheelieConf.sliderLength = 0;
mySliderWheelieConf.motorType = SliderWheelieConf::CenteredServo;
//mySliderWheelieConf.drawCenter = false;
vehicle = new SliderWheelie(odeHandle, osgHandle.changeColor(Color(1,222/255.0,0)),
mySliderWheelieConf, "Armband");
vehicle->place(Pos(0,0,.1));
SeMoXConf cc = SeMoX::getDefaultConf();
cc.cInit=1.1;
cc.modelExt=false;
cc.someInternalParams=false;
SeMoX* semox = new SeMoX(cc);
if(useSym){
semox->setParam("epsC", 0.1);
semox->setParam("epsA", 0.1);
}else{
semox->setParam("epsC", 0.1);
semox->setParam("epsA", 0.1);
}
semox->setParam("rootE", 3);
semox->setParam("s4avg", 1);
semox->setParam("gamma_cont", 0.005);
semox->setParam("gamma_teach", teacher);
//controller=semox;
controller = new CrossMotorCoupling( semox, semox, 0.4);
// One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1));
AbstractWiring* wiring = new FeedbackWiring(new ColorUniformNoise(0.1),
FeedbackWiring::Motor, 0.75);
//global.plotoptions.push_back(PlotOption(GuiLogger,Robot,5));
OdeAgent* agent = new OdeAgent(global);
agent->addCallbackable(&stats);
agent->init(controller, vehicle, wiring);
if(track) agent->setTrackOptions(TrackRobot(true,false,false, false, "uni", 20));
global.agents.push_back(agent);
global.configs.push_back(agent);
thisConfig.controller=controller;
global.configs.push_back(&thisConfig);
this->getHUDSM()->setColor(osgHandle.getColor("hud"));
this->getHUDSM()->setFontsize(16);
// this->getHUDSM()->setColor(Color(1.0,1.0,0));
// this->getHUDSM()->setFontsize(18);
this->getHUDSM()->addMeasure(teacher,"Gamma",ID,1);
this->getHUDSM()->addMeasure(thisConfig.D_display,"D",ID,1);
// if(useSym){
// int k= 0;
// std::list<int> perm;
// int len = controller->getMotorNumber();
// for(int i=0; i<len; i++){
// perm.push_back((i+k+(len)/2)%len);
// }
// CMC cmc = controller->getPermutationCMC(perm);
// controller->setCMC(cmc);
// }
}
