Pendulum::Pendulum(SimulationEngine& engine, const opal::Matrix44r& transform)
: LearningEntity(10),
mMaxAngle(180),
mMaxVel(500),
mMaxTorque(2)
{
createAgent();
opal::Simulator* sim = engine.getSimulator();
// Create the pendulum Solid. We use radius, length, and density
// values that produce a pendulum mass of ~1.
mLength = 1;
mRadius = (opal::real)0.05;
mPendulumSolid = sim->createSolid();
mPendulumSolid->setSleepiness(0);
// Make the pendulum "point" away from the joint anchor.
opal::Matrix44r pendulumTransform = transform;
pendulumTransform.rotate(-90, 1, 0, 0);
mPendulumSolid->setTransform(pendulumTransform);
opal::CapsuleShapeData capsule;
capsule.length = mLength;
capsule.radius = mRadius;
capsule.material.density = 127;
mPendulumSolid->addShape(capsule);
engine.createPhysicalEntity("pole", "Plastic/Green", mPendulumSolid);
//// TESTING
//mPendulumSolid->setLinearDamping(0.1);
// Setup the Joint that connects the pendulum to the static
// environment.
mHinge = sim->createJoint();
opal::JointAxis axis;
axis.direction.set(0, 0, 1);
axis.limitsEnabled = false;
opal::JointData jointData;
jointData.setType(opal::HINGE_JOINT);
jointData.contactsEnabled = false;
jointData.solid0 = mPendulumSolid;
jointData.solid1 = NULL;
opal::Point3r anchor = transform.getPosition();
anchor[1] += (opal::real)0.5 * mLength;
jointData.anchor = anchor;
jointData.axis[0] = axis;
mHinge->init(jointData);
// Save the initial Solid configurations for resetting.
mInitSolidData = mPendulumSolid->getData();
}
void PendulumTest::startTest()
{
SimulationEngine engine;
if (!engine.init())
{
return;
}
// Get a pointer to the trial number overlay and make it visible.
Ogre::OverlayManager::getSingleton().getByName("Verve/TrialNumber")->show();
engine.setUpdateMode(SimulationEngine::SIMULATE_REAL_TIME_MULTIPLE, 1);
engine.setCameraMoveSpeed(5);
//// Set to capture frames at 29.97 fps.
//engine.setUpdateMode(SIMULATE_CONSTANT_CHUNK, 0.0333667);
engine.getSimulator()->setStepSize(mPhysicsStepSize);
engine.getSimulator()->setGravity(opal::Vec3r(0, -9.81, 0));
// Make sure we get notified at the end of each step.
engine.getSimulator()->addPostStepEventHandler(this);
// Create a static box for a ground plane.
opal::Solid* boxSolid = engine.getSimulator()->createSolid();
boxSolid->setStatic(true);
boxSolid->setPosition(0, -1.5, 0);
opal::BoxShapeData data;
data.dimensions.set(6, 3, 6);
boxSolid->addShape(data);
engine.createPhysicalEntity("ground", "Plastic/Gray", boxSolid);
DataFile dataFile(mNumTrialsPerRun);
opal::Matrix44r initialTransform;
initialTransform.translate(0, 1.2, 0);
assert(NULL == mPendulum);
mPendulum = new Pendulum(engine, initialTransform);
mAgentDebugger = new AgentVisualDebugger(engine.getSceneManager());
for (unsigned int run = 0; run < mNumRuns; ++run)
{
mPendulum->resetBodyAndCreateNewAgent();
mAgentDebugger->setAgent(mPendulum->getAgent());
for (unsigned int trial = 0; trial < mNumTrialsPerRun; ++trial)
{
updateOverlayData(trial);
mPendulum->resetBodyAndSTM();
mPendulum->randomizeState();
mAvgRewardPerStep = 0;
mCurrentTrialTime = 0;
while (mCurrentTrialTime < mTrialLength)
{
Ogre::Real elapsedSimTime = 0;
Ogre::Real elapsedRealTime = 0;
engine.update(elapsedSimTime, elapsedRealTime);
handleInput(elapsedRealTime, engine);
if (engine.quitApp())
{
return;
}
mAgentDebugger->updateVisuals();
}
mAvgRewardPerStep = mAvgRewardPerStep * mPhysicsStepSize /
mCurrentTrialTime;
dataFile.storeData("trial", trial, (float)trial);
dataFile.storeData("avg reward per step", trial,
mAvgRewardPerStep);
printTrialAndRunStatus(run, trial, mAvgRewardPerStep);
//// Print value function data.
//if (0 == run &&
// (trial == 0 || trial == 4 || trial == 19 || trial == 99))
//{
// char fileStr[1024];
// sprintf(fileStr, "./results/pendulum-trial%d-value.dat", trial);
// mPendulum->getAgent()->saveValueData(400, fileStr);
// sprintf(fileStr, "./results/pendulum-trial%d-RBF.dat", trial);
// mPendulum->getAgent()->saveStateRBFData(fileStr);
//}
}
std::cout << "Agent age = " << mPendulum->getAgent()->getAgeString()
<< std::endl;
}
dataFile.printToFile("./results/pendulum-performance.dat");
}
void setupEnvironment()
{
// Main static arena.
opal::Blueprint arenaBlueprint;
opal::loadFile(arenaBlueprint, "../data/blueprints/arena1.xml");
opal::BlueprintInstance arenaBPInstance;
gEngine.getSimulator()->instantiateBlueprint(arenaBPInstance,
arenaBlueprint, opal::Matrix44r(), 1);
gEngine.createPhysicalEntity("staticEnvironment", "Plastic/Gray",
arenaBPInstance.getSolid("staticEnvironment"));
gEngine.createPhysicalEntity("toy1", "Plastic/Green",
arenaBPInstance.getSolid("toy1"));
gEngine.createPhysicalEntity("toy2", "Plastic/Green",
arenaBPInstance.getSolid("toy2"));
gEngine.createPhysicalEntity("toy3", "Plastic/Green",
arenaBPInstance.getSolid("toy3"));
// Seesaw.
opal::Blueprint seesawBP;
opal::loadFile(seesawBP, "../data/blueprints/seesaw.xml");
opal::BlueprintInstance seesawBPInstance;
opal::Matrix44r seesawTransform;
seesawTransform.translate(8, 0, 0);
gEngine.getSimulator()->instantiateBlueprint(seesawBPInstance,
seesawBP, seesawTransform, 1);
gEngine.createPhysicalEntity("seesawSupport", "Plastic/Black",
seesawBPInstance.getSolid("seesawSupport"));
gEngine.createPhysicalEntity("seesawPanel", "Plastic/Orange",
seesawBPInstance.getSolid("seesawPanel"));
// Add an initial torque to bring one end of the seesaw to the
// ground.
seesawBPInstance.getJoint("seesawHinge")->addTorque(0, 100, 0, true);
// Merry-go-round.
gEngine.createPhysicalEntity("merryGoRound", "Plastic/Yellow",
arenaBPInstance.getSolid("merryGoRound"));
// Curtains.
opal::Blueprint curtainsBP;
opal::loadFile(curtainsBP, "../data/blueprints/blockCurtain.xml");
opal::BlueprintInstance curtainsBPInstance;
opal::Matrix44r curtainsTransform;
curtainsTransform.rotate(45, 0, 1, 0);
curtainsTransform.translate(-10, 0, 0);
gEngine.getSimulator()->instantiateBlueprint(curtainsBPInstance,
curtainsBP, curtainsTransform, 1);
gEngine.createPhysicalEntity("curtainBase", "Plastic/Red",
curtainsBPInstance.getSolid("curtainBase"));
gEngine.createPhysicalEntity("curtainPiece0", "Plastic/Black",
curtainsBPInstance.getSolid("curtainPiece0"));
gEngine.createPhysicalEntity("curtainPiece1", "Plastic/Black",
curtainsBPInstance.getSolid("curtainPiece1"));
gEngine.createPhysicalEntity("curtainPiece2", "Plastic/Black",
curtainsBPInstance.getSolid("curtainPiece2"));
gEngine.createPhysicalEntity("curtainPiece3", "Plastic/Black",
curtainsBPInstance.getSolid("curtainPiece3"));
gEngine.createPhysicalEntity("curtainPiece4", "Plastic/Black",
curtainsBPInstance.getSolid("curtainPiece4"));
gEngine.createPhysicalEntity("curtainPiece5", "Plastic/Black",
curtainsBPInstance.getSolid("curtainPiece5"));
//// Ragdoll.
//opal::Blueprint ragdollBP;
//opal::loadFile(ragdollBP, "../data/blueprints/ragdoll.xml");
//opal::BlueprintInstance ragdollBPInstance;
//opal::Matrix44r ragdollTransform;
//ragdollTransform.translate(10, 5, 0);
//gEngine.getSimulator()->instantiateBlueprint(ragdollBPInstance,
// ragdollBP, ragdollTransform, 2);
//for (unsigned int i = 0; i < ragdollBPInstance.getNumSolids(); ++i)
//{
// opal::Solid* s = ragdollBPInstance.getSolid(i);
// gEngine.createPhysicalEntity(s->getName(), "Plastic/Red", s);
//}
//// TESTING: Simple goal box.
//opal::Solid* boxSolid = gEngine.getSimulator()->createSolid();
//boxSolid->setStatic(false);
//boxSolid->setSleepiness(0);
//boxSolid->setPosition(15.5, 10, -7);
//opal::BoxShapeData data;
//data.dimensions.set(1.5, 1.5, 1.5);
//data.material.friction = 0.1;
//data.material.density = 0.5;
//boxSolid->addShape(data);
//gEngine.createPhysicalEntity("goal box", "Plastic/Green", boxSolid);
//// TESTING: Make a volume sensor to detect the goal.
//opal::VolumeSensorData goalSensorData;
//goalSensorData.solid = gRobot->getChassis();
//goalSensorData.transform.makeTranslation(0, 0, -2);
//gGoalSensor = gEngine.getSimulator()->createVolumeSensor();
//gGoalSensor->init(goalSensorData);
//gGoalSensorVolume = gEngine.getSimulator()->createSolid();
//gGoalSensorVolume->setStatic(true);
////opal::Matrix44r m = gRobot->getChassis()->getTransform();
////m.translate(0, 0, -2);
//opal::Matrix44r m;
//m.translate(0, 100, 0);
//gGoalSensorVolume->setTransform(m);
//opal::BoxShapeData sensorVolumeShape;
//sensorVolumeShape.dimensions.set(2, 1, 2);
////gEngine.getSimulator()->setupContactGroup(5, false);
////sensorVolumeShape.contactGroup = 5;
//sensorVolumeShape.material.density = 0.01;
//gGoalSensorVolume->addShape(sensorVolumeShape);
////gEngine.createPhysicalEntityBox("goal sensor", "Translucent/Blue", boxDim,
//// gGoalSensorVolume);
//opal::JointData fixedJointData;
//fixedJointData.setType(opal::FIXED_JOINT);
//fixedJointData.solid0 = gRobot->getChassis();
//fixedJointData.solid1 = gGoalSensorVolume;
//opal::Joint* fixedJoint = gEngine.getSimulator()->createJoint();
//fixedJoint->init(fixedJointData);
}
