void BasicGLPane::updateSim(wxTimerEvent & event) { dmTimespec tv_now; dmGetSysTime(&tv_now); real_time_ratio = (simThread->sim_time-last_render_time)/timeDiff(last_draw_tv, tv_now); last_render_time = simThread->sim_time; dmGetSysTime(&last_draw_tv); camera->setPerspective(45.0, (GLfloat)getWidth()/(GLfloat)getHeight(), 1.0, 200.0); camera->update(mouse); camera->applyView(); Refresh(); // ask for repaint // if you want the GL light to move with the camera, comment the following two lines - yiping GLfloat light_position0[] = { 1.0, 1.0, 1.0, 0.0 }; glLightfv (GL_LIGHT0, GL_POSITION, light_position0); GLfloat light_position1[] = { -1.0, -1.0, 1.0, 0.0 }; glLightfv (GL_LIGHT1, GL_POSITION, light_position1); timer_count++; double rtime = timeDiff(first_tv, tv_now); double update_time = timeDiff(update_tv, tv_now); if (update_time > 2.5) { timer_count ++; cerr << "time/real_time: " << simThread->sim_time << '/' << rtime << " frame_rate: " << (double) timer_count/rtime << endl; dmGetSysTime(&update_tv); } }
// I still don't fully understand the mechanism behind idle func ... // but the following works ... need to read more documentation void BasicGLPane::updateSim(wxIdleEvent& event) { if (!paused_flag) { for (int i=0; i<render_rate; i++) { G_integrator->simulate(idt); sim_time += idt; } } camera->setPerspective(45.0, (GLfloat)getWidth()/(GLfloat)getHeight(), 1.0, 200.0); camera->update(mouse); camera->applyView(); Refresh(); // ask for repaint // if you want the GL light to move with the camera, comment the following two lines - yiping GLfloat light_position0[] = { 1.0, 1.0, 1.0, 0.0 }; glLightfv (GL_LIGHT0, GL_POSITION, light_position0); GLfloat light_position1[] = { -1.0, -1.0, 1.0, 0.0 }; glLightfv (GL_LIGHT1, GL_POSITION, light_position1); timer_count++; dmGetSysTime(&tv); double elapsed_time = ((double) tv.tv_sec - last_tv.tv_sec) + (1.0e-9*((double) tv.tv_nsec - last_tv.tv_nsec)); if (elapsed_time > 2.5) { rtime += elapsed_time; cerr << "time/real_time: " << sim_time << '/' << rtime << " frame_rate: " << (double) timer_count/elapsed_time << endl; timer_count = 0; last_tv.tv_sec = tv.tv_sec; last_tv.tv_nsec = tv.tv_nsec; } // WakeUpIdle(); //? event.RequestMore();// render continuously, not only once on idle }
//---------------------------------------------------------------------------- // Summary: // Parameters: // Returns: //---------------------------------------------------------------------------- int main(int argc, char** argv) { int i, j; glutInit(&argc, argv); //========================= char *filename = "simulation.cfg"; if (argc > 1) { filename = argv[1]; } glutInitWindowSize(640,480); glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH); glutCreateWindow("DynaMechs Example"); myinit(); mouse = dmGLMouse::dmInitGLMouse(); for (i=0; i<4; i++) { for (j=0; j<4; j++) { view_mat[i][j] = 0.0; } view_mat[i][i] = 1.0; } //view_mat[3][2] = -10.0; camera = new dmGLPolarCamera_zup(); camera->setRadius(8.0); camera->setCOI(8.0, 10.0, 2.0); camera->setTranslationScale(0.02f); // load robot stuff ifstream cfg_ptr; cfg_ptr.open(filename); // Read simulation timing information. readConfigParameterLabel(cfg_ptr,"Integration_Stepsize"); cfg_ptr >> idt; if (idt <= 0.0) { cerr << "main error: invalid integration stepsize: " << idt << endl; exit(3); } motion_plan_rate = (int) (0.5 + 0.01/idt); // fixed rate of 100Hz readConfigParameterLabel(cfg_ptr,"Display_Update_Rate"); cfg_ptr >> render_rate; if (render_rate < 1) render_rate = 1; // =========================================================================== // Initialize DynaMechs environment - must occur before any linkage systems char env_flname[FILENAME_SIZE]; readConfigParameterLabel(cfg_ptr,"Environment_Parameter_File"); readFilename(cfg_ptr, env_flname); dmEnvironment *environment = dmuLoadFile_env(env_flname); environment->drawInit(); dmEnvironment::setEnvironment(environment); // =========================================================================== // Initialize a DynaMechs linkage system char robot_flname[FILENAME_SIZE]; readConfigParameterLabel(cfg_ptr,"Robot_Parameter_File"); readFilename(cfg_ptr, robot_flname); G_robot = dynamic_cast<dmArticulation*>(dmuLoadFile_dm(robot_flname)); G_integrator = new dmIntegRK4(); G_integrator->addSystem(G_robot); initAquaControl(G_robot); glutReshapeFunc(myReshape); glutKeyboardFunc(processKeyboard); glutSpecialFunc(processSpecialKeys); glutDisplayFunc(display); glutIdleFunc(updateSim); dmGetSysTime(&last_tv); cout << endl; cout << " p - toggles dynamic simulation" << endl; cout << " UP/DOWN ARROW - increases/decreases velocity command" << endl; cout << "RIGHT/LEFT ARROW - changes heading command" << endl << endl; glutMainLoop(); return 0; /* ANSI C requires main to return int. */ }
//---------------------------------------------------------------------------- void updateSim() { if (!paused_flag) { for (int i=0; i<render_rate; i++) { // Kenji Suzuki/Kan Yoneda's motion planning algorithm // for dynamic sim. // Can run more often than rendering and less often than // the dynamic simulation. if (motion_plan_count%motion_plan_rate == 0) { // Note motion_planning requires cm/sec velocity commands // commands are run through a simple low pass filter. motion_command[0] = cos(cmd_direction*DEGTORAD)*cmd_speed*100.0; motion_command[1] = sin(cmd_direction*DEGTORAD)*cmd_speed*100.0; motion_command[2] = 0.0; // Gait algorithm gait_algorithm(sim_time, idt*motion_plan_rate, new_robot, motion_command); // Interface between gait algorithm and DynaMechs. This // returns desiredJointPos with the next set of desired // joint positions. static CartesianVector refPos; static EulerAngles refPose; interface_Gait2DynaMechs(new_robot, refPose, refPos, desiredJointPos); motion_plan_count = 0; } computeAquaControl(desiredJointPos); G_integrator->simulate(idt); sim_time += idt; motion_plan_count++; } } camera->update(mouse); camera->applyView(); display(); // compute render rate timer_count++; dmGetSysTime(&tv); double elapsed_time = ((double) tv.tv_sec - last_tv.tv_sec) + (1.0e-9*((double) tv.tv_nsec - last_tv.tv_nsec)); if (elapsed_time > 2.5) { rtime += elapsed_time; cerr << "time/real_time: " << sim_time << '/' << rtime << " frame_rate: " << (double) timer_count/elapsed_time << endl; timer_count = 0; last_tv.tv_sec = tv.tv_sec; last_tv.tv_nsec = tv.tv_nsec; } }
//---------------------------------------------------------------------------- // Summary: // Parameters: // Returns: //---------------------------------------------------------------------------- int main(int argc, char** argv) { glutInit(&argc, argv); // char *xanname = ""; if (argc > 1) { xanname = argv[1]; printf("The .xan file you are viewing is: %s \r\n", xanname); } else { printf("\r\nWARNING! - You need to specify which .xan file you want to preview\r\n\r\n"); return 0; } ifstream xan_ptr(xanname); if (!xan_ptr) { cerr << "Unable to open the specified xan file: " << xanname << endl; exit(1); } xan_ptr.close(); glutInitWindowSize(640, 480); glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH); glutCreateWindow("Xan_viewer"); myinit(); mouse = dmGLMouse::dmInitGLMouse(); camera = new dmGLPolarCamera_zup(); camera->setRadius(2.0); camera->setCOI(0.0, 0.0, 0.0); camera->setTranslationScale(0.02f); //simulation timing information. idt = 0.001; // global variable // render_rate = 20; //global variable // =========================================================================== // Initialize DUMMY DynaMechs environment - must occur before any linkage systems dmEnvironment *environment = Establish_Dummy_Env_Model(); environment->drawInit(); dmEnvironment::setEnvironment(environment); // =========================================================================== // Initialize a DUMMY DynaMechs linkage system G_robot = Establish_Dummy_Dm_Model(xanname); //global variable // =========================================================================== G_integrator = new dmIntegRK4(); G_integrator->addSystem(G_robot); glutReshapeFunc(myReshape); glutKeyboardFunc(processKeyboard); glutDisplayFunc(display); glutIdleFunc(updateFunc); dmGetSysTime(&last_tv); cout << endl; cout << "Use mouse to rotate/move/zoom the camera" << endl << endl; glutMainLoop(); return 0; /* ANSI C requires main to return int. */ }
void HumanoidController::HumanoidControl(ControlInfo & ci) { int taskRow = 0; Float discountFactor = 1; dmTimespec tv1, tv2, tv3, tv4; //Update Graphics Variables { ComPos[0] = pCom(0); ComPos[1] = pCom(1); ComPos[2] = pCom(2); ComDes[0] = pComDes(0); ComDes[1] = pComDes(1); ComDes[2] = pComDes(2); } // Perform Optimization { dmGetSysTime(&tv2); UpdateConstraintMatrix(); int maxPriorityLevels = OptimizationSchedule.maxCoeff(); const int numTasks = OptimizationSchedule.size(); if (maxPriorityLevels > 0) { for (int level=1; level<=maxPriorityLevels; level++) { taskConstrActive.setZero(numTasks); taskOptimActive.setZero(numTasks); bool runOpt = false; for (int i=0; i<numTasks;i ++) { if (OptimizationSchedule(i) == level) { taskOptimActive(i) = 1; runOpt = true; } else if ((OptimizationSchedule(i) < level) && (OptimizationSchedule(i) > -1)) { taskConstrActive(i) = 1; } } if (runOpt) { UpdateObjective(); UpdateHPTConstraintBounds(); dmGetSysTime(&tv3); //cout << "Optimizing level " << level << endl; Optimize(); for (int i=0; i<numTasks;i ++) { if (OptimizationSchedule(i) == level) { TaskBias(i) += TaskError(i); //cout << "Optimization Level " << level << " task error " << i << " = " << TaskError(i) << endl; } } } } } else { UpdateObjective(); UpdateHPTConstraintBounds(); dmGetSysTime(&tv3); Optimize(); } //exit(-1); // Extract Results hDotOpt = CentMomMat*qdd + cmBias; // Form Joint Input and simulate VectorXF jointInput = VectorXF::Zero(STATE_SIZE); // Extact Desired ZMP info zmpWrenchOpt.setZero(); Vector6F icsForce, localForce; Float * nICS = icsForce.data(), * nLoc = localForce.data(); Float * fICS = nICS+3, * fLoc = nLoc+3; for (int k1 = 0; k1 < NS; k1++) { LinkInfoStruct * listruct = artic->m_link_list[SupportIndices[k1]]; CartesianVector tmp; localForce = SupportXforms[k1].transpose()*fs.segment(6*k1,6); // Apply Spatial Force Transform Efficiently // Rotate Quantities APPLY_CARTESIAN_TENSOR(listruct->link_val2.R_ICS,nLoc,nICS); APPLY_CARTESIAN_TENSOR(listruct->link_val2.R_ICS,fLoc,fICS); // Add the r cross f CROSS_PRODUCT(listruct->link_val2.p_ICS,fICS,tmp); ADD_CARTESIAN_VECTOR(nICS,tmp); zmpWrenchOpt+=icsForce; } transformToZMP(zmpWrenchOpt,zmpPosOpt); int k = 7; // Skip over floating base (i=1 initially) for (int i=1; i<artic->m_link_list.size(); i++) { LinkInfoStruct * linki = artic->m_link_list[i]; if (linki->dof) { //cout << "Link " << i << " dof = " << linki->dof << endl; //cout << "qd = " << qdDm.segment(k,linki->dof).transpose() << endl; jointInput.segment(k,linki->dof) = tau.segment(linki->index_ext-6,linki->dof); k+=linki->link->getNumDOFs(); } } //cout << "Tau = " << tau.transpose() << endl; //cout << "Joint input = " << jointInput.transpose() << endl; //exit(-1); //jointInput.segment(7,NJ) = tau; artic->setJointInput(jointInput.data()); ComputeActualQdd(qddA); dmGetSysTime(&tv4); } //Populate Control Info Struct { ci.calcTime = timeDiff(tv1,tv2); ci.setupTime = timeDiff(tv2,tv3); ci.optimTime = timeDiff(tv3,tv4); ci.totalTime = timeDiff(tv1,tv4); ci.iter = iter; } #ifdef CONTROL_DEBUG // Debug Code { cout << "q " << q.transpose() << endl; cout << "qd " << qdDm.transpose() << endl; cout << "qd2 " << qd.transpose() << endl; cout << "Task Bias " << TaskBias << endl; //cout << "H = " << endl << artic->H << endl; cout << "CandG = " << endl << artic->CandG.transpose() << endl; if (simThread->sim_time > 0) { cout << setprecision(5); MSKboundkeye key; double bl,bu; for (int i=0; i<numCon; i++) { MSK_getbound(task, MSK_ACC_VAR, i, &key, &bl, &bu); cout << "i = " << i; switch (key) { case MSK_BK_FR: cout << " Free " << endl; break; case MSK_BK_LO: cout << " Lower Bound " << endl; break; case MSK_BK_UP: cout << " Upper Bound " << endl; break; case MSK_BK_FX: cout << " Fixed " << endl; break; case MSK_BK_RA: cout << " Ranged " << endl; break; default: cout << " Not sure(" << key << ")" << endl; break; } cout << bl << " to " << bu << endl; } cout << "x(57) = " << xx(57) << endl; cout << "tau = " << tau.transpose() << endl; cout << "qdd = " << qdd.transpose() << endl; cout << "fs = " << fs.transpose() << endl; //cout << "lambda = " << lambda.transpose() << endl; VectorXF a = TaskJacobian * qdd; //cout << "a" << endl; VectorXF e = TaskJacobian * qdd - TaskBias; cout << "e = " << e.transpose() << endl; MatrixXF H = artic->H; VectorXF CandG = artic->CandG; MatrixXF S = MatrixXF::Zero(NJ,NJ+6); S.block(0,6,NJ,NJ) = MatrixXF::Identity(NJ,NJ); VectorXF generalizedContactForce = VectorXF::Zero(NJ+6); for (int i=0; i<NS; i++) { generalizedContactForce += SupportJacobians[i].transpose()*fs.segment(6*i,6); } VectorXF qdd2 = H.inverse()*(S.transpose() * tau + generalizedContactForce- CandG); //cout << "qdd2 = " << qdd2.transpose() << endl << endl << endl; //cout << "CandG " << CandG.transpose() << endl; //cout << "Gen Contact Force " << generalizedContactForce.transpose() << endl; cout << "hdot " << (CentMomMat*qdd + cmBias).transpose() << endl; cout << "cmBias " << cmBias.transpose() << endl; cout << "qd " << qd.transpose() << endl; //VectorXF qdd3 = H.inverse()*(S.transpose() * tau - CandG); //FullPivHouseholderQR<MatrixXF> fact(H); cout <<"fNet \t" << (fs.segment(3,3) + fs.segment(9,3)).transpose() << endl; Vector3F g; g << 0,0,-9.81; cout <<"hdot - mg\t" << (CentMomMat*qdd + cmBias).segment(3,3).transpose() - totalMass * g.transpose()<< endl; exit(-1); } // Old Debug Code { VectorXF generalizedContactForce = VectorXF::Zero(NJ+6); Matrix6F X; MatrixXF Jac; X.setIdentity(); for (int i=0; i<NS; i++) { int linkIndex = SupportIndices[i]; artic->computeJacobian(linkIndex,X,Jac); dmRigidBody * link = (dmRigidBody *) artic->getLink(linkIndex); for (int j=0; j< link->getNumForces(); j++) { dmForce * f = link->getForce(j); Vector6F fContact; f->computeForce(artic->m_link_list[linkIndex]->link_val2,fContact.data()); generalizedContactForce += Jac.transpose()*fContact; } } cout << "J' f = " << generalizedContactForce.transpose() << endl; VectorXF qdd3 = H.inverse()*(S.transpose() * tau - CandG + generalizedContactForce); cout << "qdd3 = " << qdd3.transpose() << endl; VectorXF state = VectorXF::Zero(2*(NJ+7)); state.segment(0,NJ+7) = q; state.segment(NJ+7,NJ+7) = qdDm; VectorXF stateDot = VectorXF::Zero(2*(NJ+7)); //Process qdds artic->dynamics(state.data(),stateDot.data()); // VectorXF qdds = VectorXF::Zero(NJ+6); qdds.segment(0,6) = stateDot.segment(NJ+7,6); //cout << "w x v " << cr3(qd.segment(0,3))*qd.segment(3,3) << endl; qdds.segment(3,3) -= cr3(qdDm.segment(0,3))*qdDm.segment(3,3); qdds.segment(6,NJ) = stateDot.segment(NJ+7*2,NJ); Matrix3F ics_R_fb; copyRtoMat(artic->m_link_list[0]->link_val2.R_ICS,ics_R_fb); qdds.head(3) = ics_R_fb.transpose() * qdds.head(3); qdds.segment(3,3) = ics_R_fb.transpose() * qdds.segment(3,3); cout << "qdds = " << qdds.transpose() << endl; //cout << "CandG " << endl << CandG << endl; //cout << setprecision(6); //cout << "I_0^C = " << endl << artic->H.block(0,0,6,6) << endl; //exit(-1); } } #endif /*{ MatrixXF H = artic->H; VectorXF CandG = artic->CandG; MatrixXF S = MatrixXF::Zero(NJ,NJ+6); S.block(0,6,NJ,NJ) = MatrixXF::Identity(NJ,NJ); VectorXF generalizedContactForce = VectorXF::Zero(NJ+6); for (int i=0; i<NS; i++) { generalizedContactForce += SupportJacobians[i].transpose()*fs.segment(6*i,6); } qdd = H.inverse()*(S.transpose() * tau + generalizedContactForce- CandG); cout << setprecision(8); cout << "fs " << endl << fs << endl; cout << "qdd " << endl << qdd << endl; } exit(-1);*/ static int numTimes = 0; numTimes++; //Float dummy; //cin >> dummy; //if (numTimes == 2) { // exit(-1); //} //exit(-1); }