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

/**
 * 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 begining of an integration 
 *
 * @param s current state of System
 *
 * @return -1 on error, 0 otherwise.
 */
int Kinematics::
begin( SimTK::State& s )
{
	if(!proceed()) return(0);

	double time = s.getTime();
	
	// RESET STORAGE
	_pStore->reset(time);
	_vStore->reset(time);
	if (_aStore) _aStore->reset(time);

	// RECORD
	int status = 0;
	if(_pStore->getSize()<=0) {
        if(_recordAccelerations && s.getSystemStage() < SimTK::Stage::Acceleration ) 
            _model->getMultibodySystem().realize(s, SimTK::Stage::Acceleration);
		else
			_model->getMultibodySystem().realize(s, SimTK::Stage::Velocity);

		status = record(s);
	}

	return(status);
}

bool RollingOnSurfaceConstraint::setDisabled(SimTK::State& state, bool isDisabled)
{
    // All constraints treated the same as default behavior i.e. at initilization
    std::vector<bool> shouldBeOn(_numConstraintEquations, !isDisabled);

    // If dynamics has been realized, then this is an attempt to enable/disable the constraint
    // during a computation and not an initialization, in which case we must check the 
    // unilateral conditions for each constraint
    if(state.getSystemStage() > Stage::Dynamics)
        shouldBeOn = unilateralConditionsSatisfied(state);

    return setDisabled(state, isDisabled, shouldBeOn);
}

/** Solve the inverse dynamics system of equations for generalized coordinate forces, Tau. 
    Applied loads are explicity provided as generalized coordinate forces (MobilityForces)
	and/or a Vector of Spatial-body forces */
Vector InverseDynamicsSolver::solve(const SimTK::State &s, const SimTK::Vector &udot, 
	const SimTK::Vector &appliedMobilityForces, const SimTK::Vector_<SimTK::SpatialVec>& appliedBodyForces)
{
	//Results of the inverse dynamics for the generalized forces to satisfy accelerations
	Vector residualMobilityForces;

	if(s.getSystemStage() < SimTK::Stage::Dynamics)
		getModel().getMultibodySystem().realize(s,SimTK::Stage::Dynamics);
	
	// Perform inverse dynamics
	getModel().getMultibodySystem().getMatterSubsystem().calcResidualForceIgnoringConstraints(s,
			appliedMobilityForces, appliedBodyForces, udot, residualMobilityForces);

	return residualMobilityForces;
}

/**
 * From a potentially partial specification of the generalized coordinates,
 * form a complete storage of the generalized coordinates (q's) and
 * generalized speeds (u's).
 *
 * @param aQIn Storage containing the q's or a subset of the q's.  Rotational
 * q's should be in degrees.
 * @param rQComplete Storage containing all the q's.  If q's were not
 * in aQIn, the values are set to 0.0.  When a q is constrained, its value
 * is altered to be consistent with the constraint.  The caller is responsible
 * for deleting the memory associated with this storage.
 * @param rUComplete Storage containing all the u's.  The generalized speeds
 * are obtained by spline fitting the q's and differentiating the splines.
 * When a u is constrained, its value is altered to be consistent with the
 * constraint.  The caller is responsible for deleting the memory
 * associated with this storage.
 */
