int main(int argc, char* argv[]) {
// create and initialize the world
dart::simulation::World* myWorld
= dart::utils::SkelParser::readWorld(DART_DATA_PATH"/skel/chain.skel");
assert(myWorld != NULL);
// create and initialize the world
Eigen::Vector3d gravity(0.0, -9.81, 0.0);
myWorld->setGravity(gravity);
myWorld->setTimeStep(1.0/2000);
int dof = myWorld->getSkeleton(0)->getNumGenCoords();
Eigen::VectorXd initPose(dof);
for (int i = 0; i < dof; i++)
initPose[i] = dart::math::random(-0.5, 0.5);
myWorld->getSkeleton(0)->setConfigs(initPose, true, true, false);
// create a window and link it to the world
MyWindow window;
window.setWorld(myWorld);
glutInit(&argc, argv);
window.initWindow(640, 480, "Forward Simulation");
glutMainLoop();
return 0;
}
RobotStatus initRobotStatus (const int index)
{
int c = 48 + index;
RobotStatus r;
r.index = index;
r.nIterations = 0;
r.stdDev = 0;
r.isTimeElapsed = 0;
r.pose = initPose();
r.isNewPose = 1;
r.actualLocOffset = initPointF (0);
r.scanCentrePt = initPointF (MIN_FLT);
r.scanCentrePt = initPointF (MIN_FLT);
r.target = initPointF (MIN_FLT);
r.ikData.index = -1;
#ifdef IS_WIN
strcpy_s ((char*)r.locWinName, 4, "loc");
strcpy_s ((char*)r.navWinName, 4, "nav");
#elif defined (IS_GUMSTIX)
strcpy (r.locWinName, "loc");
strcpy (r.navWinName, "nav");
#else
strcpy ((char*)r.locWinName, "loc");
strcpy ((char*)r.navWinName, "nav");
#endif
r.locWinName[3] = c;
r.navWinName[3] = c;
r.locWinName[4] = 0;
r.navWinName[4] = 0;
#ifdef RERUNNING_ROBOT
r.rerunning_file = NULL;
r.rerunning_file = NULL;
r.rerunning_startPtr = NULL;
#endif // ifdef RERUNNING_ROBOT
#ifdef RECORDING_ROBOT
r.objIndex = -1;
#endif
return r;
}