C++ State::getZ方法代码示例

void testCopyModel(string fileName)
{
	Model *model = new Model(fileName);
	SimTK::State defaultState = model->initSystem();
	Model *modelCopy = new Model(*model);
	// At this point properties should all match. assert that
	ASSERT(*model==*modelCopy);
	delete model;
	SimTK::State& defaultStateOfCopy = modelCopy->initSystem();
	// Compare state
	defaultState.getY().dump("defaultState:Y");
	ASSERT ((defaultState.getY()-defaultStateOfCopy.getY()).norm() < 1e-7);
	defaultState.getZ().dump("defaultState:Z");
	ASSERT ((defaultState.getZ()-defaultStateOfCopy.getZ()).norm() < 1e-7);
	
	//  Now delete original model and make sure copy can stand
	Model *newModel = modelCopy->clone();
	// Compare state again
	delete modelCopy;
	SimTK::State& defaultStateOfCopy2 = newModel->initSystem();
	// Compare state
	ASSERT ((defaultState.getY()-defaultStateOfCopy2.getY()).norm() < 1e-7);
	ASSERT ((defaultState.getZ()-defaultStateOfCopy2.getZ()).norm() < 1e-7);
	delete newModel;
}

/**
 * This method is called at the beginning of an analysis so that any
 * necessary initializations may be performed.
 *
 * This method is meant to be called at the beginning of an integration 
 *
 * @param s Current state .
 *
 * @return -1 on error, 0 otherwise.
 */
int StaticOptimization::
begin(SimTK::State& s )
{
    if(!proceed()) return(0);

    // Make a working copy of the model
    delete _modelWorkingCopy;
    _modelWorkingCopy = _model->clone();
    _modelWorkingCopy->initSystem();

    // Replace model force set with only generalized forces
    if(_model) {
        SimTK::State& sWorkingCopyTemp = _modelWorkingCopy->updWorkingState();
        // Update the _forceSet we'll be computing inverse dynamics for
        if(_ownsForceSet) delete _forceSet;
        if(_useModelForceSet) {
            // Set pointer to model's internal force set
            _forceSet = &_modelWorkingCopy->updForceSet();
            _ownsForceSet = false;
        } else {
            ForceSet& as = _modelWorkingCopy->updForceSet();
            // Keep a copy of forces that are not muscles to restore them back.
            ForceSet* saveForces = as.clone();
            // Generate an force set consisting of a coordinate actuator for every unconstrained degree of freedom
            _forceSet = CoordinateActuator::CreateForceSetOfCoordinateActuatorsForModel(sWorkingCopyTemp,*_modelWorkingCopy,1,false);
            _ownsForceSet = false;
            _modelWorkingCopy->setAllControllersEnabled(false);
            _numCoordinateActuators = _forceSet->getSize();
            // Copy whatever forces that are not muscles back into the model
            
            for(int i=0; i<saveForces->getSize(); i++){
                const Force& f=saveForces->get(i);
                if ((dynamic_cast<const Muscle*>(&saveForces->get(i)))==NULL)
                    as.append(saveForces->get(i).clone());
            }
        }

        SimTK::State& sWorkingCopy = _modelWorkingCopy->initSystem();
        // Set modeling options for Actuators to be overriden
        for(int i=0; i<_forceSet->getSize(); i++) {
            ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->get(i));
            if( act ) {
                act->overrideActuation(sWorkingCopy, true);
            }
        }

        sWorkingCopy.setTime(s.getTime());
        sWorkingCopy.setQ(s.getQ());
        sWorkingCopy.setU(s.getU());
        sWorkingCopy.setZ(s.getZ());
        _modelWorkingCopy->getMultibodySystem().realize(s,SimTK::Stage::Velocity);
        _modelWorkingCopy->equilibrateMuscles(sWorkingCopy);
        // Gather indices into speed set corresponding to the unconstrained degrees of freedom 
        // (for which we will set acceleration constraints)
        _accelerationIndices.setSize(0);
        const CoordinateSet& coordSet = _model->getCoordinateSet();
        for(int i=0; i<coordSet.getSize(); i++) {
            const Coordinate& coord = coordSet.get(i);
            if(!coord.isConstrained(sWorkingCopy)) {
                Array<int> inds = _statesStore->
                    getColumnIndicesForIdentifier(coord.getName()) ;
                _accelerationIndices.append(inds[0]);
            }
        }

        int na = _forceSet->getSize();
        int nacc = _accelerationIndices.getSize();

        if(na < nacc) 
            throw(Exception("StaticOptimization: ERROR- over-constrained "
                "system -- need at least as many forces as there are degrees of freedom.\n") );

        _forceReporter.reset(new ForceReporter(_modelWorkingCopy));
        _forceReporter->begin(sWorkingCopy);
        _forceReporter->updForceStorage().reset();

        _parameters.resize(_modelWorkingCopy->getNumControls());
        _parameters = 0;
    }

    _statesSplineSet=GCVSplineSet(5,_statesStore);

    // DESCRIPTION AND LABELS
    constructDescription();
    constructColumnLabels();

    deleteStorage();
    allocateStorage();

    // RESET STORAGE
