/// start() is called at the start and should create all the object (obstacles, agents...).
virtual void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
setCameraHomePos(Pos(8.85341, -11.2614, 3.32813), Pos(33.5111, -7.0144, 0));
// initialization
global.odeConfig.noise=0.1;
// Playground* playground = new Playground(odeHandle, osgHandle, osg::Vec3(7.0, 0.2, 1.5));
// playground->setPosition(Pos(0,0,0)); // playground positionieren und generieren
// global.obstacles.push_back(playground);
Arm2SegmConf arm_conf=Arm2Segm::getDefaultConf();
Arm2Segm* vehicle = new Arm2Segm(odeHandle, osgHandle, arm_conf, "arm");
((OdeRobot* )vehicle)->place(Position(0,0,0));
//AbstractController *controller = new InvertNChannelController(10);
AbstractController *controller = new SineController();
global.configs.push_back(vehicle);
One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1));
OdeAgent* agent = new OdeAgent(global);
agent->init(controller, vehicle, wiring);
global.agents.push_back(agent);
global.configs.push_back(controller);
}
// starting function (executed once at the beginning of the simulation loop)
void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
setCameraHomePos(Pos(5.2728, 7.2112, 3.31768), Pos(140.539, -13.1456, 0));
// initialization
global.odeConfig.setParam("noise",0.05);
global.odeConfig.setParam("realtimefactor",1);
global.odeConfig.setParam("drawinterval", 50);
OdeRobot* robot = new ShortCircuit(odeHandle, osgHandle, channels, channels);
// InvertMotorNStepConf cc = InvertMotorNStep::getDefaultConf();
// cc.cInit=1;
// cc.cNonDiag=0.5;
// cc.someInternalParams=false;
// AbstractController *controller = new InvertMotorNStep(cc);
AbstractController *controller = new InvertNChannelController(10);
// AbstractController *controller = new InvertNChannelController_NoBias(40);
controller->setParam("eps",0.2);
// controller->setParam("factor_a",0.00);
// controller->setParam("eps",0.01);
//AbstractController *controller = new InvertMotorSpace(10,1);
// AbstractController *controller = new SineController();
//controller->setParam("nomupdate",0.001);
controller->setParam("sinerate",100.0);
controller->setParam("phaseshift",0.6);
controller->setParam("adaptrate",0.000);
controller->setParam("epsA",0.00);
controller->setParam("epsC",0.06);
controller->setParam("factorB",0.0);
controller->setParam("noiseB",0.0);
controller->setParam("steps",1);
// AbstractController *controller = new InvertNChannelController(10);
//AbstractController *controller = new SineController();
OdeAgent* agent = new OdeAgent(global);
// sineNoise = new SineWhiteNoise(omega,2,M_PI/2);
// One2OneWiring* wiring = new One2OneWiring(sineNoise, true);
One2OneWiring* wiring = new One2OneWiring(new WhiteUniformNoise(), true);
// One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.05), true);
//AbstractWiring* wiring = new SelectiveOne2OneWiring(sineNoise, &select_firsthalf);
// DerivativeWiringConf c = DerivativeWiring::getDefaultConf();
// c.useId=true;
// c.useFirstD=false;
// c.derivativeScale=20;
// c.blindMotorSets=0;
// AbstractWiring* wiring = new DerivativeWiring(c, new ColorUniformNoise(0.05));
agent->init(controller, robot, wiring);
global.agents.push_back(agent);
global.configs.push_back(controller);
}
// starting function (executed once at the beginning of the simulation loop)
void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
setCameraHomePos(Pos(5.2728, 7.2112, 3.31768), Pos(140.539, -13.1456, 0));
// 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("gravity:",1); // normally at -9.81
// initialization
Playground* playground = new Playground(odeHandle, osgHandle, osg::Vec3(25, 0.2, 1.5));
playground->setPosition(osg::Vec3(0,0,0)); // playground positionieren und generieren
global.obstacles.push_back(playground);
for(int i=0; i<5; i++){
PassiveSphere* s =
new PassiveSphere(odeHandle,
osgHandle.changeColor(Color(184 / 255.0, 233 / 255.0, 237 / 255.0)), 0.2);
s->setPosition(Pos(i*0.5-2, i*0.5, 1.0));
s->setTexture("Images/dusty.rgb");
global.obstacles.push_back(s);
}
OdeAgent* agent;
AbstractWiring* wiring;
// OdeRobot* robot;
AbstractController *controller;
CaterPillar* myCaterPillar;
CaterPillarConf myCaterPillarConf = DefaultCaterPillar::getDefaultConf();
//******* R A U P E *********/
myCaterPillarConf.segmNumber=6;
myCaterPillarConf.jointLimit=M_PI/4;
myCaterPillarConf.motorPower=0.8;
myCaterPillarConf.frictionGround=0.04;
myCaterPillar = new CaterPillar ( odeHandle, osgHandle.changeColor(Color(0.0f,1.0f,0.0f)), myCaterPillarConf, "Raupe1");
((OdeRobot*) myCaterPillar)->place(Pos(-5,-5,0.2));
InvertMotorNStepConf invertnconf = InvertMotorNStep::getDefaultConf();
invertnconf.cInit=2.0;
controller = new InvertMotorNStep(invertnconf);
wiring = new One2OneWiring(new ColorUniformNoise(0.1));
agent = new OdeAgent( global, plotoptions );
agent->init(controller, myCaterPillar, wiring);
global.agents.push_back(agent);
global.configs.push_back(controller);
global.configs.push_back(myCaterPillar);
myCaterPillar->setParam("gamma",/*gb");
global.obstacles.push_back(s)0.0000*/ 0.0);
}
/**
* restart() is called at the second and all following starts of the cylce
* The end of a cycle is determined by (simulation_time_reached==true)
* @param the odeHandle
* @param the osgHandle
* @param globalData
* @return if the simulation should be restarted; this is false by default
*/
virtual bool restart(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
// for demonstration: just repositionize the robot and restart 10 times
if (this->currentCycle==10)
return false; // don't restart, just quit
// vehicle->place(Pos(currentCycle,0,0));
if (agent!=0) {
OdeAgentList::iterator itr = find(global.agents.begin(),global.agents.end(),agent);
if (itr!=global.agents.end())
{
global.agents.erase(itr);
}
delete agent;
agent = 0;
}
Nimm2Conf c = Nimm2::getDefaultConf();
c.force = 4;
c.bumper = true;
c.cigarMode = true;
// c.irFront = true;
OdeRobot* vehicle2 = new Nimm2(odeHandle, osgHandle, c, "Nimm2");
vehicle2->place(Pos(0,6+currentCycle*1.5,0));
// use Nimm4 vehicle as robot:
// - create pointer to nimm4 (with odeHandle and osg Handle and possible other settings, see nimm4.h)
// - place robot
//OdeRobot* vehicle = new Nimm4(odeHandle, osgHandle, "Nimm4");
//vehicle->place(Pos(0,1,0));
// create pointer to controller
// push controller in global list of configurables
// AbstractController *controller = new InvertNChannelController(10);
AbstractController *controller = new InvertMotorSpace(10);
global.configs.push_back(controller);
// create pointer to one2onewiring
One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1));
// create pointer to agent
// initialize pointer with controller, robot and wiring
// push agent in globel list of agents
OdeAgent* agent = new OdeAgent(global);
agent->init(controller, vehicle2, wiring);
global.agents.push_back(agent);
// restart!
return true;
}
// starting function (executed once at the beginning of the simulation loop)
void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
setCameraHomePos(Pos(5.2728, 7.2112, 3.31768), Pos(140.539, -13.1456, 0));
setCameraMode(Follow);
global.odeConfig.setParam("noise",0.1);
global.odeConfig.setParam("controlinterval",4);
global.odeConfig.setParam("realtimefactor",4);
// global.odeConfig.setParam("gravity", 0); // no gravity
for(int i=0; i<0; i++){
PassiveSphere* s = new PassiveSphere(odeHandle, osgHandle.changeColor(Color(0.0,1.0,0.0)), 0.5);
s->setPosition(osg::Vec3(5,0,i*3));
global.obstacles.push_back(s);
}
// Spherical Robot with axis orientation sensors:
Sphererobot3MassesConf conf = Sphererobot3Masses::getDefaultConf();
conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::ZProjection));
// regular behaviour
conf.motorsensor=false;
conf.diameter=1.0;
conf.pendularrange= 0.25;
conf.motorpowerfactor = 150;
// conf.diameter=1.0;
// conf.pendularrange= 0.35;
sphere1 = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(Color(1.0,0.0,0)),
conf, "Sphere1", 0.2);
((OdeRobot*)sphere1)->place ( Pos( 0 , 0 , 0.1 ));
global.configs.push_back ( sphere1 );
controller = new Sox(.8,true);
controller->setParam("epsA",0.3); // model learning rate
controller->setParam("epsC",1); // controller learning rate
controller->setParam("causeaware",0.4);
controller->setParam("pseudo",2);
global.configs.push_back ( controller );
One2OneWiring* wiring = new One2OneWiring ( new ColorUniformNoise() );
OdeAgent* agent = new OdeAgent ( global );
agent->init ( controller , sphere1 , wiring );
if(track) agent->setTrackOptions(TrackRobot(true, true, true, false, "zaxis", 20));
global.agents.push_back ( agent );
}
/// start() is called at the start and should create all the object (obstacles, agents...).
virtual void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
// Initial position and orientation of the camera (use 'p' in graphical window to find out)
setCameraHomePos(Pos(-14,14, 10), Pos(-135, -24, 0));
// Some simulation parameters can be set here
global.odeConfig.setParam("controlinterval", 1);
global.odeConfig.setParam("gravity", -9.8);
/** New robot instance */
// Get the default configuration of the robot
DifferentialConf conf = Differential::getDefaultConf();
// Values can be modified locally
conf.wheelMass = .5;
// Instantiating the robot
OdeRobot* robot = new Differential(odeHandle, osgHandle, conf, "Differential robot");
// Placing the robot in the scene
((OdeRobot*)robot)->place(Pos(.0, .0, .2));
// Instantiatign the controller
AbstractController* controller = new BasicController("Basic Controller", "$ID$");
// Create the wiring with color noise
AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise(.1));
// Create Agent
OdeAgent* agent = new OdeAgent(global);
// Agent initialisation
agent->init(controller, robot, wiring);
// Adding the agent to the agents list
global.agents.push_back(agent);
global.configs.push_back(agent);
/** Environment and obstacles */
// New playground
Playground* playground = new Playground(odeHandle, osgHandle,osg::Vec3(15., .2, 1.2), 1);
// Set colours
playground->setGroundColor(Color(.784, .784, .0));
playground->setColor(Color(1., .784, .082, .3));
// Set position
playground->setPosition(osg::Vec3(.0, .0, .1));
// Adding playground to obstacles list
global.obstacles.push_back(playground);
// Add a new box obstacle (or use 'o' to drop random obstacles)
//PassiveBox* box = new PassiveBox(odeHandle, osgHandle, osg::Vec3(1., 1., 1.), 2.);
//box->setPose(osg::Matrix::translate(-.5, 4., .7));
//global.obstacles.push_back(box);
}
// starting function (executed once at the beginning of the simulation loop)
void OpenLoopSim::start(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
GlobalData& global) {
int num_barrels=1;
setCameraHomePos(Pos(-1.29913, 6.06201, 1.36947), Pos(-165.217, -9.32904, 0));
setCameraMode(Follow);
// initialization
global.odeConfig.setParam("noise",0);
// the simulation runs with 100Hz and we control every second
global.odeConfig.setParam("controlinterval",2);
// add a new parameter to be configured on the console
global.odeConfig.addParameterDef("friction", &friction, 0.0, "rolling friction coefficient");
global.odeConfig.addParameterDef("color", &color, 0, "color of noise (correlation length)");
/* * * * BARRELS * * * */
for(int i=0; i< num_barrels; i++){
//****************
Sphererobot3MassesConf conf = Barrel2Masses::getDefaultConf();
conf.pendularrange = 0.3;//0.15;
conf.motorpowerfactor = 200;//150;
conf.motorsensor=false;
conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::ZProjection, Sensor::X | Sensor::Y));
conf.spheremass = 1;
robot = new Barrel2Masses ( odeHandle, osgHandle.changeColor(Color(0.0,0.0,1.0)),
conf, "Barrel", 0.4);
robot->place (osg::Matrix::rotate(M_PI/2, 1,0,0) /* rotate to laying position */
* osg::Matrix::rotate(M_PI/4, 0,1,0) /* both axis at 45Deg*/
* osg::Matrix::translate(0,0,.15+2*i)); // place at 0,0 and in some height
controller = new SineController();
controller->setParam("period", 300);
controller->setParam("phaseshift", 1);
noisegen = new ColorUniformNoise();
wiring = new MotorNoiseWiring(noisegen, 0);
OdeAgent* agent = new OdeAgent ( global );
agent->init ( controller , robot , wiring );
global.agents.push_back ( agent );
global.configs.push_back ( agent );
}
}
// starting function (executed once at the beginning of the simulation loop)
void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
int num_barrels=1;
int num_barrels_test=0;
sensor=0;
setCameraMode(Follow);
bool normalplayground=false;
// setCameraHomePos(Pos(-0.497163, 11.6358, 3.67419), Pos(-179.213, -11.6718, 0));
setCameraHomePos(Pos(-2.60384, 13.1299, 2.64348), Pos(-179.063, -9.7594, 0));
// initialization
global.odeConfig.setParam("noise",0.1);
// global.odeConfig.setParam("gravity",-10);
global.odeConfig.setParam("controlinterval",4);
global.odeConfig.setParam("realtimefactor",5);
// add a new parameter to be configured on the console
global.odeConfig.addParameterDef("friction", &friction, 0.1, "rolling friction coefficient");
if(normalplayground){
Playground* playground = new Playground(odeHandle, osgHandle,osg::Vec3(20, 0.01, 0.01 ), 1);
playground->setGroundColor(Color(255/255.0,200/255.0,0/255.0));
playground->setGroundTexture("Images/really_white.rgb");
playground->setColor(Color(255/255.0,200/255.0,21/255.0, 0.1));
playground->setPosition(osg::Vec3(0,0,0.05));
global.obstacles.push_back(playground);
}
/* * * * BARRELS * * * */
for(int i=0; i< num_barrels; i++){
//****************
Sphererobot3MassesConf conf = Barrel2Masses::getDefaultConf();
conf.pendularrange = 0.3;//0.15;
conf.motorpowerfactor = 200;//150;
conf.motorsensor=false;
conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::ZProjection, Sensor::X | Sensor::Y));
conf.spheremass = 1;
conf.addSensor(new SpeedSensor(10, SpeedSensor::Translational, Sensor::X ));
conf.irAxis1=false;
conf.irAxis2=false;
conf.irAxis3=false;
conf.axesShift = 0;
// conf.axesShift = conf.diameter/2 - conf.pendularrange/2;
sphere1 = new Barrel2Masses ( odeHandle, osgHandle.changeColor(Color(0.0,0.0,1.0)),
conf, "Barrel1", 0.4);
sphere1->place (osg::Matrix::rotate(M_PI/2, 1,0,0)*osg::Matrix::translate(0,0,0.2));
// InvertMotorNStepConf cc = InvertMotorNStep::getDefaultConf();
// cc.cInit=1;
// // cc.useSD=true;
// controller = new InvertMotorNStep(cc);
// controller->setParam("steps", 2);
// controller->setParam("adaptrate", 0.0);
// controller->setParam("nomupdate", 0.00);
// controller->setParam("epsC", 0.03);
// controller->setParam("epsA", 0.05);
// controller->setParam("rootE", 0);
// controller->setParam("logaE", 0);
//controller = new PiMax();
//controller->setParam("epsC", 0.001);
// controller = new Sox();
// controller->setParam("epsC", 0.5);
controller = new ROSController("Test");
AbstractWiring* wiring = new SelectiveOne2OneWiring(new ColorUniformNoise(), new select_from_to(0,1));
// OdeAgent* agent = new OdeAgent ( PlotOption(File, Robot, 1) );
OdeAgent* agent = new OdeAgent ();
agent->init ( controller , sphere1 , wiring );
// agent->setTrackOptions(TrackRobot(true, false, false, "ZSens_Ring10_11", 50));
global.agents.push_back ( agent );
global.configs.push_back ( controller );
}
/* * * * TEST BARRELS * * * */
for(int i=0; i< num_barrels_test; i++){
global.odeConfig.setParam("realtimefactor",1);
//****************
Sphererobot3MassesConf conf = Sphererobot3Masses::getDefaultConf();
conf.pendularrange = 0.15;
conf.motorsensor=true;
conf.irAxis1=false;
conf.irAxis2=false;
conf.irAxis3=false;
conf.spheremass = 1;
sphere1 = new Barrel2Masses ( odeHandle, osgHandle.changeColor(Color(0.0,0.0,1.0)),
conf, "Barrel1", 0.4);
sphere1->place ( osg::Matrix::rotate(M_PI/2, 1,0,0));
controller = new SineController();
controller->setParam("sinerate", 15);
controller->setParam("phaseshift", 0.45);
// DerivativeWiringConf dc = DerivativeWiring::getDefaultConf();
// dc.useId=true;
// dc.useFirstD=false;
// AbstractWiring* wiring = new DerivativeWiring(dc,new ColorUniformNoise());
AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise());
OdeAgent* agent = new OdeAgent ();
agent->init ( controller , sphere1 , wiring );
// agent->setTrackOptions(TrackRobot(true, false, false, "ZSens_Ring10_11", 50));
global.agents.push_back ( agent );
global.configs.push_back ( controller );
}
}
virtual void addCallback(GlobalData& global, bool draw, bool pause, bool control) {
if(!control || pause) return;
if(counter == maxCounter){
counter = 0;
if(sphere) {
// measure final speed
double dat[3];
speedsensor->sense(global);
speedsensor->get(dat,3);
Matrix s(1,3,dat);
logfile << s << endl;
cout << "End: " << s << endl;
if(runcounter>=startAngles.size()){
this->pause=true;
simulation_time_reached=true;
return;
}
// destroy sphere
global.agents.pop_back();
delete sphere;
delete controller;
delete wiring;
delete agent;
}
// create new sphere
sphere = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(Color(0,0.0,2.0)),
conf, "Sphere_test_sat", 0.3);
// we will accelerate the sphere in positive x direction
// we place it the way that all (straight) rolling modes are possible
double theta = startAngles[runcounter].val(0,0);
double omega = startAngles[runcounter].val(1,0);
sphere->place ( osg::Matrix::rotate(omega, 0,0,1)*osg::Matrix::rotate(theta, 1,0,0));
// sphere->place ( osg::Matrix::rotate(M_PI/2, 0,1,0));
// sphere->place ( osg::Matrix::rotate(M_PI/2, 0,0,1));
speedsensor->init(sphere->getMainPrimitive());
// controller = new SatNetControl(networkfilename);
controller = new SatNetControl_NoY(networkfilename);
wiring = new One2OneWiring ( new ColorUniformNoise(0.20) );
agent = new OdeAgent ( plotoptions );
agent->init ( controller , sphere , wiring );
// agent->setTrackOptions(TrackRobot(true, true, false, true, "ZSens", 50));
global.agents.push_back ( agent );
runcounter++;
} else if(counter > 1 && counter < accelerationTime){
// speed up sphere
double power = startAngles[runcounter-1].val(2,0);
dBodyAddTorque ( sphere->getMainPrimitive()->getBody() , 0, power, 0 );
}else if(counter == accelerationTime){ // measure start speed
double dat[3];
speedsensor->sense(global);
speedsensor->get(dat,3);
Matrix s(1,3,dat);
logfile << s;
cout << "Run: " << runcounter << "/" << startAngles.size() << endl;
cout << "Start: " << s << endl;
}
counter++;
}
// starting function (executed once at the beginning of the simulation loop)
void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
setCameraHomePos(Pos(46.8304, -1.4434, 19.3963), Pos(88.9764, -26.2964, 0));
OdeHandle elast = odeHandle;
elast.substance.toMetal(0.8);
// initialization
// - set global noise to 0.1
global.odeConfig.setParam("noise",0.01);
// global.odeConfig.setParam("gravity", 0); // no gravity
Playground* playground1 = new Playground(elast, osgHandle, osg::Vec3(30.0, 0.2, 1.0), 1, true);
playground1->setColor(Color(0.88f,0.4f,0.26f,0.2f));
playground1->setTexture("Images/really_white.rgb");
playground1->setGroundColor(Color(200/255.0,174.0/255.0,21.0/255.0));
playground1->setGroundTexture("Images/really_white.rgb");
playground1->setPosition(osg::Vec3(0,0,0.0)); // playground positionieren und generieren
global.obstacles.push_back(playground1);
// Agents
numrobots = 2;
ForcedSphereConf conf;
ForcedSphere** spheres = new ForcedSphere*[numrobots];
Sensor** sensors = new Sensor*[numrobots];
controllers = new AbstractController*[numrobots];
AbstractWiring* wiring;
OdeAgent* agent;
for(int i=0; i<numrobots; i++){
conf = ForcedSphere::getDefaultConf();
// conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::OnlyZAxis));
// RelativePositionSensor* s = new RelativePositionSensor(4,1,Sensor::X | Sensor::Y);
// s->setReference(playground1->getMainPrimitive());
SpeedSensor* s = new SpeedSensor(5,SpeedSensor::Translational,
Sensor::X | Sensor::Y);
conf.addSensor(s);
conf.addSensor(new SoundSensor());
conf.addMotor(new Speaker(-1));
conf.maxForce = 10;
conf.speedDriven = true;
conf.maxSpeed = 5;
conf.radius = 0.5;
conf.cylinderBody=true;
ForcedSphere* sphere1;
sphere1 = new ForcedSphere ( elast, osgHandle.changeColor(Color(i==0,i==1,i==2)),
conf, "Agent1");
((OdeRobot*)sphere1)->place ( Pos( 2*i , 0 , 0.1 ));
InvertMotorNStepConf cc = InvertMotorNStep::getDefaultConf();
cc.useSD=true;
controller = new InvertMotorNStep(cc);
controller->setParam("epsA",0.03); // model learning rate
controller->setParam("epsC",0.01); // controller learning rate
controller->setParam("rootE",3); // model and contoller learn with square rooted error
controller->setParam("factorB",0.2);
controller->setParam("noiseB",0.01);
controller->setParam("s4avg",10);
controller->setParam("adaptrate",0.000);
controller->setParam("nomupdate",0.001);
controller->setParam("noiseY",0.0);
// controller = new SineController();
// controller = new InvertNChannelController(10,1.2);
// controller->setParam("eps",0.2);
global.configs.push_back ( controller );
wiring = new One2OneWiring ( new ColorUniformNoise() );
// DerivativeWiringConf wc = DerivativeWiring::getDefaultConf();
// wc.useId=false;
// wc.useSecondD=true;
// wc.eps=1;
// wc.derivativeScale=100;
// wiring = new DerivativeWiring ( wc, new ColorUniformNoise());
agent = new OdeAgent ( i==0 ? plotoptions : std::list<PlotOption>() );
agent->init ( controller , sphere1 , wiring );
global.agents.push_back ( agent );
spheres[i]=sphere1;
sensors[i]=s;
controllers[i]=controller;
}
// connect them
// let robot 2 actually persive robot 1
// sensors[1]->init(spheres[0]->getMainPrimitive());
myspeaker=0;
// myspeaker = new Speaker(1);
// myspeaker->init(playground1->getMainPrimitive());
}
/// start() is called at the start and should create all the object (obstacles, agents...).
virtual void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global){
setCameraHomePos(Pos(1.86452, 21.1998, 8.7964), Pos(174.993, -10.688, 0));
setCameraMode(Follow);
global.odeConfig.setParam("controlinterval",1);
global.odeConfig.setParam("gravity", -9.81);
global.odeConfig.setParam("noise", 0);
global.odeConfig.setParam("realtimefactor", 1.);
int sliderwheelies = 1;
int snakes = 0;
Playground* playground = new Playground(odeHandle, osgHandle,
osg::Vec3(120, 0.2, 2.5),.5,true);
// playground->setColor(Color(1,0.2,0,0.1));
playground->setPosition(osg::Vec3(0,0,0)); // playground positionieren und generieren
global.obstacles.push_back(playground);
// Creation of Obstacle
PassiveBox* b =
new PassiveBox(odeHandle,
osgHandle.changeColor(Color(180.0 / 255.0, 180.0 / 255.0, 130.0 / 255.0)),
osg::Vec3(4.0, 20.0, .8), 0);
b->setTexture(0,TextureDescr("Images/playfulmachines.rgb",1,1));
b->setPosition(Pos(-32, 0, 0));
global.obstacles.push_back(b);
/******* S L I D E R - W H E E L I E *********/
for(int i=0; i<sliderwheelies; i++){
SliderWheelieConf mySliderWheelieConf = SliderWheelie::getDefaultConf();
mySliderWheelieConf.segmNumber=12;
mySliderWheelieConf.jointLimitIn=M_PI/3;
mySliderWheelieConf.frictionGround=0.5;
mySliderWheelieConf.motorPower=8;
mySliderWheelieConf.motorDamp=0.05;
mySliderWheelieConf.sliderLength=0.5;
mySliderWheelieConf.segmLength=1.4;
OdeRobot* robot = new SliderWheelie(odeHandle, osgHandle, mySliderWheelieConf, "Slider Armband");
robot->place(Pos(0,0,2.0));
//controller = new Sos(1.0);
controller = new OneControllerPerChannel(new ControlGen(),"OnePerJoint");
AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.05));
OdeAgent* agent = new OdeAgent(global);
// only the first controller is exported to guilogger and Co
agent->addInspectable(((OneControllerPerChannel*)controller)->getControllers()[0]);
agent->init(controller, robot, wiring);
if(track)
agent->setTrackOptions(TrackRobot(true,true,false,false,"split_control",1));
global.agents.push_back(agent);
global.configs.push_back(agent);
}
//****** SNAKES **********/
for(int i=0; i<snakes; i++){
//****************/
SchlangeConf conf = Schlange::getDefaultConf();
// conf.segmMass = .2;
// conf.segmLength= .5// 0.8;
// conf.segmDia=.6;
// conf.motorPower=.5;
conf.segmNumber = 12+2*i;//-i/2;
conf.jointLimit=conf.jointLimit* 1.6;
conf.frictionJoint=0.02;
conf.useServoVel=true;
SchlangeServo2* schlange1;
if (i==0) {
schlange1 =
new SchlangeServo2 ( odeHandle, osgHandle.changeColor(Color(0.1, 0.3, 0.8)),
conf, "S1");
} else {
schlange1 =
new SchlangeServo2 ( odeHandle, osgHandle.changeColor(Color(0.1, 0.3, 0.8)),
conf, "S2");
}
//Positionieren und rotieren
schlange1->place(osg::Matrix::rotate(M_PI/2,0, 1, 0)*
osg::Matrix::translate(-.7+0.7*i,0,(i+1)*(.2+conf.segmNumber)/2.0/*+2*/));
// osg::Matrix::translate(5-i,2 + i*2,height+2));
schlange1->setTexture("Images/whitemetal_farbig_small.rgb");
if (i==0) {
schlange1->setHeadColor(Color(1.0,0,0));
} else {
schlange1->setHeadColor(Color(0,1.0,0));
}
OneControllerPerChannel *controller = new OneControllerPerChannel(new ControlGen(),"OnePerJoint");
AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.05));
OdeAgent* agent = new OdeAgent(global);
agent->addInspectable(controller->getControllers()[0]);
agent->init(controller, schlange1, wiring);
global.agents.push_back(agent);
global.configs.push_back(agent);
}//creation of snakes End
}
// starting function (executed once at the beginning of the simulation loop)
void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
setCameraHomePos(Pos(5.2728, 7.2112, 3.31768), Pos(140.539, -13.1456, 0));
// initialization
// - set noise to 0.1
// - register file chess.ppm as a texture called chessTexture (used for the wheels)
global.odeConfig.noise=0.05;
// global.odeConfig.setParam("gravity", 0);
global.odeConfig.setParam("controlinterval", 5);
global.odeConfig.setParam("simstepsize", 0.01);
// use Playground as boundary:
OctaPlayground* playground = new OctaPlayground(odeHandle, osgHandle, osg::Vec3(11, 0.2, 1), 12);
playground->setPosition(osg::Vec3(0,0,0)); // playground positionieren und generieren
global.obstacles.push_back(playground);
/*
for(int i=0; i<50; i++){
PassiveSphere* s = new PassiveSphere(odeHandle, osgHandle.changeColor(Color(0.0,1.0,0.0)), 0.5);
s->setPosition(osg::Vec3(-4+(i/10),-4+(i%10),1));
global.obstacles.push_back(s);
}
*/
OdeRobot* nimm2;
// AbstractController* contrl;
AbstractWiring* wiring;
OdeAgent* agent;
Nimm2Conf conf = Nimm2::getDefaultConf();
conf.speed=10;
conf.force=1.0;
//conf.bumper=true;
//conf.cigarMode=true;
//conf.irFront=true;
//conf.irBack=true;
wiring = new One2OneWiring(new ColorUniformNoise(0.1));
//controller = new InvertMotorNStep();
controller = new InvertNChannelController(10);
// controller = new InvertMotorSpace(10);
agent = new OdeAgent(global);
nimm2 = new Nimm2(odeHandle, osgHandle, conf, "Nimm2Yellow");
nimm2->setColor(Color(1.0,1.0,0));
global.configs.push_back(controller);
agent->init(controller, nimm2, wiring);
/* controller->setParam("adaptrate", 0.000);
controller->setParam("nomupdate", 0.0005);
controller->setParam("epsA", 0.01);
controller->setParam("epsC", 0.05);
controller->setParam("rootE", 0);
controller->setParam("steps", 2);
controller->setParam("s4avg", 5);
controller->setParam("s4del", 5);
controller->setParam("factorB",0);*/
controller->setParam("eps",0.1);
controller->setParam("factor_a",0.01);
nimm2->place(Pos(2.5,1.26,0));
global.agents.push_back(agent);
}
// starting function (executed once at the beginning of the simulation loop)
void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
setCameraHomePos(Pos(6.5701, 17.3534, 11.7749), Pos(159.449, -30.0839, 0));
// initialization
// - set noise to 0.1
// - register file chess.ppm as a texture called chessTexture (used for the wheels)
global.odeConfig.noise=0.1;
// global.odeConfig.setParam("gravity", 0);
// int chessTexture = dsRegisterTexture("chess.ppm");
// use Playground as boundary:
// - create pointer to playground (odeHandle contains things like world and space the
// playground should be created in; odeHandle is generated in simulation.cpp)
// - setting geometry for each wall of playground:
// setGeometry(double length, double width, double height)
// - setting initial position of the playground: setPosition(double x, double y, double z)
// - push playground in the global list of obstacles(globla list comes from simulation.cpp)
OctaPlayground* playground =
new OctaPlayground(odeHandle, osgHandle,
osg::Vec3(8.0f, 0.2, 6.0f),
12, // number of corners
true); // if ground is created (only create one)
// true); // if ground is created (only create one)
// when you not set this texture, a brown wooden texture
// is used, the setColor would overlay with the wooden texture
playground->setColor(Color(1.0,0.5,0.0,0.2));
// when you not set this texture, a brown wooden texture
// is used, the setColor would overlay with the wooden texture
playground->setTexture("Images/really_white.rgb");
playground->setPosition(osg::Vec3(0,0,0)); // playground positionieren und generieren
global.obstacles.push_back(playground);
OctaPlayground* playground2 =
new OctaPlayground(odeHandle, osgHandle,
osg::Vec3(1.0f, 0.2, 2.5f),
12, // number of corners
false); // if ground is created (only create one)
playground2->setColor(Color(1.0,0.5,0.0,0.1));
// when you not set this texture, a brown wooden texture
// is used, the setColor would overlay with the wooden texture
playground2->setTexture("Images/really_white.rgb");
playground2->setPosition(osg::Vec3(0.0,0.0,0)); // playground positionieren und generieren
global.obstacles.push_back(playground2);
// Creation of passive boxes
// add passive spheres, boxes and capsules as obstacles
// - create pointer to sphere, box or capsule (with odehandle, osghandle and
// optional parameters radius and mass,where the latter is not used here) )
// - set Pose(Position) of sphere, box or capsule
// - set a texture for the sphere, box or capsule
// - add sphere, box or capsule to list of obstacles
// note that the dependent header file has to be include above
// ("passivesphere.h", "passivebox.h" and/or "passivecapsule.h"
int n=10;
int m=3;
for (int j=0;j<m;j++) {
for(int i=0; i<n; i++){
PassiveSphere* s =
new PassiveSphere(odeHandle,
osgHandle.changeColor(Color(184 / 255.0, 233 / 255.0, 237 / 255.0)),
/*Diameter=*/.2+i*0.03,/*Mass=*/.5);
s->setPosition(Pos(sin(2*M_PI*(((float)i)/((float)n)+0.05f))*(3.0f+2.0f*j),
cos(2*M_PI*(((float)i)/((float)n)+0.05f))*(3.0f+2.0f*j),
0.8f+2.0f*j));
s->setTexture("Images/dusty.rgb");
global.obstacles.push_back(s);
}
}
n=8;
m=3;
for (int j=0;j<m;j++) {
for(int i=0; i<n; i++){
PassiveBox* b =
new PassiveBox(odeHandle,
osgHandle.changeColor(Color(184 / 255.0, 233 / 255.0, 237 / 255.0)),
osg::Vec3(0.4+i*0.1, 0.8-i*0.03, 0.4), 0.2+i*0.1);
if (i==1)
b->setTexture("Images/light_chess.rgb");
else
b->setTexture("Images/dusty.rgb");
b->setPosition(Pos(sin(2*M_PI*(((float)i)/((float)n))+0.1f)*(3.0f+2.0f*j),
cos(2*M_PI*(((float)i)/((float)n))+0.1f)*(3.0f+2.0f*j),
3.0f+2.0f*j));
global.obstacles.push_back(b);
}
}
n=15;
m=3;
for (int j=0;j<m;j++) {
for(int i=0; i<n; i++){
PassiveCapsule* b =
new PassiveCapsule(odeHandle,
osgHandle.changeColor(Color(184 / 255.0, 233 / 255.0, 237 / 255.0)),
0.2f+0.03*i,1.0f-i*0.04);
b->setPosition(Pos(sin(2*M_PI*(((float)i)/((float)n)))*(3.0f+2.0f*j),
cos(2*M_PI*(((float)i)/((float)n)))*(3.0f+2.0f*j),
5.0f+2.0f*j));
b->setTexture("Images/dusty.rgb");
global.obstacles.push_back(b);
}
}
// Creation of spherical robots:
for(int i=0; i<0; i++){
OdeRobot* sphere1;
Sphererobot3MassesConf conf = Sphererobot3Masses::getDefaultConf();
conf.diameter=2;
conf.irAxis1=true;
conf.irAxis2=true;
conf.irAxis3=true;
sphere1 = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(Color(0.0+.1*i,0.0,1.0)),
conf, "Sphere1", 0.4);
// sphere1 = new ForcedSphere(odeHandle, osgHandle, "FSphere");
// sphere1->place ( Pos(-3,1/2,3+2*i));
sphere1->place ( Pos(0,0,7+2*i));
AbstractController* controller = new InvertMotorNStep();
controller->setParam("steps", 2);
controller->setParam("adaptrate", 0.005);
controller->setParam("nomupdate", 0.01);
controller->setParam("epsC", 0.01);
controller->setParam("epsA", 0.005);
controller->setParam("rootE", 3);
DerivativeWiringConf c = DerivativeWiring::getDefaultConf();
c.useId = true;
c.useFirstD = true;
DerivativeWiring* wiring = new DerivativeWiring ( c , new ColorUniformNoise() );
// One2OneWiring* wiring = new One2OneWiring ( new ColorUniformNoise() );
OdeAgent* agent = new OdeAgent ( plotoptions );
agent->init ( controller , sphere1 , wiring );
// agent->setTrackOptions(TrackRobot(true, false, false, "ZSens_Ring10_11", 50));
global.agents.push_back ( agent );
global.configs.push_back ( controller );
}
//creation of snakes
for(int i=0; i<1; i++){
//****************/
SchlangeConf conf = Schlange::getDefaultConf();
conf.motorPower=.3;
conf.segmNumber =10-i/2;
conf.segmDia = 0.15; // diameter of a snake element
// conf.jointLimit=conf.jointLimit*3;
conf.jointLimit=conf.jointLimit*2.0;
conf.frictionJoint=0.002;
SchlangeServo2* schlange1 =
//new SchlangeServo2 ( odeHandle, osgHandle.changeColor(Color(0.8, 0.3, 0.5)),
new SchlangeServo2 ( odeHandle, osgHandle.changeColor(Color(1.0, 1.0, 1.0)),
conf, "S1");
//Positionieren und rotieren
schlange1->place(osg::Matrix::rotate(M_PI/2, 0, 1, 0)*
osg::Matrix::translate(2*i,i,conf.segmNumber/2+2));
if (i==0) {
// schlange1->setTexture("Images/whitemetal_tiefgruen.rgb");
// schlange1->setHeadTexture("Images/whitemetal_tiefrot.rgb");
} else {
// schlange1->setTexture("Images/whitemetal_tiefrot.rgb");
// schlange1->setHeadTexture("Images/whitemetal_tiefgruen.rgb");
}
//AbstractController *controller = new InvertNChannelController(100/*,true*/);
// AbstractController *controller = new InvertMotorSpace(100/*,true*/);
AbstractController *controller = new InvertMotorNStep();
// AbstractController *controller = new invertmotornstep();
//AbstractController *controller = new SineController();
// AbstractController *controller = new InvertMotorNStep();
// AbstractController *controller = new SineController();
// AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.05)); //Only this line for one2Onewiring
// AbstractWiring* wiring = new DerivativeWiring(c, new ColorUniformNoise(0.1));
DerivativeWiringConf c = DerivativeWiring::getDefaultConf();
c.useId = true;
c.useFirstD = true;
// c.derivativeScale=10;
DerivativeWiring* wiring = new DerivativeWiring ( c , new ColorUniformNoise() );
OdeAgent* agent = new OdeAgent(global);
agent->init(controller, schlange1, wiring);
global.agents.push_back(agent);
global.configs.push_back(controller);
global.configs.push_back(schlange1);
global.odeConfig.setParam("controlinterval",2);
global.odeConfig.setParam("gravity", -2);
controller->setParam("steps",2);
controller->setParam("epsC",0.01);
controller->setParam("epsA",0.01);
controller->setParam("adaptrate",0.005);
controller->setParam("logaE",3);
// controller->setParam("desens",0.0);
controller->setParam("s4delay",3.0);
// controller->setParam("s4avg",1.0);
controller->setParam("factorB",0.0);
// controller->setParam("zetaupdate",0.1);
}//creation of snakes End
}
/**************************Reset Function***************************************************************/
virtual bool restart(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
std::cout << "\n begin restart " << currentCycle << "\n";
std::cout<<"Current Cycle"<<this->currentCycle<<std::endl;
//Temporary variable storing the pointer to the controller if the latter should not be resetted
AbstractController* temp_controller;
//OdeAgent* temp_agent;
// We would like to have 10 runs!
// after it we must clean all and return false because we don't want a new restart
if (this->currentCycle == repeat_number) // This will be delete all after finish simulation!!!!!!
{
//clean robots
while (global.agents.size() > 0)
{
OdeAgent* agent = *global.agents.begin();
AbstractController* controller = agent->getController();
OdeRobot* robot = agent->getRobot();
AbstractWiring* wiring = agent->getWiring();
global.configs.erase(std::find(global.configs.begin(),
global.configs.end(), controller));
delete controller;///////////////////////////////////////////////This will finally delete which is OK
delete robot;
delete wiring;
delete (agent);
global.agents.erase(global.agents.begin());
}
// clean the playgrounds
while (global.obstacles.size() > 0)
{
std::vector<AbstractObstacle*>::iterator iter =
global.obstacles.begin();
delete (*iter);
global.obstacles.erase(iter);
}
std::cout << "end.";
return false; // don't restart, just quit
}
// Now we must delete all robots and agents from the simulation and create new robots and agents.
// BUT NOT CONTROLLER for learning
while (global.agents.size() > 0)
{
// std::cout << "\n MAIN WHILE LOOP" << currentCycle << "\n";
OdeAgent* agent = *global.agents.begin();
AbstractController* controller = agent->getController();
OdeRobot* robot = agent->getRobot();
AbstractWiring* wiring = agent->getWiring();
global.configs.erase(std::find(global.configs.begin(),
global.configs.end(), controller));
delete robot;
delete wiring;
//DON'T delete controller if DON't want to reset controller!------------------------------------------------------(FRANK)
if(delete_controller)
{
delete controller;
//this calls destroy:
} else { // instead save the pointer in a temporary variable
temp_controller = controller;
// temp_agent = agent;
}
//Need to add put the current controller to temp controller?????----STILL NOT IMPLEMENT-----------------------------(FRANK)
//delete (agent);
global.agents.erase(global.agents.begin());
// std::cout << "\n END OF MAIN WHILE LOOP" << currentCycle << "\n";
}
///////////////Recreate Robot Start//////////////////////////////////////////////////////////////////////////////////////
//1) Activate IR sensors
FourWheeledConf fconf =FourWheeledRPos::getDefaultConf();
///2) relative sensors
for (int i=0; i < number_spheres; i++)
{
fconf.rpos_sensor_references.push_back(obst.at(i)->getMainPrimitive());
}
OdeRobot* vehicle3 = new FourWheeledRPos(odeHandle, osgHandle, fconf, "FourWheeled");
if(random_positon)
{
/*******Generating Random Position****/
//srand (time(NULL));
random_position = ((MAX_x-MIN_x)*((float)rand()/RAND_MAX))+MIN_x; // Input 0 (m) between -2.4 and 2.4
/************************************/
do
{
random_positionx = ((MAX_x-MIN_x)*((float)rand()/RAND_MAX))+MIN_x;
random_positiony = ((MAX_y-MIN_y)*((float)rand()/RAND_MAX))+MIN_y;
//std::cout<<"\n\n\n\n\n"<<"Inital Random_Robot X position"<<" = "<<random_position<<"\t"<<"Inital Random_Robot Y position"<<" = "<<-1*random_position<<"\n\n\n\n";
}while(abs(abs(random_position_S) - abs(random_positionx)) <= 4 || abs(abs(pos_obstacle_x) - abs(random_positionx)) <= 1.5 || abs(random_positiony) <= 0.5);
std::cout<<"\n\n"<<"Reset Inital Random X position"<<" = "<<random_positionx<<"\t"<<"Reset Inital Random Y position"<<" = "<<random_positiony<<"\n\n";
vehicle3->place(Pos(random_positionx/*10 +x = to left, -x = to right*/, random_positiony /* +y = to close, -y = to far*/,0/*z*/));
}
else
{
vehicle3->place(Pos(position_x-5.0/*x, +x = to left, -x = to right*/,0.0/*y*/,0.0/*z*/));
}
if(random_orientation)
{
/*******Generating Random Position****/
//srand (time(NULL));
int r = rand();
std::cout << "random value 3: " << r << std::endl;
random_or = ((MAX_or-MIN_or)*((float)r/RAND_MAX))+MIN_or; // between -pi/3 (60 deg) and pi/3 (60 deg)
/************************************/
Pos pos(position_x-5.0/*5.5x, +x = to left, -x = to right*/,0.0/*y*/,0.0/*z*/);
std::cout << "random_or2: " << random_or<<std::endl;
//setting position and orientation
vehicle3->place(osg::Matrix::rotate(random_or, 0, 0, 1) *osg::Matrix::translate(pos));
//setting only position
//vehicle3->place(Pos(position_x-5.0/*5.5x, +x = to left, -x = to right*/,0.0/*y*/,0.0/*z*/));
}
else
{
Pos pos(position_x-5.0/*5.5x, +x = to left, -x = to right*/,0.0/*y*/,0.0/*z*/);
//setting position and orientation
vehicle3->place(osg::Matrix::rotate(0.0, 0, 0, 1) *osg::Matrix::translate(pos));
//setting only position
//vehicle3->place(Pos(position_x-5.0/*5.5x, +x = to left, -x = to right*/,0.0/*y*/,0.0/*z*/));
}
if(repeat_experiment)
{
qcontroller = (EmptyController*)temp_controller;
}
global.configs.push_back(qcontroller);
// create pointer to one2onewiring
AbstractWiring* wiring3 = new One2OneWiring(new ColorUniformNoise(0.1));
// create pointer to agent
plotoptions.push_back(PlotOption(NoPlot /*select "File" to save signals, "NoPlot" to not save*/));
OdeAgent* agent3 = new OdeAgent(plotoptions);
//OdeAgent* agent3 = new OdeAgent(global);
agent3->init(qcontroller, vehicle3, wiring3);
if(drawtrace_on)
{
TrackRobot* track3 = new TrackRobot(/*bool trackPos*/true/*true*/,
/*bool trackSpeed*/false,
/*bool trackOrientation*/false,
/*bool displayTrace*/ true //,
/*const char *scene="", int interval=1*/);
agent3->setTrackOptions(*track3);
}
global.agents.push_back(agent3);
std::cout << "\n end restart " << currentCycle << "\n";
// restart!
return true;
}
void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
setCameraHomePos ( Pos ( 0, 0, 7), Pos ( -0.0828247 , -89.9146 , 0) );
// initialization
// - set noise to 0.1
// - register file chess.ppm as a texture called chessTexture (used for the wheels)
global.odeConfig.noise=0.1;
// global.odeConfig.setParam("gravity", 0);
// int chessTexture = dsRegisterTexture("chess.ppm");
// use Playground as boundary:
// - create pointer to playground (odeHandle contains things like world and space the
// playground should be created in; odeHandle is generated in simulation.cpp)
// - setting geometry for each wall of playground:
// setGeometry(double length, double width, double height)
// - setting initial position of the playground: setPosition(double x, double y, double z)
// - push playground in the global list of obstacles(globla list comes from simulation.cpp)
OctaPlayground* playground = new OctaPlayground(odeHandle, osgHandle, osg::Vec3(11, 0.2, 1), 12);
playground->setPosition(osg::Vec3(0,0,0)); // playground positionieren und generieren
global.obstacles.push_back(playground);
//****************
ComponentConf cConf = Component::getDefaultConf ();
cConf.max_force = 2;
cConf.speed = 10;
//adding the controller for the component-connections
InvertMotorNStepConf cc = InvertMotorNStep::getDefaultConf();
cc.cInit=1.2;
AbstractController* controller;
DerivativeWiringConf c = DerivativeWiring::getDefaultConf ();
DerivativeWiring* wiring;
OdeAgent* agent;
body = new SimpleComponent ( odeHandle , osgHandle , cConf );
wheel[0] = new SimpleComponent ( odeHandle , osgHandle , cConf );
wheel[1] = new SimpleComponent ( odeHandle , osgHandle , cConf );
wheel[2] = new SimpleComponent ( odeHandle , osgHandle , cConf );
wheel[3] = new SimpleComponent ( odeHandle , osgHandle , cConf );
body->setSimplePrimitive ( new Capsule ( 0.1 , 1 ) );
body->getMainPrimitive ()->init ( odeHandle , 0.5 , osgHandle.changeColor ( Color ( 2, 156/255.0, 0, 1.0f ) ) );
body->getMainPrimitive()->getOSGPrimitive()->setTexture("Images/wood.rgb");
for ( int n = 0; n < 4; n++ )
{
wheel[n]->setSimplePrimitive ( (Primitive*) new Sphere ( 0.3 ) );
wheel[n]->getMainPrimitive ()->init ( odeHandle , 0.2 , osgHandle.changeColor ( Color ( 0.8,0.8,0.8 )) );
wheel[n]->getMainPrimitive()->getOSGPrimitive()->setTexture("Images/wood.rgb");
}
body->place ( osg::Matrix::rotate ( M_PI/2 , osg::Vec3 (1 , 0 , 0 ))*osg::Matrix::translate ( 0 , 0 , 0.5 ) );
wheel[0]->place ( Pos ( -0.45 , -0.5 , 0.5 ) );
wheel[1]->place ( Pos ( 0.45 , -0.5 , 0.5 ) );
wheel[2]->place ( Pos ( -0.45 , 0.5 , 0.5 ) );
wheel[3]->place ( Pos ( 0.45 , 0.5 , 0.5 ) );
for ( int n = 0; n < 4; n++ )
{
Axis axis = Axis ( ( wheel[n<2?0:2]->getPosition () - wheel[n<2?1:3]->getPosition ()).toArray() );
HingeJoint* j1 = new HingeJoint ( body->getMainPrimitive () , wheel[n]->getMainPrimitive () , osg::Vec3 (0,0.5*( n<2?-1:1),0.5) , axis );
j1->init ( odeHandle , osgHandle , true , 1 );
body->addSubcomponent ( wheel[n] , j1 , false );
}
//controller
controller = new InvertMotorNStep ( cc );
controller->setParam ("adaptrate", 0.0);
controller->setParam ("epsC", 0.1);
controller->setParam ("epsA", 0.01);
controller->setParam ("rootE", 3);
controller->setParam ("steps", 2);
controller->setParam ("s4avg", 2);
controller->setParam ("factorB",0);
// controller = new SineController ( 18 );
//wiring
wiring = new DerivativeWiring ( c , new ColorUniformNoise () );
//agent
agent = new OdeAgent ( plotoptions );
agent->init ( controller , body , wiring );
global.agents.push_back ( agent );
global.configs.push_back ( controller );
}
// starting function (executed once at the beginning of the simulation loop)
void NormalSim::start(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
GlobalData& global) {
int num_barrels=1;
setCameraHomePos(Pos(-1.29913, 6.06201, 1.36947), Pos(-165.217, -9.32904, 0));
setCameraMode(Follow);
// initialization
global.odeConfig.setParam("noise",0.01);
// global.odeConfig.setParam("gravity",-10);
global.odeConfig.setParam("controlinterval",1);
global.odeConfig.setParam("cameraspeed",200);
// add a new parameter to be configured on the console
global.odeConfig.addParameterDef("friction", &friction, 0.05, "rolling friction coefficient");
/* * * * BARRELS * * * */
for(int i=0; i< num_barrels; i++){
//****************
Sphererobot3MassesConf conf = Barrel2Masses::getDefaultConf();
conf.pendularrange = 0.3;//0.15;
conf.motorpowerfactor = 200;//150;
conf.motorsensor=false;
conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::ZProjection, Sensor::X | Sensor::Y));
conf.spheremass = 1;
robot = new Barrel2Masses ( odeHandle, osgHandle.changeColor(Color(0.0,0.0,1.0)),
conf, "Barrel", 0.4);
robot->place (osg::Matrix::rotate(M_PI/2, 1,0,0) /* rotate to laying postion */
* osg::Matrix::rotate(M_PI/4, 0,1,0) /* both axis at 45Deg*/
* osg::Matrix::translate(0,0,.15+2*i));
if(mode==NoLearn){
controller = new ClosedLoopController();
} else {
controller = new CtrlOutline();
}
AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise());
// OdeAgent* agent = new OdeAgent ( PlotOption(File, Robot, 1) );
OdeAgent* agent = new OdeAgent ( global );
agent->init ( controller , robot , wiring );
// controller is now initialized and we can modify the matrices
Matrix C(2,2);
switch(mode){
case NoLearn: break;
case LearnContHS:
controller->setParam("TLE",0);
controller->setParam("epsA",0.1);
controller->setParam("epsC",0);
controller->setParam("creativity",0);
C.val(0,0)= .7;
C.val(0,1)= .7;
C.val(1,0)= -.4;
C.val(1,1)= .4;
if(dynamic_cast<CtrlOutline*>(controller)){
dynamic_cast<CtrlOutline*>(controller)->setC(C);
}
break;
case LearnHS:
controller->setParam("TLE",0);
controller->setParam("epsA",0.1);
controller->setParam("epsC",0.1);
controller->setParam("creativity",0);
global.odeConfig.setParam("friction", 0.1);
break;
case LearnHK:
setParam("friction", 0.1);
controller->setParam("TLE",1);
break;
// // controller is now initialized and we can modify the matrices
// Matrix C(2,2);
// C.val(0,0)= -5;
// C.val(0,1)= -5;
// C.val(1,0)= 5;
// C.val(1,1)= -5;
// if(dynamic_cast<CtrlOutline*>(controller)){
// dynamic_cast<CtrlOutline*>(controller)->setC(C);
// }
}
// agent->setTrackOptions(TrackRobot(true, false, false, "", 50));
global.agents.push_back ( agent );
global.configs.push_back ( controller );
}
}
// 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);
// }
}
// starting function (executed once at the beginning of the simulation loop)
void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
setCameraHomePos(Pos(-5.44372, 7.37141, 3.31768), Pos(-142.211, -21.1623, 0));
// 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("gravity", 0);
// use Playground as boundary:
// 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);
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, "sliderWheelie_" + std::itos(teacher*10000));
vehicle->place(Pos(0,0,0.1));
global.configs.push_back(vehicle);
// create pointer to controller
// push controller in global list of configurables
InvertMotorNStepConf cc = InvertMotorNStep::getDefaultConf();
cc.cInit=1.0;
cc.useS=false;
cc.someInternalParams=true;
controller = new InvertMotorNStep(cc);
// AbstractController* controller = new SineController(~0, SineController::Sine); // local variable!
// // // motorpower 20
// controller->setParam("period", 300);
// controller->setParam("phaseshift", 0.3);
controller->setParam("adaptrate", 0.000);
if(useSym) {
controller->setParam("epsC", 0.1);
controller->setParam("epsA", 0.1);
} else {
controller->setParam("epsC", 0.1);
controller->setParam("epsA", 0.1);
}
controller->setParam("rootE", 3);
controller->setParam("steps", 1);
controller->setParam("s4avg", 1);
controller->setParam("continuity", 0.005);
controller->setParam("teacher", teacher);
// One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1));
AbstractWiring* wiring = new FeedbackWiring(new ColorUniformNoise(0.1),
FeedbackWiring::Motor, 0.75);
//plotoptions.push_back(PlotOption(GuiLogger,Robot,5));
OdeAgent* agent = new OdeAgent(global);
agent->init(controller, vehicle, wiring);
if(track) agent->setTrackOptions(TrackRobot(true,false,false, false,
change < 50 ? std::itos(change).c_str() : "uni", 50));
global.agents.push_back(agent);
global.configs.push_back(controller);
}
// starting function (executed once at the beginning of the simulation loop)
void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
setCameraHomePos(Pos(-0.0114359, 6.66848, 0.922832), Pos(178.866, -7.43884, 0));
// 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("cameraspeed", 250);
// use Playground as boundary:
// 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);
controller=0;
addParameter("k",&k);
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),0.0);
b->setPosition(osg::Vec3(10+i*7,0,0));
global.obstacles.push_back(b);
}
double h = 0.;
RandGen rgen;
rgen.init(2);
for(int i=0; i<stairs; i++){
do{
h+=(rgen.rand()-.5)*0.6; // values between (-.25.25)
}while(h<0);
PassiveBox* b = new PassiveBox(odeHandle, osgHandle.changeColor(Color(0.3,0.3,0.3)),
osg::Vec3(1,10,h),0.0);
b->setPosition(osg::Vec3(5+i,0,0));
global.obstacles.push_back(b);
}
/******* 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.showCenter = false;
vehicle = new SliderWheelie(odeHandle, osgHandle.changeColor(Color(1,222/255.0,0)),
mySliderWheelieConf, "sliderWheelie_" + std::itos(teacher*10000));
vehicle->place(Pos(0,0,.1));
global.configs.push_back(vehicle);
// 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);
SeMoXConf cc = SeMoX::getDefaultConf();
//cc.cInit=.95;
cc.cInit=.5;
cc.modelExt=false;
// cc.someInternalParams=true;
cc.someInternalParams=false;
SeMoX* semox = new SeMoX(cc);
// AbstractController* controller = new SineController(~0, SineController::Sine); // local variable!
// // // motorpower 20
// controller->setParam("period", 300);
// controller->setParam("phaseshift", 0.3);
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);
// AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1));
//AbstractWiring* wiring = new MotorAsSensors(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->init(controller, vehicle, wiring);
if(track) agent->setTrackOptions(TrackRobot(true,false,false, false,
change != 0 ? std::itos(change).c_str() : "uni", 50));
global.agents.push_back(agent);
global.configs.push_back(controller);
this->getHUDSM()->setColor(Color(1.0,1.0,0));
this->getHUDSM()->setFontsize(18);
this->getHUDSM()->addMeasure(teacher,"gamma_s",ID,1);
this->getHUDSM()->addMeasure(k_double,"k",ID,1);
setCMC(k);
blink=0;
}
void addRobot(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global, int i){
Color col;
Sphererobot3MassesConf conf = Sphererobot3Masses::getDefaultConf();
conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::ZProjection));
conf.diameter=1.0;
conf.pendularrange= 0.30; // 0.15;
conf.motorpowerfactor = 150;
conf.spheremass = 1;
conf.motorsensor=false;
conf.addSensor(new SpeedSensor(5, SpeedSensor::RotationalRel));
//SphererobotArms* sphere = new SphererobotArms ( odeHandle, conf);
switch(i){
case 0:
col.r()=0;
col.g()=1;
col.b()=0.1;
sphere = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(col), conf, "sphere 1", 0.4);
sphere->place ( osg::Matrix::translate(9.5 , 0 , height+1 ));
break;
case 1:
col.r()=1;
col.g()=0.2;
col.b()=0;
sphere = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(col), conf, "sphere 2", 0.4);
sphere->place ( osg::Matrix::translate( 2 , -2 , height+1 ));
break;
case 3:
col.r()=0;
col.g()=0;
col.b()=1;
sphere = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(col), conf, "sphere" +
string(envnames[env]), 0.4);
sphere->place ( osg::Matrix::translate( 0 , 0 , .5 ));
break;
default:
case 2:
col.r()=0;
col.g()=1;
col.b()=0.4;
sphere = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(col), conf, "sphere 3", 0.4);
sphere->place ( osg::Matrix::translate( double(rand())/RAND_MAX*10 , 0 , height+1 ));
break;
}
Sox* sox = new Sox(.8,true);
sox->setParam("epsC", 0.2);
sox->setParam("epsA", 0.2);
if(env==ElipticBasin){
sox->setParam("epsC", 0.3);
sox->setParam("epsA", 0.3);
}
sox->setParam("Logarithmic", 0);
sox->setParam("sense", 0.5);
AbstractController *controller = new GroupController(sox, 3);
// AbstractWiring* wiring = new One2OneWiring ( new ColorUniformNoise() );
AbstractWiring* wiring = new SelectiveOne2OneWiring(new WhiteUniformNoise(),
new select_from_to(0,2),
AbstractWiring::Robot);
OdeAgent* agent;
if(i==0 || i==3 ){
agent = new OdeAgent (global);
}
else
agent = new OdeAgent (global, PlotOption(NoPlot));
agent->init ( controller , sphere , wiring );
if(track)
agent->setTrackOptions(TrackRobot(true,true,true,false,"",2));
global.agents.push_back ( agent );
//global.configs.push_back ( controller );
global.configs.push_back ( sphere);
}
// 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(-1.64766, 4.48823, 1.71381), Pos(-158.908, -10.5863, 0));
// initialization
// - set noise to 0.0
// - register file chess.ppm as a texture called chessTexture (used for the wheels)
global.odeConfig.setParam("controlinterval",1);
global.odeConfig.setParam("noise",0.01);
global.odeConfig.setParam("realtimefactor",1);
global.odeConfig.setParam("gravity", -6);
// global.odeConfig.setParam("cameraspeed", 250);
// int chessTexture = dsRegisterTexture("chess.ppm");
bool useDerivativeWiring=false;
// environemnt (type is set in constructor)
env.numSpheres = 0;
env.numBoxes = 0;
env.numCapsules = 0;
env.numSeeSaws = 0;
env.roughness = 1.0;
// use Playground as boundary:
if(barriers){
env.widthground = 4;
env.distance = 6;
env.numgrounds = 5;
env.height = 0.1;
env.heightincrease =0.1;
}else {
env.widthground = 32;
env.height = 1.0;
}
env.create(odeHandle, osgHandle, global);
global.configs.push_back(&env);
// Boxpile* boxpile = new Boxpile(odeHandle, osgHandle, Pos(10,10,0.2),
// 100,0, Pos(0.3,0.3,0.05), Pos(0.3,0.3,0.05)
// );
// boxpile->setColor("wall");
// boxpile->setPose(ROTM(M_PI/5.0,0,0,1)*TRANSM(0, 0 ,0.2));
// global.obstacles.push_back(boxpile);
for (int i=0; i< 1/*2*/; i++){ //Several dogs
// VierBeinerConf conf = VierBeiner::getDefaultConf();
VierBeinerConf conf = VierBeiner::getDefaultConfVelServos();
conf.dampingFactor = .0;
conf.powerFactor = 1.3;
if(air) conf.powerFactor = 0.3;
if (hippo) conf.powerFactor = 1.5;
conf.hipJointLimit = M_PI/3.0;
if ( barriers) conf.hipJointLimit = M_PI/2.5;
conf.kneeJointLimit = M_PI/3;
conf.legNumber = 4; /* for the dog's sake use only even numbers */
conf.useBigBox = false;
if(hippo) conf.useBigBox = false;
conf.drawstupidface=true;
conf.hippo = hippo;
// conf.onlyMainParameters = false; // all parameters
OdeHandle doghandle = odeHandle;
doghandle.substance.toRubber(10);
VierBeiner* dog = new VierBeiner(doghandle, osgHandle,conf, "Dog");
std::list<Sensor*> sensors;
sensors.push_back(new SpeedSensor(1,SpeedSensor::TranslationalRel));
AddSensors2RobotAdapter* robot =
new AddSensors2RobotAdapter(odeHandle,osgHandle,dog, sensors);
//dog->place(osg::Matrix::translate(0,0,0.15));
robot->place(osg::Matrix::translate(0,0,.5 + 4*i));
if(air || barriers){
Primitive* trunk = dog->getMainPrimitive();
fixator = new FixedJoint(trunk, global.environment);
fixator->init(odeHandle, osgHandle);
}
// create pointer to one2onewiring
//AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1));
double booster = 0.05;
if(air) booster=0;
AbstractWiring* wiring;
if(!useDerivativeWiring){
wiring = new ForceBoostWiring(new ColorUniformNoise(0.1), booster);
}else{
////////////////
AbstractWiring* fbw = new ForceBoostWiring(new NoNoise(), booster);
DerivativeWiringConf dc = DerivativeWiring::getDefaultConf();
dc.useId=true;
dc.useFirstD=true;
// dc.derivativeScale = 1.0;
AbstractWiring* drw = new DerivativeWiring(dc, new ColorUniformNoise(0.1));
if(!useDerivativeWiring){ wiring = new WiringSequence(fbw, drw); // TODO booster bei derivative wiring ???
}
}
AbstractController *controller = new Sox(.7, false);
//AbstractController *controller = new SineController();
controller->setParam("Logarithmic", 1);
controller->setParam("epsC",0.05);
if ( hippo) controller->setParam("epsC",0.1);
controller->setParam("epsA",0.01);
controller->setParam("damping",0.0001);
controller->setParam("period",300);
controller->setParam("phaseshift",0);
AbstractController* cont= new GroupController(controller,3); // 3 context sensor
OdeAgent* agent = new OdeAgent(global);
agent->init(cont, robot, wiring);
if(!hippo){
// add an operator to keep robot from falling over
agent->addOperator(new LimitOrientationOperator(Axis(0,0,1), Axis(0,0,1),
M_PI*0.35, 10));
}
//agent->setTrackOptions(TrackRobot(true,true,false,true,"bodyheight",20)); // position and speed tracking every 20 steps
global.agents.push_back(agent);
global.configs.push_back(agent);
}// Several dogs end
}
// starting function (executed once at the beginning of the simulation loop)
void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
D=0;
setCameraHomePos(Pos(-1.55424, 10.0881, 1.58559), Pos(-170.16, -7.29053, 0));
// 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("gravity", 0);
for(int i=0; i< 2; 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(30+i*7,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, "sliderWheelie_" + std::itos(teacher*10000));
vehicle->place(Pos(0,0,2));
global.configs.push_back(vehicle);
// 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);
SeMoXConf cc = SeMoX::getDefaultConf();
cc.cInit=1.2;
cc.modelExt=false;
cc.someInternalParams=true;
SeMoX* semox = new SeMoX(cc);
// AbstractController* controller = new SineController(~0, SineController::Sine); // local variable!
// // // motorpower 20
// controller->setParam("period", 300);
// controller->setParam("phaseshift", 0.3);
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,
change < 50 ? std::itos(change).c_str() : "uni", 50));
global.agents.push_back(agent);
global.configs.push_back(controller);
this->getHUDSM()->setColor(Color(1.0,1.0,0));
this->getHUDSM()->setFontsize(18);
this->getHUDSM()->addMeasure(teacher,"Gamma_s",ID,1);
this->getHUDSM()->addMeasure(D,"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);
// }
}
// starting function (executed once at the beginning of the simulation loop/first cycle)
void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
// first: position(x,y,z) second: view(alpha,beta,gamma)
// gamma=0;
// alpha == horizontal angle
// beta == vertical angle
setCameraHomePos(Pos(5.2728, 7.2112, 3.31768), Pos(140.539, -13.1456, 0));
// initialization
// - set noise to 0.1
global.odeConfig.noise=0.05;
// set realtimefactor to maximum
global.odeConfig.setParam("realtimefactor", 0);
// use Playground as boundary:
// - create pointer to playground (odeHandle contains things like world and space the
// playground should be created in; odeHandle is generated in simulation.cpp)
// - setting geometry for each wall of playground:
// setGeometry(double length, double width, double height)
// - setting initial position of the playground: setPosition(double x, double y, double z)
// - push playground in the global list of obstacles(globla list comes from simulation.cpp)
// odeHandle and osgHandle are global references
// vec3 == length, width, height
Playground* playground = new Playground(odeHandle, osgHandle, osg::Vec3(32, 0.2, 0.5));
playground->setPosition(osg::Vec3(0,0,0.05)); // playground positionieren und generieren
// register playground in obstacles list
global.obstacles.push_back(playground);
// add passive spheres as obstacles
// - create pointer to sphere (with odehandle, osghandle and
// optional parameters radius and mass,where the latter is not used here) )
// - set Pose(Position) of sphere
// - set a texture for the sphere
// - add sphere to list of obstacles
for (int i=0; i < 0/*2*/; i++){
PassiveSphere* s1 = new PassiveSphere(odeHandle, osgHandle, 0.5);
s1->setPosition(osg::Vec3(-4.5+i*4.5,0,0));
s1->setTexture("Images/dusty.rgb");
global.obstacles.push_back(s1);
}
// use Nimm2 vehicle as robot:
// - get default configuration for nimm2
// - activate bumpers, cigar mode and infrared front sensors of the nimm2 robot
// - create pointer to nimm2 (with odeHandle, osg Handle and configuration)
// - place robot
Nimm2Conf c = Nimm2::getDefaultConf();
c.force = 4;
c.bumper = true;
c.cigarMode = true;
// c.irFront = true;
vehicle = new Nimm2(odeHandle, osgHandle, c, "Nimm2");
vehicle->place(Pos(0,0,0));
// use Nimm4 vehicle as robot:
// - create pointer to nimm4 (with odeHandle and osg Handle and possible other settings, see nimm4.h)
// - place robot
//OdeRobot* vehicle = new Nimm4(odeHandle, osgHandle, "Nimm4");
//vehicle->place(Pos(0,1,0));
// create pointer to controller
// push controller in global list of configurables
// AbstractController *controller = new InvertNChannelController(10);
AbstractController *controller = new InvertMotorSpace(10);
global.configs.push_back(controller);
// create pointer to one2onewiring
One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1));
// create pointer to agent
// initialize pointer with controller, robot and wiring
// push agent in globel list of agents
agent = new OdeAgent(global);
agent->init(controller, vehicle, wiring);
global.agents.push_back(agent);
}
// starting function (executed once at the beginning of the simulation loop)
void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
// first: position(x,y,z) second: view(alpha,beta,gamma)
// gamma=0;
// alpha == horizontal angle
// beta == vertical angle
setCameraHomePos(Pos(-0.18, 20.36, 13.63), Pos(-179.93, -34.37, 0));
// initialization
// - set noise to 0.1
global.odeConfig.noise=0.1;
global.odeConfig.setParam("controlinterval", 10);
// use Playground as boundary:
// - create pointer to playground (odeHandle contains things like world and space the
// playground should be created in; odeHandle is generated in simulation.cpp)
// - setting geometry for each wall of playground:
// setGeometry(double length, double width, double height)
// - setting initial position of the playground: setPosition(double x, double y, double z)
// - push playground in the global list of obstacles(globla list comes from simulation.cpp)
bool labyrint=false;
bool squarecorridor=false;
bool normalplayground=true;
bool boxes = true;
if(normalplayground){
// Playground* playground = new Playground(odeHandle, osgHandle, osg::Vec3(12, 0.2, 0.5));
Playground* playground = new Playground(odeHandle, osgHandle, osg::Vec3(17, 0.2, 1.0));
playground->setPosition(osg::Vec3(0,0,0.05)); // playground positionieren und generieren
// register playground in obstacles list
global.obstacles.push_back(playground);
}
if(squarecorridor){
Playground* playground = new Playground(odeHandle, osgHandle,osg::Vec3(15, 0.2, 1.2 ), 1);
playground->setGroundColor(Color(255/255.0,200/255.0,0/255.0));
playground->setGroundTexture("Images/really_white.rgb");
playground->setColor(Color(255/255.0,200/255.0,21/255.0, 0.1));
playground->setPosition(osg::Vec3(0,0,0.1));
playground->setTexture("");
global.obstacles.push_back(playground);
// // inner playground
playground = new Playground(odeHandle, osgHandle,osg::Vec3(10, 0.2, 1.2), 1, false);
playground->setColor(Color(255/255.0,200/255.0,0/255.0, 0.1));
playground->setPosition(osg::Vec3(0,0,0.1));
playground->setTexture("");
global.obstacles.push_back(playground);
}
if(labyrint){
double radius=7.5;
Playground* playground = new Playground(odeHandle, osgHandle,osg::Vec3(radius*2+1, 0.2, 5 ), 1);
playground->setGroundColor(Color(255/255.0,200/255.0,0/255.0));
playground->setGroundTexture("Images/really_white.rgb");
playground->setColor(Color(255/255.0,200/255.0,21/255.0, 0.1));
playground->setPosition(osg::Vec3(0,0,0.1));
playground->setTexture("");
global.obstacles.push_back(playground);
int obstanz=30;
OsgHandle rotOsgHandle = osgHandle.changeColor(Color(255/255.0, 47/255.0,0/255.0));
OsgHandle gruenOsgHandle = osgHandle.changeColor(Color(0,1,0));
for(int i=0; i<obstanz; i++){
PassiveBox* s = new PassiveBox(odeHandle, (i%2)==0 ? rotOsgHandle : gruenOsgHandle,
osg::Vec3(random_minusone_to_one(0)+1.2,
random_minusone_to_one(0)+1.2 ,1),5);
s->setPose(osg::Matrix::translate(radius/(obstanz+10)*(i+10),0,i)
* osg::Matrix::rotate(2*M_PI/obstanz*i,0,0,1));
global.obstacles.push_back(s);
}
}
if(boxes) {
for (int i=0; i<= 2; i+=2){
PassiveBox* s1 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(1,1,1), 0.4);
s1->setTexture("Images/dusty.rgb");
s1->setPosition(osg::Vec3(-5+i*5,0,0));
global.obstacles.push_back(s1);
Joint* fixator;
Primitive* p = s1->getMainPrimitive();
fixator = new FixedJoint(p, global.environment);
fixator->init(odeHandle, osgHandle);
s1 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(1,1,1), 0.4);
s1->setTexture("Images/dusty.rgb");
s1->setPosition(osg::Vec3(0,-5+i*5,0));
global.obstacles.push_back(s1);
p = s1->getMainPrimitive();
fixator = new FixedJoint(p, global.environment);
fixator->init(odeHandle, osgHandle);
s1 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(1,1,1), 0.4);
s1->setTexture("Images/dusty.rgb");
s1->setPosition(osg::Vec3(-3.5+i*3.5,-3.5+i*3.5,0));
global.obstacles.push_back(s1);
p = s1->getMainPrimitive();
fixator = new FixedJoint(p, global.environment);
fixator->init(odeHandle, osgHandle);
s1 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(1,1,1), 0.4);
s1->setTexture("Images/dusty.rgb");
s1->setPosition(osg::Vec3(-3.5+i*3.5,3.5-i*3.5,0));
global.obstacles.push_back(s1);
p = s1->getMainPrimitive();
fixator = new FixedJoint(p, global.environment);
fixator->init(odeHandle, osgHandle);
}
}
// add passive spheres as obstacles
// - create pointer to sphere (with odehandle, osghandle and
// optional parameters radius and mass,where the latter is not used here) )
// - set Pose(Position) of sphere
// - set a texture for the sphere
// - add sphere to list of obstacles
for (int i=0; i < 0/*2*/; i++){
PassiveSphere* s1 = new PassiveSphere(odeHandle, osgHandle, 0.5);
s1->setPosition(osg::Vec3(-4.5+i*4.5,0,0));
s1->setTexture("Images/dusty.rgb");
global.obstacles.push_back(s1);
}
// use Nimm2 vehicle as robot:
// - get default configuration for nimm2
// - activate bumpers, cigar mode and infrared front sensors of the nimm2 robot
// - create pointer to nimm2 (with odeHandle, osg Handle and configuration)
// - place robot
Nimm2Conf c = Nimm2::getDefaultConf();
c.bumper = false;//true;
c.cigarMode = false;//true;
c.irFront = true;
c.irBack = true;
//c.irSide = true;
c.irRange = 1.2;//2;//3;
c.force=2;
c.speed=8;
OdeRobot* vehicle = new Nimm2(odeHandle, osgHandle, c, "Nimm2");
vehicle->place(Pos(0,0,0.5));
// vehicle->place(Pos(0,6.25,0));
// use Nimm4 vehicle as robot:
// - create pointer to nimm4 (with odeHandle and osg Handle and possible other settings, see nimm4.h)
// - place robot
//OdeRobot* vehicle = new Nimm4(odeHandle, osgHandle, "Nimm4");
//vehicle->place(Pos(0,1,0));
// create pointer to controller
// push controller in global list of configurables
AbstractController *controller = new LayeredController(10);
controller->setParam("eps",0.1);
controller->setParam("factor_a",0.1);
controller->setParam("eps_hebb",0.03);
global.configs.push_back(controller);
// create pointer to one2onewiring
One2OneWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1));
// create pointer to agent
// initialize pointer with controller, robot and wiring
// push agent in globel list of agents
OdeAgent* agent = new OdeAgent(global);
agent->init(controller, vehicle, wiring);
agent->setTrackOptions(TrackRobot(true, false, false, false, "hebbh" ,1));
global.agents.push_back(agent);
}
// starting function (executed once at the beginning of the simulation loop)
virtual bool restart(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
std::cout << "\n begin restart " << currentCycle << "\n";
std::cout<<"Current Cycle"<<this->currentCycle<<std::endl;
// remove agents
while (global.agents.size() > 0)
{
OdeAgent* agent = *global.agents.begin();
AbstractController* controller = agent->getController();
OdeRobot* robot = agent->getRobot();
AbstractWiring* wiring = agent->getWiring();
global.configs.erase(std::find(global.configs.begin(),
global.configs.end(), controller));
delete robot;
delete wiring;
global.agents.erase(global.agents.begin());
}
// clean the playgrounds
while (global.obstacles.size() > 0)
{
std::vector<AbstractObstacle*>::iterator iter =
global.obstacles.begin();
delete (*iter);
global.obstacles.erase(iter);
}
boxPrimitives.clear();
relative_sensor_obst.clear();
grippables.clear();
///////////////Recreate Robot Start//////////////////////////////////////////////////////////////////////////////////////
/**************************************************************************************************
*** Set up Environment
**************************************************************************************************/
setup_Playground(global);
/**************************************************************************************************
*** Set up 4 landmark and 1 goal spheres
**************************************************************************************************/
generate_spheres(global);
/**************************************************************************************************
*** Set up 3 pushable boxes and add the first one as graspable
************************************************************************************************/
generate_boxes(global);
grippables.push_back(boxPrimitives[0]);
// grippables.push_back(boxPrimitives[1]);
// grippables.push_back(boxPrimitives[2]);
/**************************************************************************************************
*** Set up robot and controller
**************************************************************************************************/
//1) Activate IR sensors
FourWheeledConfGripper fconf = FourWheeledRPosGripper::getDefaultConf();
///2) relative sensors
for (unsigned int i=0; i < relative_sensor_obst.size(); i++) {
fconf.rpos_sensor_references.push_back(relative_sensor_obst.at(i)->getMainPrimitive());
}
vehicle = new FourWheeledRPosGripper(odeHandle, osgHandle, fconf);
/****Initial position of Nimm4******/
Pos pos(0.0 , 0.0 , 1.0);
//setting position and orientation
vehicle->place(osg::Matrix::rotate(0, 0, 0, 1) *osg::Matrix::translate(pos));
// only set new grippables otherwise keep old controller
qcontroller->setGrippablesAndVehicle(vehicle,grippables);
global.configs.push_back(qcontroller);
// create pointer to one2onewiring
AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1));
// create pointer to agent
OdeAgent* agent = new OdeAgent(global);
agent->init(qcontroller, vehicle, wiring);///////////// Initial controller!!!
global.agents.push_back(agent);
std::cout << "\n end restart " << currentCycle << "\n";
// restart!
qcontroller->setReset(false);
return true;
}
// starting function (executed once at the beginning of the simulation loop)
void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
setCameraHomePos(Pos(5.2728, 7.2112, 3.31768), Pos(140.539, -13.1456, 0));
// initialization
// - set global noise sensor to 0.05
global.odeConfig.setParam("noise",0.05);
// global.odeConfig.setParam("gravity", 0); // no gravity
// use Playground as boundary:
// - create pointer to playground (odeHandle contains things like world and space the
// playground should be created in; odeHandle is generated in simulation.cpp)
// - setting initial position of the playground: setPosition(osg::Vec3(double x, double y, double z))
// - push playground to the global list of obstacles (global list comes from simulation.cpp)
OctaPlayground* playground = new OctaPlayground(odeHandle, osgHandle, osg::Vec3(10, 0.2, 1), 12);
playground->setPosition(osg::Vec3(0,0,0)); // place and create playground
global.obstacles.push_back(playground);
// add passive spheres as obstacles
// - create pointer to sphere (with odehandle, osghandle and
// optional parameters radius and mass,where the latter is not used here) )
// - set Pose(Position) of sphere
// - add sphere to list of obstacles
for(int i=0; i<8; i++){
PassiveSphere* s = new PassiveSphere(odeHandle, osgHandle.changeColor(Color(0.0,1.0,0.0)), 0.5);
s->setPosition(osg::Vec3(5,0,i*3));
global.obstacles.push_back(s);
}
// Spherical Robot with axis (gyro) sensors:
// - get default configuration for robot
// - create pointer to spherical robot (with odeHandle, osgHandle and configuration)
// - place robot (unfortunatelly there is a type cast necessary, which is not quite understandable)
Sphererobot3MassesConf conf = Sphererobot3Masses::getDefaultConf();
conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::ZProjection));
conf.diameter=1.0;
conf.pendularrange= 0.35;
robot = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(Color(1.0,0.0,0)),
conf, "Spherical", 0.2);
(robot)->place ( Pos( 0 , 0 , 0.1 ));
// Selforg - Controller (Note there are several: use Sos or Sox now)
// create pointer to controller
// set some parameters
// push controller in global list of configurables
controller = new InvertMotorSpace(10);
controller->setParam("epsA",0.05); // model learning rate
controller->setParam("epsC",0.2); // controller learning rate
controller->setParam("rootE",3); // model and contoller learn with square rooted error
global.configs.push_back ( controller );
// SineController (produces just sine waves)
// controller = new SineController();
// controller->setParam("sinerate", 40);
// controller->setParam("phaseshift", 0.0);
// create pointer to one2onewiring which uses colored-noise
One2OneWiring* wiring = new One2OneWiring ( new ColorUniformNoise() );
// create pointer to agent
// initialize pointer with controller, robot and wiring
// push agent in globel list of agents
OdeAgent* agent = new OdeAgent ( global );
agent->init ( controller , robot , wiring );
if(track){
// the following line will enables a position tracking of the robot, which is written into a file
// you can customize what is logged with the TrackRobotConf
TrackRobotConf tc = TrackRobot::getDefaultConf();
tc.scene = "zaxis";
tc.displayTrace = true;
agent->setTrackOptions(TrackRobot(tc));
}
global.agents.push_back ( agent );
}
// starting function (executed once at the beginning of the simulation loop)
void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
/**************************************************************************************************
*** Camera Position
**************************************************************************************************/
//setCameraHomePos(Pos(0, 40, 10), Pos(0, 0, 0)); // viewing full scene from side
setCameraHomePos(Pos(0, 5, 5), Pos(0, 0, 0));
//setCameraHomePos(Pos(0, 20, 20), Pos(0, 0, 0));
/**************************************************************************************************
*** Simulation Parameters
**************************************************************************************************/
//1) - set noise to 0.1
global.odeConfig.noise= 0.0;//0.02;//0.05;
//2) - set controlinterval -> default = 1
global.odeConfig.setParam("controlinterval", 1);/*update frequency of the simulation ~> amos = 20*/
//3) - set simulation setp size
global.odeConfig.setParam("simstepsize", 0.01); /*stepsize of the physical simulation (in seconds)*/
//Update frequency of simulation = 1*0.01 = 0.01 s ; 100 Hz
//4) - set gravity if not set then it uses -9.81 =earth gravity
//global.odeConfig.setParam("gravity", -9.81);
/**************************************************************************************************
*** Set up Environment
**************************************************************************************************/
setup_Playground(global);
Substance material(5.0,10.0,99.0,1.0);
this->setGroundSubstance(material);
/**************************************************************************************************
*** Set up 4 landmark and 1 goal spheres
**************************************************************************************************/
// not being used atm
//generate_spheres(global);
/**************************************************************************************************
*** Set up 3 pushable boxes and add the first one as graspable
************************************************************************************************/
generate_boxes(global);
grippables.push_back(boxPrimitives[0]);
// grippables.push_back(boxPrimitives[1]);
// grippables.push_back(boxPrimitives[2]);
/**************************************************************************************************
*** Set up robot and controller
**************************************************************************************************/
//1) Activate IR sensors
FourWheeledConfGripper fconf = FourWheeledRPosGripper::getDefaultConf();
///2) relative sensors
for (unsigned int i=0; i < relative_sensor_obst.size(); i++) {
fconf.rpos_sensor_references.push_back(relative_sensor_obst.at(i)->getMainPrimitive());
}
vehicle = new FourWheeledRPosGripper(odeHandle, osgHandle, fconf);
/****Initial position of Nimm4******/
Pos pos(0.0 , 0.0 , 1.0);
//setting position and orientation
vehicle->place(osg::Matrix::rotate(0, 0, 0, 1) *osg::Matrix::translate(pos));
qcontroller = new ASLController("1","1");
// qcontroller = new FSMController("1","1", vehicle, grippables);
qcontroller->setGrippablesAndVehicle(vehicle,grippables);
global.configs.push_back(qcontroller);
// create pointer to one2onewiring
AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1));
// create pointer to agent
OdeAgent* agent = new OdeAgent(global);
agent->init(qcontroller, vehicle, wiring);///////////// Initial controller!!!
global.agents.push_back(agent);
}
