// starting function (executed once at the beginning of the simulation loop)
void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
{
setCameraHomePos(Pos(-0.497163, 11.6358, 3.67419), Pos(-179.213, -11.6718, 0));
// initialization
global.odeConfig.setParam("noise",0.0);
// global.odeConfig.setParam("gravity",-10);
global.odeConfig.setParam("controlinterval",2);
global.odeConfig.setParam("realtimefactor",1);
global.odeConfig.setParam("realtimefactor",0);
global.odeConfig.setParam("drawinterval",2000);
runcounter=0;
maxCounter=10000;
// maxCounter=500;
counter = maxCounter;
sphere=0;
// we want to start the sphere in different initial conditions and
// see how the sat networks will end up driving the robot
string fn=string(networkfilename) + string("scan.log");
logfile.open(fn.c_str(),ofstream::out);
logfile << "# speed scan for sat network " << networkfilename << endl;
logfile << "#C sx sy sz ex ey ez" << endl;
// generate start speeds on 3 spheres
double stepsize;
double powers[3] = {5,10,15};
// double powers[3] = {5,15,30};
startAngles.push_back(Matrix(3,1)); // add zero initial condition
//{ double theta =0;
for(int p=0; p < 3; p++){
// stepsize=M_PI/(6.0*(p+1.0)); // 663 initial condictions
stepsize=M_PI/(3.0*(p+1.0)); // 172 initial conditions
for(double theta=-M_PI/2; theta < M_PI/2; theta +=stepsize){
// { double omega=0;
for(double omega=-M_PI; omega < M_PI; omega += (stepsize/cos(theta)) ){
Matrix m(3,1);
m.val(0,0)=theta;
m.val(1,0)=omega;
m.val(2,0)=powers[p];
startAngles.push_back(m);
}
}
}
speedsensor = new SpeedSensor(5, SpeedSensor::RotationalRel);
//****************
conf = Sphererobot3Masses::getDefaultConf();
conf.pendularrange = 0.15;
conf.motorpowerfactor = 150;
conf.motorsensor=false;
// conf.spheremass = 1;
conf.spheremass = 0.3;
conf.addSensor(new AxisOrientationSensor(AxisOrientationSensor::ZProjection));
}
// 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 );
}
// 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)
{
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 );
}
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)
{
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 );
}
}
// 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 );
}
}
