C++ FunctionSet::getSize方法代码示例

/** Solve the inverse dynamics system of equations for generalized coordinate forces, Tau. 
    Now the state is not given, but is constructed from known coordinates, q as functions of time.
	Coordinate functions must be twice differentiable. 
	NOTE: state dependent forces and other applied loads are NOT computed since these may depend on
	state variables (like muscle fiber lengths) that are not known */
Vector InverseDynamicsSolver::solve(State &s, const FunctionSet &Qs, double time)
{
	int nq = getModel().getNumCoordinates();

	if(Qs.getSize() != nq){
		throw Exception("InverseDynamicsSolver::solve invalid number of q functions.");
	}

	if( nq != getModel().getNumSpeeds()){
		throw Exception("InverseDynamicsSolver::solve using FunctionSet, nq != nu not supported.");
	}

	// update the State so we get the correct gravity and coriolis effects
	// direct references into the state so no allocation required
	s.updTime() = time;
	Vector &q = s.updQ();
	Vector &u = s.updU();
	Vector &udot = s.updUDot();

	for(int i=0; i<nq; i++){
		q[i] = Qs.evaluate(i, 0, time);
		u[i] = Qs.evaluate(i, 1, time);
		udot[i] = Qs.evaluate(i, 2, time);
	}

	// Perform general inverse dynamics
	return solve(s,	udot);
}

/**
 * Run the inverse Dynamics tool.
 */
bool InverseDynamicsTool::run()
{
	bool success = false;
	bool modelFromFile=true;
	try{
		//Load and create the indicated model
		if (!_model) 
			_model = new Model(_modelFileName);
		else
			modelFromFile = false;
		_model->printBasicInfo(cout);

		cout<<"Running tool " << getName() <<".\n"<<endl;

		// Do the maneuver to change then restore working directory 
		// so that the parsing code behaves properly if called from a different directory.
		string saveWorkingDirectory = IO::getCwd();
		string directoryOfSetupFile = IO::getParentDirectory(getDocumentFileName());
		IO::chDir(directoryOfSetupFile);

		const CoordinateSet &coords = _model->getCoordinateSet();
		int nq = _model->getNumCoordinates();

		FunctionSet *coordFunctions = NULL;
		//Storage *coordinateValues = NULL;

		if(hasCoordinateValues()){
			if(_lowpassCutoffFrequency>=0) {
				cout<<"\n\nLow-pass filtering coordinates data with a cutoff frequency of "<<_lowpassCutoffFrequency<<"..."<<endl<<endl;
				_coordinateValues->pad(_coordinateValues->getSize()/2);
				_coordinateValues->lowpassIIR(_lowpassCutoffFrequency);
				if (getVerboseLevel()==Debug) _coordinateValues->print("coordinateDataFiltered.sto");
			}
			// Convert degrees to radian if indicated
			if(_coordinateValues->isInDegrees()){
				_model->getSimbodyEngine().convertDegreesToRadians(*_coordinateValues);
			}
			// Create differentiable splines of the coordinate data
			coordFunctions = new GCVSplineSet(5, _coordinateValues);

			//Functions must correspond to model coordinates and their order for the solver
			for(int i=0; i<nq; i++){
				if(coordFunctions->contains(coords[i].getName())){
					coordFunctions->insert(i,coordFunctions->get(coords[i].getName()));
				}
				else{
					coordFunctions->insert(i,new Constant(coords[i].getDefaultValue()));
					std::cout << "InverseDynamicsTool: coordinate file does not contain coordinate " << coords[i].getName() << " assuming default value" << std::endl;
				}
			}
			if(coordFunctions->getSize() > nq){
				coordFunctions->setSize(nq);
			}
		}
		else{
			IO::chDir(saveWorkingDirectory);
			throw Exception("InverseDynamicsTool: no coordinate file found.");

		}

		bool externalLoads = createExternalLoads(_externalLoadsFileName, *_model, _coordinateValues);
		// Initialize the the model's underlying computational system and get its default state.
		SimTK::State& s = _model->initSystem();

		// Exclude user-specified forces from the dynamics for this analysis
		disableModelForces(*_model, s, _excludedForces);

		double first_time = _coordinateValues->getFirstTime();
		double last_time = _coordinateValues->getLastTime();

		// Determine the starting and final time for the Tool by comparing to what data is available
		double start_time = ( first_time > _timeRange[0]) ? first_time : _timeRange[0];
		double final_time = ( last_time < _timeRange[1]) ? last_time : _timeRange[1];
		int start_index = _coordinateValues->findIndex(start_time);
		int final_index = _coordinateValues->findIndex(final_time);

		// create the solver given the input data
		InverseDynamicsSolver ivdSolver(*_model);

		const clock_t start = clock();

		int nt = final_index-start_index+1;
		
		Array_<double> times(nt, 0.0);
		for(int i=0; i<nt; i++){
			times[i]=_coordinateValues->getStateVector(start_index+i)->getTime();
		}

		// Preallocate results
		Array_<Vector> genForceTraj(nt, Vector(nq, 0.0));

		// solve for the trajectory of generlized forces that correspond to the coordinate
		// trajectories provided
		ivdSolver.solve(s, *coordFunctions, times, genForceTraj);


		success = true;
//.........这里部分代码省略.........