void SimbodyEngine::
formCompleteStorages( const SimTK::State& s, const OpenSim::Storage &aQIn,
    OpenSim::Storage *&rQComplete,OpenSim::Storage *&rUComplete) const
{
    int i;
    int nq = _model->getNumCoordinates();
    int nu = _model->getNumSpeeds();

    // Get coordinate file indices
    Array<string> columnLabels, speedLabels, coordStateNames;
    columnLabels.append("time");
    speedLabels = columnLabels;

    Array<int> index(-1,nq);
    const CoordinateSet& coordinateSet = _model->getCoordinateSet();
    int sizeCoordSet = coordinateSet.getSize();
    for(i=0;i<sizeCoordSet;i++) {
        Coordinate& coord = coordinateSet.get(i);
        string prefix = coord.getJoint().getName() + "/" + coord.getName() + "/";
        coordStateNames = coord.getStateVariableNames();
        columnLabels.append(prefix+coordStateNames[0]);
        speedLabels.append(prefix+coordStateNames[1]);
        int fix = aQIn.getStateIndex(coord.getName());
        if (fix < 0) {
            fix = aQIn.getStateIndex(columnLabels[i+1]);
        }

        index[i] = fix;
        if(index[i]<0) {
            string msg = "Model::formCompleteStorages(): WARNING- Did not find column ";
            msg += coordStateNames[0];
            msg += " in storage object.\n";
            cout << msg << endl;
        }
    }

    // Extract Coordinates
    double time;
    Array<double> data(0.0);
    Array<double> q(0.0,nq);
    Storage *qStore = new Storage();
    qStore->setInDegrees(aQIn.isInDegrees());
    qStore->setName("GeneralizedCoordinates");
    qStore->setColumnLabels(columnLabels);
    int size = aQIn.getSize();
    StateVector *vector;
    int j;
    for(i=0;i<size;i++) {
        vector = aQIn.getStateVector(i);
        data = vector->getData();
        time = vector->getTime();

        for(j=0;j<nq;j++) {
            q[j] = 0.0;
            if(index[j]<0) continue;
            q[j] = data[index[j]];
        }

        qStore->append(time,nq,&q[0]);
    }

    // Convert to radians
    if (aQIn.isInDegrees())
        convertDegreesToRadians(*qStore);


    // Compute generalized speeds
    GCVSplineSet tempQset(5,qStore);
    Storage *uStore = tempQset.constructStorage(1);

    // Compute constraints
    Array<double> qu(0.0,nq+nu);
    rQComplete = new Storage();
    rUComplete = new Storage();
    State constrainedState = s;
     _model->getMultibodySystem().realize(constrainedState, s.getSystemStage());
    for(i=0;i<size;i++) {
        qStore->getTime(i,time);
        qStore->getData(i,nq,&qu[0]);
        uStore->getData(i,nq,&qu[nq]);
        for (int j = 0; j < nq; j++) {
            Coordinate& coord = coordinateSet.get(j);
            coord.setValue(constrainedState, qu[j], false);
//.........这里部分代码省略.........

//.........这里部分代码省略.........
                forceWarning = true;
            }
            continue;
        }
    }

    // Cannot compute system dynamics without mass
    if(hasMass){
        // state derivatives (activation rate and fiber velocity) evaluated at dynamics
        _model->getMultibodySystem().realize(s,SimTK::Stage::Dynamics);

        for(int i=0; i<nm; ++i) {
            try{
                //Velocities
                fibVel[i] = _muscleArray[i]->getFiberVelocity(s);
                normFibVel[i] =  _muscleArray[i]->getNormalizedFiberVelocity(s);
                penAngVel[i] =  _muscleArray[i]->getPennationAngularVelocity(s);
                //Powers
                fibActivePower[i] = _muscleArray[i]->getFiberActivePower(s);
                fibPassivePower[i] = _muscleArray[i]->getFiberPassivePower(s);
                tendonPower[i] = _muscleArray[i]->getTendonPower(s);
                muscPower[i] = _muscleArray[i]->getMusclePower(s);
            }
            catch (const std::exception& e) {
                if(!dynamicsWarning){
                    cout << "WARNING- MuscleAnalysis::record() unable to evaluate ";
                    cout << "muscle forces at time " << s.getTime() << " for reason: ";
                    cout << e.what() << endl;
                    dynamicsWarning = true;
                }
            continue;
            }
        }
    }
    else {
        if(!dynamicsWarning){
            cout << "WARNING- MuscleAnalysis::record() unable to evaluate ";
            cout << "muscle dynamics at time " << s.getTime() << " because ";
            cout << "model has no mass and system dynamics cannot be computed." << endl;
            dynamicsWarning = true;
        }
    }

    // APPEND TO STORAGE
    _pennationAngleStore->append(tReal,penang.getSize(),&penang[0]);
    _lengthStore->append(tReal,len.getSize(),&len[0]);
    _fiberLengthStore->append(tReal,fiblen.getSize(),&fiblen[0]);
    _normalizedFiberLengthStore
        ->append(tReal,normfiblen.getSize(),&normfiblen[0]);
    _tendonLengthStore->append(tReal,tlen.getSize(),&tlen[0]);

    _fiberVelocityStore->append(tReal,fibVel.getSize(),&fibVel[0]);
    _normFiberVelocityStore->append(tReal,normFibVel.getSize(),&normFibVel[0]);
    _pennationAngularVelocityStore
        ->append(tReal,penAngVel.getSize(),&penAngVel[0]);

    _forceStore->append(tReal,force.getSize(),&force[0]);
    _fiberForceStore->append(tReal,fibforce.getSize(),&fibforce[0]);
    _activeFiberForceStore->append(tReal,actfibforce.getSize(),&actfibforce[0]);
    _passiveFiberForceStore
        ->append(tReal,passfibforce.getSize(),&passfibforce[0]);
    _activeFiberForceAlongTendonStore
        ->append(tReal,actfibforcealongten.getSize(),&actfibforcealongten[0]);
    _passiveFiberForceAlongTendonStore
        ->append(tReal,passfibforcealongten.getSize(),&passfibforcealongten[0]);

    _fiberActivePowerStore
        ->append(tReal,fibActivePower.getSize(),&fibActivePower[0]);
    _fiberPassivePowerStore
        ->append(tReal,fibPassivePower.getSize(),&fibPassivePower[0]);
    _tendonPowerStore->append(tReal,tendonPower.getSize(),&tendonPower[0]);
    _musclePowerStore->append(tReal,muscPower.getSize(),&muscPower[0]);

    if (_computeMoments){
        // LOOP OVER ACTIVE MOMENT ARM STORAGE OBJECTS
        Coordinate *q = NULL;
        Storage *maStore=NULL, *mStore=NULL;
        int nq = _momentArmStorageArray.getSize();
        Array<double> ma(0.0,nm),m(0.0,nm);

        for(int i=0; i<nq; i++) {

            q = _momentArmStorageArray[i]->q;
            maStore = _momentArmStorageArray[i]->momentArmStore;
            mStore = _momentArmStorageArray[i]->momentStore;
           
            // bool locked = q->getLocked(s);

            _model->getMultibodySystem().realize(s, s.getSystemStage());
            // LOOP OVER MUSCLES
            for(int j=0; j<nm; j++) {
                ma[j] = _muscleArray[j]->computeMomentArm(s,*q);
                m[j] = ma[j] * force[j];
            }
            maStore->append(s.getTime(),nm,&ma[0]);
            mStore->append(s.getTime(),nm,&m[0]);
        }
    }
    return 0;
}

