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

/** 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);
}

//.........这里部分代码省略.........
		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;

		cout << "InverseDynamicsTool: " << nt << " time frames in " <<(double)(clock()-start)/CLOCKS_PER_SEC << "s\n" <<endl;
	
		JointSet jointsForEquivalentBodyForces;
		getJointsByName(*_model, _jointsForReportingBodyForces, jointsForEquivalentBodyForces);
		int nj = jointsForEquivalentBodyForces.getSize();

		Array<string> labels("time", nq+1);
		for(int i=0; i<nq; i++){
			labels[i+1] = coords[i].getName();
			labels[i+1] += (coords[i].getMotionType() == Coordinate::Rotational) ? "_moment" : "_force";
		}

		Array<string> body_force_labels("time", 6*nj+1);
		string XYZ = "XYZ";
		for(int i=0; i<nj; i++){
			string joint_body_label = jointsForEquivalentBodyForces[i].getName()+"_";
			joint_body_label += jointsForEquivalentBodyForces[i].getBody().getName();
			for(int k=0; k<3; ++k){
				body_force_labels[6*i+k+1] =  joint_body_label+"_F"+XYZ[k]; //first label is time
				body_force_labels[6*i+k+3+1] =  joint_body_label+"_M"+XYZ[k];
			}
		}

		Storage genForceResults(nt);
		Storage bodyForcesResults(nt);
		SpatialVec equivalentBodyForceAtJoint;

		for(int i=0; i<nt; i++){
			StateVector genForceVec(times[i], nq, &((genForceTraj[i])[0]));
			genForceResults.append(genForceVec);

			// if there are joints requested for equivalent body forces then calculate them
			if(nj>0){
				Vector forces(6*nj, 0.0);
				StateVector bodyForcesVec(times[i], 6*nj, &forces[0]);

				s.updTime() = times[i];
				Vector &q = s.updQ();
				Vector &u = s.updU();

				for(int j=0; j<nq; ++j){
					q[j] = coordFunctions->evaluate(j, 0, times[i]);
					u[j] = coordFunctions->evaluate(j, 1, times[i]);
				}
			
				for(int j=0; j<nj; ++j){
					equivalentBodyForceAtJoint = jointsForEquivalentBodyForces[j].calcEquivalentSpatialForce(s, genForceTraj[i]);
					for(int k=0; k<3; ++k){
						// body force components
						bodyForcesVec.setDataValue(6*j+k, equivalentBodyForceAtJoint[1][k]); 
						// body torque components
						bodyForcesVec.setDataValue(6*j+k+3, equivalentBodyForceAtJoint[0][k]);
					}
				}
				bodyForcesResults.append(bodyForcesVec);

			}
		}

		genForceResults.setColumnLabels(labels);
		genForceResults.setName("Inverse Dynamics Generalized Forces");

		IO::makeDir(getResultsDir());
		Storage::printResult(&genForceResults, _outputGenForceFileName, getResultsDir(), -1, ".sto");
		IO::chDir(saveWorkingDirectory);

		// if body forces to be reported for specified joints
		if(nj >0){
			bodyForcesResults.setColumnLabels(body_force_labels);
			bodyForcesResults.setName("Inverse Dynamics Body Forces at Specified Joints");

			IO::makeDir(getResultsDir());
			Storage::printResult(&bodyForcesResults, _outputBodyForcesAtJointsFileName, getResultsDir(), -1, ".sto");
			IO::chDir(saveWorkingDirectory);
		}

	}
	catch (const OpenSim::Exception& ex) {
		std::cout << "InverseDynamicsTool Failed: " << ex.what() << std::endl;
		throw (Exception("InverseDynamicsTool Failed, please see messages window for details..."));
	}

	if (modelFromFile) delete _model;
	return success;
}

/**
 * Compute the controls for a simulation.
 *
 * This method alters the control set in order to control the simulation.
 */
