Пример #1
0
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);
    }
}
Пример #2
0
// 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
}
Пример #3
0
//----------------------------------------------------------------------------
//    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. */
}
Пример #4
0
//----------------------------------------------------------------------------
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;
   }
}
Пример #5
0
//----------------------------------------------------------------------------
//    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. */
}
Пример #6
0
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);
}