void FunctionBasedBushingForce::generateDecorations(
        bool                                        fixed, 
        const ModelDisplayHints&                    hints,
        const SimTK::State&                         s,
        SimTK::Array_<SimTK::DecorativeGeometry>&   geometryArray) const
    {
        // invoke parent class method
        Super::generateDecorations(fixed, hints , s, geometryArray); 
        // draw frame1 as red
        SimTK::Vec3 frame1color(1.0, 0.0, 0.0);
        // draw frame2 as blue
        SimTK::Vec3 frame2color(0.0, 0.5, 1.0);
        // the moment on frame2 will be yellow
        SimTK::Vec3 moment2color(1.0, 1.0, 0.0);
        // the force on frame2 will be green
        SimTK::Vec3 force2color(0.0, 1.0, 0.0);

        // create decorative frames to be fixed on frame1 and frame2
        SimTK::DecorativeFrame decorativeFrame1(0.2);
        SimTK::DecorativeFrame decorativeFrame2(0.2);

        // get connected frames
        const PhysicalFrame& frame1 = getFrame1();
        const PhysicalFrame& frame2 = getFrame2();

        // attach frame 1 to body 1, translate and rotate it to the location of the bushing
        decorativeFrame1.setBodyId(frame1.getMobilizedBodyIndex());
        decorativeFrame1.setTransform(frame1.findTransformInBaseFrame());
        decorativeFrame1.setColor(frame1color);

        // attach frame 2 to body 2, translate and rotate it to the location of the bushing
        decorativeFrame2.setBodyId(frame2.getMobilizedBodyIndex());
        decorativeFrame2.setTransform(frame2.findTransformInBaseFrame());
        decorativeFrame2.setColor(frame2color);

        geometryArray.push_back(decorativeFrame1);
        geometryArray.push_back(decorativeFrame2);

        // if the model is moving and the state is adequately realized,
        // calculate and draw the bushing forces.
        if (!fixed && (s.getSystemStage() >= Stage::Dynamics)) {
            SpatialVec F_GM(Vec3(0.0), Vec3(0.0));
            SpatialVec F_GF(Vec3(0.0), Vec3(0.0));

            // total bushing force in the internal basis of the deflection (dq) 
            Vec6 f = calcStiffnessForce(s) + calcDampingForce(s);

            convertInternalForceToForcesOnFrames(s, f, F_GF, F_GM);

            // location of the bushing on frame2
            //SimTK::Vec3 p_b2M_b2 = frame2.findTransformInBaseFrame().p();
            SimTK::Vec3 p_GM_G = frame2.getTransformInGround(s).p();

            // Add moment on frame2 as line vector starting at bushing location
            SimTK::Vec3 scaled_M_GM(get_moment_visual_scale()*F_GM[0]);
            SimTK::Real m_length(scaled_M_GM.norm());
            SimTK::Real m_radius(m_length / get_visual_aspect_ratio() / 2.0);
            SimTK::Transform   X_m2cylinder(SimTK::Rotation(SimTK::UnitVec3(scaled_M_GM), SimTK::YAxis), p_GM_G + scaled_M_GM / 2.0);
            SimTK::DecorativeCylinder frame2Moment(m_radius, m_length / 2.0);
            frame2Moment.setTransform(X_m2cylinder);
            frame2Moment.setColor(moment2color);
            geometryArray.push_back(frame2Moment);

            // Add force on frame2 as line vector starting at bushing location
            SimTK::Vec3 scaled_F_GM(get_force_visual_scale()*F_GM[1]);
            SimTK::Real f_length(scaled_F_GM.norm());
            SimTK::Real f_radius(f_length / get_visual_aspect_ratio() / 2.0);
            SimTK::Transform   X_f2cylinder(SimTK::Rotation(SimTK::UnitVec3(scaled_F_GM), SimTK::YAxis), p_GM_G + scaled_F_GM / 2.0);
            SimTK::DecorativeCylinder frame2Force(f_radius, f_length / 2.0);
            frame2Force.setTransform(X_f2cylinder);
            frame2Force.setColor(force2color);

            geometryArray.push_back(frame2Force);
        }
    }

