Example #1
0
void Workspace::LobattoTwo(double **&vcm,double **&omega,double **&torque, double **&fcm){
	int numsys = currentIndex;
	int numbodies;				
	double time = 0.0;   
	int * mappings;
	double SysKE =0.0;		
	for (int i = 0; i <= numsys; i++){
		mappings = system[i].system->GetMappings();
		numbodies = system[i].system->GetNumBodies() - 1;
		Matrix FF(6,numbodies);				
				
		for (int j=1; j<=numbodies; j++){
			FF(1,j)  = torque[mappings[j - 1]-1][0]*ConFac;
			FF(2,j)  = torque[mappings[j - 1]-1][1]*ConFac;
			FF(3,j)  = torque[mappings[j - 1]-1][2]*ConFac;
			FF(4,j)  = fcm[mappings[j - 1]-1][0]*ConFac;
			FF(5,j)  = fcm[mappings[j - 1]-1][1]*ConFac;
			FF(6,j)  = fcm[mappings[j - 1]-1][2]*ConFac;
		}			
		
		//------------------------------------//
		// Get a solver and solve that system.		
		Solver * theSolver = Solver::GetSolver((SolverType)system[i].solver);		
		theSolver->SetSystem(system[i].system);    			
		theSolver->Solve(time, FF);		
		ColMatrix tempv = *((*theSolver).GetStateDerivative());
		ColMatrix tempa = *((*theSolver).GetStateDerivativeDerivative());
		*((*theSolver).GetStateDerivative()) = tempv + Thalf*tempa;
		
						
		int numjoints = system[i].system->joints.GetNumElements();
		for(int k = 0; k < numjoints; k++){
			system[i].system->joints(k)->ForwardKinematics(); 
		}				
		    		
		for(int k = 0; k < numbodies; k++){
			Vect3 temp1 =system[i].system->bodies(k+1)->r;			
			Vect3 temp2 =system[i].system->bodies(k+1)->v;	    	
			Vect3 temp3 =system[i].system->bodies(k+1)->omega;
			SysKE = SysKE + system[i].system->bodies(k+1)->KE;
			for(int m = 0; m < 3; m++){
				vcm[mappings[k]-1][m]   = temp2(m+1);
				omega[mappings[k]-1][m] = temp3(m+1);
			}			
		}
		delete theSolver;
	}	
}
Example #2
0
  void Workspace::RKStep(double **&xcm, double **&vcm,double **&omega,double **&torque, double **&fcm, double **&ex_space, double **&ey_space, double **&ez_space){
	
	  double a[6];
	  double b[6][6];
	  double c[6];
	  //double e[6];
	 
	  a[1] = 0.2;
	  a[2] = 0.3;
	  a[3] = 0.6;
	  a[4] = 1.0;
	  a[5] = 0.875;

	  b[1][0] = 0.2;
	  b[2][0] = 0.075;           b[2][1] = 0.225;
	  b[3][0] = 0.3;             b[3][1] = -0.9;        b[3][2] = 1.2;
	  b[4][0] = -11.0/54.0;      b[4][1] = 2.5;         b[4][2] = -70.0/27.0;    b[4][3] = 35.0/27.0;
	  b[5][0] = 1631.0/55296.0; b[5][1] = 175.0/512.0; b[5][2] = 575.0/13824.0; b[5][3] = 44275.0/110592.0; b[5][4] = 253.0/4096.0;

	  c[0] = 37.0/378.0;
	  c[1] = 0.0;
	  c[2] = 250.0/621.0;
	  c[3] = 125.0/594.0;
	  c[4] = 0.0;
	  c[5] = 512.0/1771.0;

	  int numsys = currentIndex; 
	  int numbodies;				
	  double time = 0.0;   
	  int * mappings;
	  double SysKE =0.0;

	  
	  for (int i = 0; i <= numsys; i++){
		  mappings = system[i].system->GetMappings();  

		  numbodies = system[i].system->GetNumBodies() - 1;
		  Matrix FF(6,numbodies);
		  for (int j=1; j<=numbodies; j++){
			  FF(1,j)  = ConFac*torque[mappings[j - 1]-1][0];
			  FF(2,j)  = ConFac*torque[mappings[j - 1]-1][1];
			  FF(3,j)  = ConFac*torque[mappings[j - 1]-1][2];            			   

			  FF(4,j)  = ConFac*fcm[mappings[j - 1]-1][0];
			  FF(5,j)  = ConFac*fcm[mappings[j - 1]-1][1];
			  FF(6,j)  = ConFac*fcm[mappings[j - 1]-1][2];
		  }					  
		
		
		  //------------------------------------//
		// Get a solver and solve that system.		
		  Solver * theSolver = Solver::GetSolver((SolverType)system[i].solver);		
		  theSolver->SetSystem(system[i].system);    			
		  theSolver->Solve(time, FF);	
	
		  ColMatrix initial_x;  
		  ColMatrix initial_xdot;
		  ColMatMap* x;
		  ColMatMap* xdot;
		  ColMatMap* xdotdot;
	
		  x = theSolver->GetState();  
		  xdot = theSolver->GetStateDerivative();
		  xdotdot=theSolver->GetStateDerivativeDerivative(); 
	
		  initial_x = *x;  
		  initial_xdot = *xdot;  
		  ColMatrix f[6];
		  ColMatrix ff[6];
	
		  ff[0] = initial_xdot; 
		  f[0] = *xdotdot; 
	
		  for(int ii=1;ii<6;ii++){
			  time = a[ii] * Tfull;		
			  (*x) = initial_x;		
			  (*xdot) = initial_xdot;		
			  for(int j=0;j<ii;j++){
				  (*x) = (*x) + (b[ii][j]*Tfull)*ff[j];      		
				  (*xdot) = (*xdot) + (b[ii][j]*Tfull)*f[j];      		
			  }			
			  theSolver->Solve(time,FF);    
			  f[ii] = (*xdotdot);
			  ff[ii] = (*xdot);
		  }  	
  
	
		  (*x) = initial_x + (c[0]*Tfull)*ff[0] + (c[2]*Tfull)*ff[2] + (c[3]*Tfull)*ff[3] + (c[5]*Tfull)*ff[5];  
	
		  (*xdot) = initial_xdot + (c[0]*Tfull)*f[0] + (c[2]*Tfull)*f[2] + (c[3]*Tfull)*f[3] + (c[5]*Tfull)*f[5];  
	
	
		  int numjoints = system[i].system->joints.GetNumElements();
		  for(int k = 0; k < numjoints; k++){
			  system[i].system->joints(k)->ForwardKinematics();
		  }		      
						      	
		
		  for(int k = 0; k < numbodies; k++){
			  Vect3 temp1 =system[i].system->bodies(k+1)->r;
			  Vect3 temp2 =system[i].system->bodies(k+1)->v;
			  Vect3 temp3 =system[i].system->bodies(k+1)->omega;
			  Mat3x3 temp4 =system[i].system->bodies(k+1)->n_C_k;
			  SysKE = SysKE + system[i].system->bodies(k+1)->KE;			  
			  for(int m = 0; m < 3; m++){
				  xcm[mappings[k]-1][m]   = temp1(m+1);
				  vcm[mappings[k]-1][m]   = temp2(m+1);
				  omega[mappings[k]-1][m] = temp3(m+1);
				
				  ex_space[mappings[k]-1][m] = temp4(m+1,1);				
				  ey_space[mappings[k]-1][m] = temp4(m+1,2);
				  ez_space[mappings[k]-1][m] = temp4(m+1,3);
			  }         
		  } 		
		  delete theSolver;		      
	  }	 	
  }