void CMC::
computeControls(SimTK::State& s, ControlSet &controlSet)
{
    // CONTROLS SHOULD BE RECOMPUTED- NEED A NEW TARGET TIME
    _tf = s.getTime() + _targetDT;

    int i,j;

    // TURN ANALYSES OFF
    _model->updAnalysisSet().setOn(false);

    // TIME STUFF
    double tiReal = s.getTime(); 
    double tfReal = _tf; 

    cout<<"CMC.computeControls:  t = "<<s.getTime()<<endl;
    if(_verbose) { 
        cout<<"\n\n----------------------------------\n";
        cout<<"integration step size = "<<_targetDT<<",  target time = "<<_tf<<endl;
    }

    // SET CORRECTIONS 
    int nq = _model->getNumCoordinates();
    int nu = _model->getNumSpeeds();
    FunctionSet *qSet = _predictor->getCMCActSubsys()->getCoordinateTrajectories();
    FunctionSet *uSet = _predictor->getCMCActSubsys()->getSpeedTrajectories();
    Array<double> qDesired(0.0,nq),uDesired(0.0,nu);
    qSet->evaluate(qDesired,0,tiReal);
    if(uSet!=NULL) {
        uSet->evaluate(uDesired,0,tiReal);
    } else {
        qSet->evaluate(uDesired,1,tiReal);
    }
    Array<double> qCorrection(0.0,nq),uCorrection(0.0,nu);
       const Vector& q = s.getQ();
       const Vector& u = s.getU();

    for(i=0;i<nq;i++) qCorrection[i] = q[i] - qDesired[i];
    for(i=0;i<nu;i++) uCorrection[i] = u[i] - uDesired[i];

    _predictor->getCMCActSubsys()->setCoordinateCorrections(&qCorrection[0]);
    _predictor->getCMCActSubsys()->setSpeedCorrections(&uCorrection[0]);

    if( _verbose ) {
        cout << "\n=============================" << endl;
        cout << "\nCMC:computeControls"  << endl;
        cout << "\nq's = " << s.getQ() << endl;
        cout << "\nu's = " << s.getU() << endl;
        cout << "\nz's = " << s.getZ() << endl;
        cout<<"\nqDesired:"<<qDesired << endl;
        cout<<"\nuDesired:"<<uDesired << endl;
        cout<<"\nQCorrections:"<<qCorrection << endl;
        cout<<"\nUCorrections:"<<uCorrection << endl;
    }

    // realize to Velocity because some tasks (eg. CMC_Point) need to be
    // at velocity to compute errors
    _model->getMultibodySystem().realize(s, Stage::Velocity );

    // ERRORS
    _taskSet->computeErrors(s, tiReal);
    _taskSet->recordErrorsAsLastErrors();
    Array<double> &pErr = _taskSet->getPositionErrors();
    Array<double> &vErr = _taskSet->getVelocityErrors();
    if(_verbose) cout<<"\nErrors at time "<<s.getTime()<<":"<<endl;
    int e=0;
    for(i=0;i<_taskSet->getSize();i++) {
        
        TrackingTask& task = _taskSet->get(i);

        if(_verbose) {
            for(j=0;j<task.getNumTaskFunctions();j++) {
                cout<<task.getName()<<":  ";
                cout<<"pErr="<<pErr[e]<<" vErr="<<vErr[e]<<endl;
                e++;
            }
        }
    }

    double *err = new double[pErr.getSize()];
    for(i=0;i<pErr.getSize();i++) err[i] = pErr[i];
    _pErrStore->append(tiReal,pErr.getSize(),err);
    for(i=0;i<vErr.getSize();i++) err[i] = vErr[i];
    _vErrStore->append(tiReal,vErr.getSize(),err);

    
    // COMPUTE DESIRED ACCELERATIONS
    _taskSet->computeDesiredAccelerations(s, tiReal,tfReal);

    // Set the weight of the stress term in the optimization target based on this sigmoid-function-blending
    // Note that if no task limits are set then by default the weight will be 1.
    // TODO: clean this up -- currently using dynamic_casts to make sure we're not using fast target, etc.
    if(dynamic_cast<ActuatorForceTarget*>(_target)) {
        double relativeTau = 0.1;
        ActuatorForceTarget *realTarget = dynamic_cast<ActuatorForceTarget*>(_target);
//.........这里部分代码省略.........