/**
 * Compute and record the results.
 *
 * This method computes the reaction loads at all joints in the model, then
 * truncates the results to contain only the loads at the requested joints
 * and finally, if necessary, modifies the loads to be acting on the specified
 * body and expressed in the specified frame
 *
 * @param aT Current time in the simulation.
 * @param aX Current values of the controls.
 * @param aY Current values of the states.
 */
int JointReaction::
record(const SimTK::State& s)
{
	/** if a forces file is specified replace the computed actuation with the 
	    forces from storage.*/
	SimTK::State s_analysis = s;

	_model->updMultibodySystem().realize(s_analysis, s.getSystemStage());
	if(_useForceStorage){

		const Set<Actuator> *actuatorSet = &_model->getActuators();
		int nA = actuatorSet->getSize();
		Array<double> forces(0,nA);
		_storeActuation->getDataAtTime(s.getTime(),nA,forces);
		int storageIndex = -1;
		for(int actuatorIndex=0;actuatorIndex<nA;actuatorIndex++)
		{
			//Actuator* act = dynamic_cast<Actuator*>(&_forceSet->get(actuatorIndex));
			std::string actuatorName = actuatorSet->get(actuatorIndex).getName();
			storageIndex = _storeActuation->getStateIndex(actuatorName, 0);
			if(storageIndex == -1){
				cout << "The actuator, " << actuatorName << ", was not found in the forces file." << endl;
				break;
			}
			actuatorSet->get(actuatorIndex).overrideForce(s_analysis,true);
			actuatorSet->get(actuatorIndex).setOverrideForce(s_analysis,forces[storageIndex]);
		}
	}
	// VARIABLES
	int numBodies = _model->getNumBodies();

