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(); }
int main(int argc, char* argv[]) { srand((unsigned int)time(NULL)); SDL_Init(SDL_INIT_JOYSTICK); if(SDL_NumJoysticks() > 0) { // Setup the joystick. std::cout << "========================================" << std::endl; std::cout << "Initializing game controller: " << SDL_JoystickName(0) << std::endl; gGamePad = SDL_JoystickOpen(0); std::cout << SDL_JoystickNumAxes(gGamePad) << " axes" << std::endl; std::cout << SDL_JoystickNumBalls(gGamePad) << " trackballs" << std::endl; std::cout << SDL_JoystickNumHats(gGamePad) << " hats" << std::endl; std::cout << SDL_JoystickNumButtons(gGamePad) << " buttons" << std::endl; std::cout << "========================================" << std::endl; } else { std::cout << "========================================" << std::endl; std::cout << "No game controller detected" << std::endl; std::cout << "========================================" << std::endl; } //Ogre::Overlay* trialOverlay; ///// The current amount of elapsed time within a trial. //Ogre::Real mCurrentTrialTime; ///// The length of each trial in seconds. //Ogre::Real mTrialLength; ///// The rewards received during a single trial, in rewards per step. //verve::real mAvgRewardPerStep; if (!gEngine.init()) { return 0; } gEngine.setUpdateMode(SimulationEngine::SIMULATE_REAL_TIME_MULTIPLE, 1); //// Set to capture frames at 29.97 fps. //engine.setUpdateMode(SIMULATE_CONSTANT_CHUNK, 0.0333667); // Use feet for this simulation. gEngine.getSimulator()->setGravity(opal::Vec3r(0, -30, 0)); gEngine.getSimulator()->setStepSize(gPhysicsStepSize); // Make sure we get notified at the end of each step. gEngine.getSimulator()->addPostStepEventHandler(&gPostStepEventHandler); // Create the robot. opal::Matrix44r robotTransform; robotTransform.translate(0, 1, 0); gRobot = new Robot(gEngine, 5); gRobot->init("../data/blueprints/robot1.xml", "Plastic/LightBlue", 0.5, robotTransform, 2); gRobot->resetBodyAndCreateNewAgent(); gRobot->resetBodyAndSTM(); gRobot->randomizeState(); gRobot->getFLMotor()->setMaxTorque((opal::real)2); gRobot->getFRMotor()->setMaxTorque((opal::real)2); gRobot->getFLMotor()->setMaxVelocity(1000); gRobot->getFRMotor()->setMaxVelocity(1000); // Create the car. opal::Matrix44r carTransform; carTransform.translate(-12, 2, 4); gCar = new Car(gEngine); gCar->init("../data/blueprints/car1.xml", "Plastic/Blue", 1, carTransform, 1); //DataFile dataFile(mNumTrialsPerRun); //updateOverlayData(trial); //mAvgRewardPerStep = 0; //mCurrentTrialTime = 0; gAgentDebugger = new AgentVisualDebugger(gEngine.getSceneManager()); gAgentDebugger->setAgent(gRobot->getAgent()); gAgentDebugger->setDisplayEnabled(false); Ogre::OverlayManager::getSingleton().getByName("Verve/Debug")->hide(); Ogre::OverlayManager::getSingleton().getByName("Core/DebugOverlay")->hide(); // Setup camera. gEngine.getCamera()->setPosition(opal::Point3r(0, 25, 25)); gEngine.getCamera()->lookAt(opal::Point3r(0, (opal::real)0.1, 0)); gEngine.setCameraMoveSpeed(15); setupEnvironment(); mainLoop(); delete gRobot; delete gCar; delete gAgentDebugger; return 0; }
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); }