//.........这里部分代码省略.........

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

/**
 * Compute the initial states for a simulation.
 *
 * The caller should send in an initial guess.  The Qs and Us are set
 * based on the desired trajectories.  The actuator states are set by
 * solving for a desired set of actuator forces, and then letting the states
 * come to equilibrium for those forces.
 *
 * @param rTI Initial time in normalized time.  Note this is changed to
 * the time corresponding to the new initial states on return.
 * @param s Initial states.
 */
void CMC::
computeInitialStates(SimTK::State& s, double &rTI)
{
    
    int i,j;

    int N = _predictor->getNX();
    SimTK::State initialState = s;
    Array<double> xmin(0.01,N),forces(0.0,N);

    double tiReal = rTI;
    if( _verbose ) {
        cout<<"\n\n=============================================\n";
        cout<<"enter CMC.computeInitialStates: ti="<< rTI << "  q's=" << s.getQ() <<endl;
        cout<<"\nenter CMC.computeInitialStates: ti="<< rTI << "  u's=" << s.getU() <<endl;
        cout<<"\nenter CMC.computeInitialStates: ti="<< rTI << "  z's=" << s.getZ() <<endl;
        cout<<"=============================================\n";
    }


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

    // CONSTRUCT CONTROL SET
    ControlSet xiSet;

    for(i=0;i< getNumControls();i++) {
        ControlConstant *x = new ControlConstant();
        x->setName(_controlSet.get(i).getName());
        x->setIsModelControl(true);
        // This is not a very good way to set the bounds on the controls because ConrtolConstant only supports constant
        // min/max bounds but we may have time-dependent min/max curves specified in the controls constraints file
        //
        Control& xPredictor = _controlSet.get(i);
        x->setDefaultParameterMin(xPredictor.getDefaultParameterMin());
        x->setDefaultParameterMax(xPredictor.getDefaultParameterMax());
        double xmin = xPredictor.getControlValueMin(tiReal);
        if(!SimTK::isNaN(xmin)) x->setControlValueMin(tiReal,xmin);
        double xmax = xPredictor.getControlValueMax(tiReal);
        if(!SimTK::isNaN(xmax)) x->setControlValueMax(tiReal,xmax);
        xiSet.adoptAndAppend(x);
    }

    // ACTUATOR EQUILIBRIUM
    // 1
    //
    // perform integration but reset the coords and speeds so only actuator
    // states at changed

    obtainActuatorEquilibrium(s,tiReal,0.200,xmin,true);
    if( _verbose ) {
        cout<<"\n\n=============================================\n";
        cout<<"#1 act Equ.  CMC.computeInitialStates: ti="<< rTI << "  q's=" << s.getQ() <<endl;
        cout<<"\n#1 act Equ.  CMC.computeInitialStates: ti="<< rTI << "  u's=" << s.getU() <<endl;
        cout<<"\n#1 act Equ.  CMC.computeInitialStates: ti="<< rTI << "  z's=" << s.getZ() <<endl;
        cout<<"=============================================\n";
    }
    restoreConfiguration( s, initialState ); // set internal coord,speeds to initial vals. 

    // 2
    obtainActuatorEquilibrium(s,tiReal,0.200,xmin,true);
    if( _verbose ) {
        cout<<"\n\n=============================================\n";
        cout<<"#2 act Equ.  CMC.computeInitialStates: ti="<< rTI << "  q's=" << s.getQ() <<endl;
        cout<<"\n#2 act Equ.  CMC.computeInitialStates: ti="<< rTI << "  u's=" << s.getU() <<endl;
        cout<<"\n#2 act Equ.  CMC.computeInitialStates: ti="<< rTI << "  z's=" << s.getZ() <<endl;
        cout<<"=============================================\n";
    }
    restoreConfiguration( s, initialState );

    // CHANGE THE TARGET DT ON THE CONTROLLER TEMPORARILY
    double oldTargetDT = getTargetDT();
    double newTargetDT = 0.030;
    setTargetDT(newTargetDT);

    // REPEATEDLY CONTROL OVER THE FIRST TIME STEP
    Array<double> xi(0.0, getNumControls());
    for(i=0;i<2;i++) {

        // CLEAR ANY PREVIOUS CONTROL NODES
        for(j=0;j<_controlSet.getSize();j++) {
            ControlLinear& control = (ControlLinear&)_controlSet.get(j);
            control.clearControlNodes();
        }

        // COMPUTE CONTROLS

        s.updTime() = rTI;
//.........这里部分代码省略.........

/**
 * Compute and record the results.
 *
 * This method, for the purpose of example, records the position and
 * orientation of each body in the model.  You will need to customize it
 * to perform your analysis.
 *
 * @param aT Current time in the simulation.
 * @param aX Current values of the controls.
 * @param aY Current values of the states: includes generalized coords and speeds
 */
int InducedAccelerations::record(const SimTK::State& s)
{
	int nu = _model->getNumSpeeds();
	double aT = s.getTime();
	cout << "time = " << aT << endl;

	SimTK::Vector Q = s.getQ();

	// Reset Accelerations for coordinates at this time step
	for(int i=0;i<_coordSet.getSize();i++) {
		_coordIndAccs[i]->setSize(0);
	}

	// Reset Accelerations for bodies at this time step
	for(int i=0;i<_bodySet.getSize();i++) {
		_bodyIndAccs[i]->setSize(0);
	}

	// Reset Accelerations for system center of mass at this time step
	_comIndAccs.setSize(0);
	_constraintReactions.setSize(0);

    SimTK::State s_analysis = _model->getWorkingState();

	_model->initStateWithoutRecreatingSystem(s_analysis);
	// Just need to set current time and position to determine state of constraints
	s_analysis.setTime(aT);
	s_analysis.setQ(Q);

	// Check the external forces and determine if contact constraints should be applied at this time
	// and turn constraint on if it should be.
	Array<bool> constraintOn = applyContactConstraintAccordingToExternalForces(s_analysis);

	// Hang on to a state that has the right flags for contact constraints turned on/off
	_model->setPropertiesFromState(s_analysis);
	// Use this state for the remainder of this step (record)
	s_analysis = _model->getMultibodySystem().realizeTopology();
	// DO NOT recreate the system, will lose location of constraint
	_model->initStateWithoutRecreatingSystem(s_analysis);

	// Cycle through the force contributors to the system acceleration
	for(int c=0; c< _contributors.getSize(); c++){			
		//cout << "Solving for contributor: " << _contributors[c] << endl;
		// Need to be at the dynamics stage to disable a force
		_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Dynamics);
		
		if(_contributors[c] == "total"){
			// Set gravity ON
			_model->getGravityForce().enable(s_analysis);

			//Use same conditions on constraints
			s_analysis.setTime(aT);
			// Set the configuration (gen. coords and speeds) of the model.
			s_analysis.setQ(Q);
			s_analysis.setU(s.getU());
			s_analysis.setZ(s.getZ());

			//Make sure all the actuators are on!
			for(int f=0; f<_model->getActuators().getSize(); f++){
				_model->updActuators().get(f).setDisabled(s_analysis, false);
			}

			// Get to  the point where we can evaluate unilateral constraint conditions
			 _model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Acceleration);

		    /* *********************************** ERROR CHECKING *******************************
			SimTK::Vec3 pcom =_model->getMultibodySystem().getMatterSubsystem().calcSystemMassCenterLocationInGround(s_analysis);
			SimTK::Vec3 vcom =_model->getMultibodySystem().getMatterSubsystem().calcSystemMassCenterVelocityInGround(s_analysis);
			SimTK::Vec3 acom =_model->getMultibodySystem().getMatterSubsystem().calcSystemMassCenterAccelerationInGround(s_analysis);

			SimTK::Matrix M;
			_model->getMultibodySystem().getMatterSubsystem().calcM(s_analysis, M);
			cout << "mass matrix: " << M << endl;

			SimTK::Inertia sysInertia = _model->getMultibodySystem().getMatterSubsystem().calcSystemCentralInertiaInGround(s_analysis);
			cout << "system inertia: " << sysInertia << endl;

			SimTK::SpatialVec sysMomentum =_model->getMultibodySystem().getMatterSubsystem().calcSystemMomentumAboutGroundOrigin(s_analysis);
			cout << "system momentum: " << sysMomentum << endl;

			const SimTK::Vector &appliedMobilityForces = _model->getMultibodySystem().getMobilityForces(s_analysis, SimTK::Stage::Dynamics);
			appliedMobilityForces.dump("All Applied Mobility Forces");
		
			// Get all applied body forces like those from conact
			const SimTK::Vector_<SimTK::SpatialVec>& appliedBodyForces = _model->getMultibodySystem().getRigidBodyForces(s_analysis, SimTK::Stage::Dynamics);
			appliedBodyForces.dump("All Applied Body Forces");

			SimTK::Vector ucUdot;
			SimTK::Vector_<SimTK::SpatialVec> ucA_GB;
//.........这里部分代码省略.........