	/** define 2 variable length vectors of Vec3 vectors to contain calculated  
	*   forces and moments for all the bodies in the model */
	Vector_<Vec3> allForcesVec(numBodies);
	Vector_<Vec3> allMomentsVec(numBodies);
	double Mass = 0.0;

	//// BodySet and JointSet and ground body index
	const BodySet& bodySet = _model->getBodySet();
	const JointSet& jointSet = _model->getJointSet();
	Body &ground = _model->getSimbodyEngine().getGroundBody();
	int groundIndex = bodySet.getIndex(ground.getName());

	/* Calculate All joint reaction forces and moments.
	*  Applied to child bodies, expressed in ground frame.  
	*  computeReactions realizes to the acceleration stage internally
	*  so you don't have to call realize in this analysis.*/ 
	_model->getSimbodyEngine().computeReactions(s_analysis, allForcesVec, allMomentsVec);

	/* retrieved desired joint reactions, convert to desired bodies, and convert
	*  to desired reference frames*/
	int numOutputJoints = _reactionList.getSize();
	Vector_<Vec3> forcesVec(numOutputJoints), momentsVec(numOutputJoints), pointsVec(numOutputJoints);
	for(int i=0; i<numOutputJoints; i++) {
		JointReactionKey currentKey = _reactionList[i];
		const Joint& joint = jointSet.get(currentKey.jointName);
		Vec3 force = allForcesVec[currentKey.reactionIndex];
		Vec3 moment = allMomentsVec[currentKey.reactionIndex];
		Body& expressedInBody = bodySet.get(currentKey.inFrameIndex);
		// find the point of application of the joint load on the child
		Vec3 childLocation(0,0,0);
		joint.getLocation(childLocation);
		// and find it's current location in the ground reference frame
		Vec3 childLocationInGlobal(0,0,0);
		_model->getSimbodyEngine().getPosition(s_analysis, joint.getBody(), childLocation,childLocationInGlobal);
		// set the point of application to the joint location in the child body
		Vec3 pointOfApplication(0,0,0);
		
		// check if the load on the child needs to be converted to an equivalent
		// load on the parent body.
		if(currentKey.onBodyIndex != currentKey.reactionIndex){
			/*Take reaction load from child and apply on parent*/
			force = -force;
			moment = -moment;
			Vec3 parentLocation(0,0,0);
			
			joint.getLocationInParent(parentLocation);
			Vec3 parentLocationInGlobal(0,0,0);
			//_model->getSimbodyEngine().getPosition(s_analysis, joint.getBody(), childLocation,childLocationInGlobal);
			_model->getSimbodyEngine().getPosition(s_analysis,joint.getParentBody(), parentLocation, parentLocationInGlobal);

			// define vector from the mobilizer location on the child to the location on the parent
			Vec3 translation = parentLocationInGlobal - childLocationInGlobal;
			// find equivalent moment if the load is shifted to the parent loaction
			moment -= translation % force;

			// reset the point of application to the joint location in the parent expressed in ground
			pointOfApplication = parentLocationInGlobal;
//.........这里部分代码省略.........