Esempio n. 1
0
/**
 * Load and read file
 */
void BVHReader::readData()
{
	
	ifstream *datafile = new ifstream(this->filename);
	if (datafile->is_open())
	{
		string line;
		while (!datafile->eof())
		{
			getline (*datafile, line, '\n');
			// Hierarchy
			if(line.compare("HIERARCHY") == 0) {
				doHierarchy(datafile);
			}
			// Animation data
			else if(line.compare("MOTION") == 0) {
				doMotion(datafile);
			}
		}
		datafile->close();
		//
		for (int i=0;i<objectsNumber;i++) {
			BVHObject *bvh = (*sequence)[i];
		}
	} else {
		cout << "Unable to open file";
	}
}
void RK_CALL manipulator_dynamics_model::computeOutput(double aTime,const ReaK::vect_n<double>& aState, ReaK::vect_n<double>& aOutput) {
  setJointStates(aState);
  
  doMotion();
  clearForce();
  doForce();
  
  aOutput.resize(getOutputsCount());
  
  unsigned int i = 0;
  for(std::vector< shared_ptr<system_output> >::iterator it = mOutputs.begin(); it != mOutputs.end(); ++it)
    for(unsigned int k = 0; k < (*it)->getOutputCount(); ++k)
      aOutput[i++] = (*it)->getOutput(k);
};
void RK_CALL manipulator_dynamics_model::computeStateRate(double aTime,const vect_n<double>& aState, vect_n<double>& aStateRate) {
  setJointStates(aState);
  
  doMotion();
  clearForce();
  doForce();
  
  aStateRate.resize(getJointStatesCount());
  
  unsigned int j = 0;
  for(std::vector< shared_ptr< gen_coord<double> > >::const_iterator it = mCoords.begin(); 
      it < mCoords.end(); ++it, ++j)
    aStateRate[j] = (*it)->q_dot;

  for(std::vector< shared_ptr< frame_2D<double> > >::const_iterator it = mFrames2D.begin(); 
      it < mFrames2D.end(); ++it) {
    aStateRate[j] = (*it)->Velocity[0]; ++j;
    aStateRate[j] = (*it)->Velocity[1]; ++j;
    aStateRate[j] = -(*it)->Rotation[1] * (*it)->AngVelocity; ++j;
    aStateRate[j] =  (*it)->Rotation[0] * (*it)->AngVelocity; ++j;
  };

  for(std::vector< shared_ptr< frame_3D<double> > >::const_iterator it = mFrames3D.begin(); 
      it < mFrames3D.end(); ++it) {
    aStateRate[j] = (*it)->Velocity[0]; ++j;
    aStateRate[j] = (*it)->Velocity[1]; ++j;
    aStateRate[j] = (*it)->Velocity[2]; ++j;
    aStateRate[j] = (*it)->QuatDot[0]; ++j;
    aStateRate[j] = (*it)->QuatDot[1]; ++j; 
    aStateRate[j] = (*it)->QuatDot[2]; ++j; 
    aStateRate[j] = (*it)->QuatDot[3]; ++j; 
  };
  
  for(std::vector< shared_ptr< gen_coord<double> > >::const_iterator it = mCoords.begin(); 
      it < mCoords.end(); ++it, ++j)
    aStateRate[j] = (*it)->f;

  for(std::vector< shared_ptr< frame_2D<double> > >::const_iterator it = mFrames2D.begin(); 
      it < mFrames2D.end(); ++it) {
    aStateRate[j] = (*it)->Force[0]; ++j;
    aStateRate[j] = (*it)->Force[1]; ++j;
    aStateRate[j] = (*it)->Torque; ++j;
  };

  for(std::vector< shared_ptr< frame_3D<double> > >::const_iterator it = mFrames3D.begin(); 
      it < mFrames3D.end(); ++it) {
    aStateRate[j] = (*it)->Force[0]; ++j;
    aStateRate[j] = (*it)->Force[1]; ++j;
    aStateRate[j] = (*it)->Force[2]; ++j;
    aStateRate[j] = (*it)->Torque[0]; ++j;
    aStateRate[j] = (*it)->Torque[1]; ++j; 
    aStateRate[j] = (*it)->Torque[2]; ++j; 
  };
  
  mat<double,mat_structure::symmetric> Msys(getJointAccelerationsCount());
  getMassMatrix(Msys);
  try {
    mat_vect_adaptor< vect_n<double> > acc_as_mat(aStateRate, getJointAccelerationsCount(), 1, getJointPositionsCount());
    linsolve_Cholesky(Msys,acc_as_mat);
  } catch(singularity_error& e) { RK_UNUSED(e);
    std::stringstream ss; ss << "Mass matrix is singular in the manipulator model '" << getName() << "' at time " << aTime << " seconds.";
    throw singularity_error(ss.str());
  };
};