コード例 #1
1
ファイル: CMC.cpp プロジェクト: bit20090138/opensim-core
/**
 * 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);
    
        Array<double> &pErr = _taskSet->getPositionErrors();
        double stressTermWeight = 1;
        for(i=0;i<_taskSet->getSize();i++) {
            if(dynamic_cast<CMC_Joint*>(&_taskSet->get(i))) {
                CMC_Joint& jointTask = dynamic_cast<CMC_Joint&>(_taskSet->get(i));
                if(jointTask.getLimit()) {
                    double w = ForwardTool::SigmaDn(jointTask.getLimit() * relativeTau, jointTask.getLimit(), fabs(pErr[i]));
                    if(_verbose) cout << "Task " << i << ": err=" << pErr[i] << ", limit=" << jointTask.getLimit() << ", sigmoid=" << w << endl;
                    stressTermWeight = min(stressTermWeight, w);
                }
            }
        }
        if(_verbose) cout << "Setting stress term weight to " << stressTermWeight << " (relativeTau was " << relativeTau << ")" << std::endl;
        realTarget->setStressTermWeight(stressTermWeight);

        for(i=0;i<vErr.getSize();i++) err[i] = vErr[i];
        _stressTermWeightStore->append(tiReal,1,&stressTermWeight);
    }

    // SET BOUNDS ON CONTROLS
    int N = _predictor->getNX();
    Array<double> xmin(0.0,N),xmax(1.0,N);
    for(i=0;i<N;i++) {
        Control& x = controlSet.get(i);
        xmin[i] = x.getControlValueMin(tiReal);
        xmax[i] = x.getControlValueMax(tiReal);
    }

    if(_verbose) {
        cout<<"\nxmin:\n"<<xmin<<endl;
        cout<<"\nxmax:\n"<<xmax<<endl;
    }

    // COMPUTE BOUNDS ON MUSCLE FORCES
    Array<double> zero(0.0,N);
    Array<double> fmin(0.0,N),fmax(0.0,N);
    _predictor->setInitialTime(tiReal);
    _predictor->setFinalTime(tfReal);
    _predictor->setTargetForces(&zero[0]);
    _predictor->evaluate(s, &xmin[0], &fmin[0]);
    _predictor->evaluate(s, &xmax[0], &fmax[0]);

    SimTK::State newState = _predictor->getCMCActSubsys()->getCompleteState();
    
     if(_verbose) {
        cout<<endl<<endl;
        cout<<"\ntiReal = "<<tiReal<<"  tfReal = "<<tfReal<<endl;
        cout<<"Min forces:\n";
        cout<<fmin<<endl;
        cout<<"Max forces:\n";
        cout<<fmax<<endl;
    }

    // Print actuator force range if range is small
    double range;
    for(i=0;i<N;i++) {
        range = fmax[i] - fmin[i];
        if(range<1.0) {
            cout << "CMC::computeControls WARNING- small force range for "
                 << getActuatorSet()[i].getName()
                 << " ("<<fmin[i]<<" to "<<fmax[i]<<")\n" << endl;
            // if the force range is so small it means the control value, x, 
            // is inconsequential and we might as well choose the smallest control
            // value possible, or else the RootSolver will choose the last value
            // it used to evaluate the force, which will be the maximum control
            // value. In other words, if the fiber length is so short that no level
            // of activation can produce force, the RootSolver gets the same answer
            // for force if it uses xmin or:: xmax, but since it uses xmax last
            // it returns xmax as the control value. Make xmax = xmin to avoid that.
            xmax[i] = xmin[i];
        }
    }


    // SOLVE STATIC OPTIMIZATION FOR DESIRED ACTUATOR FORCES
    SimTK::Vector lowerBounds(N), upperBounds(N);
    for(i=0;i<N;i++) {
        if(fmin[i]<fmax[i]) {
            lowerBounds[i] = fmin[i];
            upperBounds[i] = fmax[i];
        } else {
            lowerBounds[i] = fmax[i];
            upperBounds[i] = fmin[i];
        }
    }

    _target->setParameterLimits(lowerBounds, upperBounds);

    // OPTIMIZER ERROR TRAP
    _f.setSize(N);

    if(!_target->prepareToOptimize(newState, &_f[0])) {
        // No direct solution, need to run optimizer
        Vector fVector(N,&_f[0],true);

        try {
            _optimizer->optimize(fVector);
        }
        catch (const SimTK::Exception::Base& ex) {
            cout << ex.getMessage() << endl;
            cout << "OPTIMIZATION FAILED..." << endl;
            cout<<endl;

            ostringstream msg;
            msg << "CMC.computeControls: ERROR- Optimizer could not find a solution." << endl;
            msg << "Unable to find a feasible solution at time = " << s.getTime() << "." << endl;
            msg << "Model cannot generate the forces necessary to achieve the target acceleration." << endl;
            msg << "Possible issues: 1. not all model degrees-of-freedom are actuated, " << endl;
            msg << "2. there are tracking tasks for locked coordinates, and/or" << endl;
            msg << "3. there are unnecessary control constraints on reserve/residual actuators." << endl;
                   
            cout<<"\n"<<msg.str()<<endl<<endl;

         throw(new OpenSim::Exception(msg.str(), __FILE__,__LINE__));
        }
    } else {
        // Got a direct solution, don't need to run optimizer
    }

    if(_verbose) _target->printPerformance(&_f[0]);

    if(_verbose) {
        cout<<"\nDesired actuator forces:\n";
        cout<<_f<<endl;
    }


    // ROOT SOLVE FOR EXCITATIONS
    _predictor->setTargetForces(&_f[0]);
    RootSolver rootSolver(_predictor);
    Array<double> tol(4.0e-3,N);
    Array<double> fErrors(0.0,N);
    Array<double> controls(0.0,N);
    controls = rootSolver.solve(s, xmin,xmax,tol);
    if(_verbose) {
       cout<<"\n\nXXX t=" << _tf << "   Controls:" <<controls<<endl;
    }
    
    // FILTER OSCILLATIONS IN CONTROL VALUES
    if(_useCurvatureFilter) FilterControls(s, controlSet,_targetDT,controls,_verbose);

    // SET EXCITATIONS
    controlSet.setControlValues(_tf,&controls[0]);

    _model->updAnalysisSet().setOn(true);
}
コード例 #2
0
/**
 * This method is called at the beginning of an analysis so that any
 * necessary initializations may be performed.
 *
 *
 * This method should be overridden in the child class.  It is
 * included here so that the child class will not have to implement it if it
 * is not necessary.
 *
 * @param state system State
 * @param aStep Step number of the integration.
 *
 * @return -1 on error, 0 otherwise.
 */
int ForceReporter::
begin(SimTK::State& s)
{
    if(!proceed()) return(0);

    tidyForceNames();
    // LABELS
    constructColumnLabels(s);
    // RESET STORAGE
    _forceStore.reset(s.getTime());

    // RECORD
    int status = 0;
    if(_forceStore.getSize()<=0) {
        status = record(s);
    }

    return(status);
}
コード例 #3
0
/**
 * 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 in
 * Model::integBeginCallback() and has the same argument list.
 *
 * @param s SimTK:State
 *
 * @return -1 on error, 0 otherwise.
 */
int InducedAccelerations::begin(SimTK::State &s)
{
	if(!proceed()) return(0);

	initialize(s);

	// RESET STORAGES
	for(int i = 0; i<_storeInducedAccelerations.getSize(); i++){
		_storeInducedAccelerations[i]->reset(s.getTime());
	}

	cout << "Performing Induced Accelerations Analysis" << endl;

	// RECORD
	int status = 0;
	status = record(s);

	return(status);
}
コード例 #4
0
/**
* Record the actuation quantities.
*/
int Actuation::
record(const SimTK::State& s)
{
    if (_model == NULL) return(-1);

    // MAKE SURE ALL ACTUATION QUANTITIES ARE VALID
    _model->getMultibodySystem().realize(s, SimTK::Stage::Dynamics);

    // TIME NORMALIZATION
    double tReal = s.getTime();

    // FORCE
    const Set<Actuator>& fSet = _model->getActuators();
    for (int i = 0, iact = 0; i<fSet.getSize(); i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
        if (!fSet.get(i).get_isDisabled())
            _fsp[iact++] = act->getActuation(s);
    }
    _forceStore->append(tReal, _na, _fsp);

    // SPEED
    for (int i = 0, iact = 0; i<fSet.getSize(); i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
        if (!fSet.get(i).get_isDisabled())
            _fsp[iact++] = act->getSpeed(s);
    }
    _speedStore->append(tReal, _na, _fsp);

    // POWER
    for (int i = 0, iact = 0; i<fSet.getSize(); i++) {
        if (!fSet.get(i).get_isDisabled())
            _fsp[iact++] = fSet.get(i).getPower(s);
    }
    _powerStore->append(tReal, _na, _fsp);


    return(0);
}
コード例 #5
0
/**
 * Record the ForceReporter quantities.
 */
int ForceReporter::record(const SimTK::State& s)
{
    if(_model==NULL) return(-1);

    // MAKE SURE ALL ForceReporter QUANTITIES ARE VALID
    _model->getMultibodySystem().realize(s, SimTK::Stage::Dynamics );

    StateVector nextRow = StateVector(s.getTime());

    // NUMBER OF Forces
    const ForceSet& forces = _model->getForceSet(); // This does not contain gravity
    int nf = forces.getSize();

    for(int i=0;i<nf;i++) {
        // If body force we need to record six values for torque+force
        // If muscle we record one scalar
        OpenSim::Force& nextForce = (OpenSim::Force&)forces[i];
        if (nextForce.isDisabled(s)) continue;
        Array<double> values = nextForce.getRecordValues(s);
        nextRow.getData().append(values);
    }

    if(_includeConstraintForces){
        // NUMBER OF Constraints
        const ConstraintSet& constraints = _model->getConstraintSet(); // This does not contain gravity
        int nc = constraints.getSize();

        for(int i=0;i<nc;i++) {
            OpenSim::Constraint& nextConstraint = (OpenSim::Constraint&)constraints[i];
            if (nextConstraint.isDisabled(s)) continue;
            Array<double> values = nextConstraint.getRecordValues(s);
            nextRow.getData().append(values);
        }
    }
    _forceStore.append(nextRow);

    return(0);
}
コード例 #6
0
/**
 * Record the kinematics.
 */
int PointKinematics::
record(const SimTK::State& s)
{
    const SimbodyEngine& de = _model->getSimbodyEngine();

    // VARIABLES
    SimTK::Vec3 vec;

    const double& time = s.getTime();

    // POSITION
    de.getPosition(s, *_body,_point,vec);
    if(_relativeToBody){
        de.transformPosition(s, _model->getGround(), vec, *_relativeToBody, vec);
    }

    _pStore->append(time, vec);

    // VELOCITY
    de.getVelocity(s, *_body,_point,vec);
    if(_relativeToBody){
        de.transform(s, _model->getGround(), vec, *_relativeToBody, vec);
    }

    _vStore->append(time, vec);

    // ACCELERATIONS
    _model->getMultibodySystem().realize(s, SimTK::Stage::Acceleration);
    de.getAcceleration(s, *_body,_point,vec);
    if(_relativeToBody){
        de.transform(s, _model->getGround(), vec, *_relativeToBody, vec);
    }

    _aStore->append(time, vec);

    return(0);
}
コード例 #7
0
    double getSomething(const SimTK::State& state) const {
        const_cast<Foo *>(this)->m_ctr++;
        m_mutableCtr++;

        return state.getTime();
    }
コード例 #8
0
void testNumberOfMarkersMismatch()
{
    cout << 
        "\ntestInverseKinematicsSolver::testNumberOfMarkersMismatch()"
        << endl;

    std::unique_ptr<Model> pendulum{ constructPendulumWithMarkers() };
    const Coordinate& coord = pendulum->getCoordinateSet()[0];

    SimTK::State state = pendulum->initSystem();
    StatesTrajectory states;

    // sample time
    double dt = 0.1;
    int N = 11;
    for (int i = 0; i < N; ++i) {
        state.updTime() = i*dt;
        coord.setValue(state, i*dt*SimTK::Pi / 3);
        states.append(state);
    }

    double err = 0.05;
    SimTK::RowVector_<SimTK::Vec3> biases(3, SimTK::Vec3(0));
    // bias m0
    biases[0] += SimTK::Vec3(0, err, 0);
    cout << "biases: " << biases << endl;

    auto markerTable = generateMarkerDataFromModelAndStates(*pendulum,
                        states,
                        biases,
                        0.0,
                        true);

    SimTK::Vector_<SimTK::Vec3> unusedCol(N, SimTK::Vec3(0.987654321));

    auto usedMarkerNames = markerTable.getColumnLabels();

    // add an unused marker to the marker data
    markerTable.appendColumn("unused", unusedCol);

    cout << "Before:\n" << markerTable << endl;
    
    // re-order "observed" marker data
    SimTK::Matrix_<SimTK::Vec3> dataGutsCopy = markerTable.getMatrix();
    int last = dataGutsCopy.ncol() - 1;
    // swap first and last columns 
    markerTable.updMatrix()(0) = dataGutsCopy(last);
    markerTable.updMatrix()(last) = dataGutsCopy(0);
    auto columnNames = markerTable.getColumnLabels();
    markerTable.setColumnLabel(0, columnNames[last]);
    markerTable.setColumnLabel(last, columnNames[0]);
    columnNames = markerTable.getColumnLabels();

    // Inject NaN in "observations" of "mR" marker data
    for (int i = 4; i < 7; ++i) {
        markerTable.updMatrix()(i, 1) = SimTK::NaN;
    }

    cout << "After reorder and NaN injections:\n" << markerTable << endl;

    MarkersReference markersRef(markerTable);
    int nmr = markersRef.getNumRefs();
    auto& markerNames = markersRef.getNames();
    cout << markerNames << endl;

    SimTK::Array_<CoordinateReference> coordRefs;
    // Reset the initial coordinate value
    coord.setValue(state, 0.0);
    InverseKinematicsSolver ikSolver(*pendulum, markersRef, coordRefs);
    double tol = 1e-4;
    ikSolver.setAccuracy(tol);
    ikSolver.assemble(state);

    int nm = ikSolver.getNumMarkersInUse();

    SimTK::Array_<double> markerErrors(nm);
    for (unsigned i = 0; i < markersRef.getNumFrames(); ++i) {
        state.updTime() = i*dt;
        ikSolver.track(state);

        //get the marker errors
        ikSolver.computeCurrentMarkerErrors(markerErrors);

        int nme = markerErrors.size();

        SimTK_ASSERT_ALWAYS(nme == nm,
            "InverseKinematicsSolver failed to account "
            "for unused marker reference (observation).");

        cout << "time: " << state.getTime() << " |";
        auto namesIter = usedMarkerNames.begin();
        for (int j = 0; j < nme; ++j) {
            const auto& markerName = ikSolver.getMarkerNameForIndex(j);
            cout << " " << markerName << " error = " << markerErrors[j];

            SimTK_ASSERT_ALWAYS( *namesIter++ != "unused",
                "InverseKinematicsSolver failed to ignore "
                "unused marker reference (observation).");
            
            if (markerName == "m0") {//should see error on biased marker
                SimTK_ASSERT_ALWAYS(abs(markerErrors[j]-err) <= tol,
                    "InverseKinematicsSolver mangled marker order.");
            }
            else { // other markers should be minimally affected
                SimTK_ASSERT_ALWAYS(markerErrors[j] <= tol,
                    "InverseKinematicsSolver mangled marker order.");
            }
        }
        cout << endl;
    }
}
コード例 #9
0
bool State::isConsistent(const SimTK::State& otherState) const {

    if (getNumSubsystems() != otherState.getNumSubsystems())
        return false;

    // State variables.
    if (getNQ() != otherState.getNQ())
        return false;
    if (getNU() != otherState.getNU())
        return false;
    if (getNZ() != otherState.getNZ())
        return false;

    // Constraints.
    if (getNQErr() != otherState.getNQErr())
        return false;
    if (getNUErr() != otherState.getNUErr())
        return false;
    if (getNUDotErr() != otherState.getNUDotErr())
        return false;
    // NMultipliers should be the same as NUDotErr, but we leave this check
    // here in case they diverge in the future.
    if (getNMultipliers() != otherState.getNMultipliers())
        return false;

    // Events.
    if (getNEventTriggers() != otherState.getNEventTriggers())
        return false;

    // Per-subsystem quantities.
    // TODO we could get rid of the total-over-subsystems checks above, but
    // those checks would let us exit earlier.
    for (SimTK::SubsystemIndex isub(0); isub < getNumSubsystems();
            ++isub) {
        if (getNQ(isub) != otherState.getNQ(isub))
            return false;
        if (getNU(isub) != otherState.getNU(isub))
            return false;
        if (getNZ(isub) != otherState.getNZ(isub))
            return false;
        if (getNQErr(isub) != otherState.getNQErr(isub))
            return false;
        if (getNUErr(isub) != otherState.getNUErr(isub))
            return false;
        if (getNUDotErr(isub) != otherState.getNUDotErr(isub))
            return false;
        // NMultipliers should be the same as NUDotErr, but we leave this check
        // here in case they diverge in the future.
        if (getNMultipliers(isub) != otherState.getNMultipliers(isub))
            return false;

        for(SimTK::Stage stage = SimTK::Stage::LowestValid;
                stage <= SimTK::Stage::HighestRuntime; ++stage) {
            if (getNEventTriggersByStage(isub, stage) !=
                    otherState.getNEventTriggersByStage(isub, stage))
                return false;
        }
    }
    return true;
}
コード例 #10
0
/**
 * Record the results.
 */
int StaticOptimization::
record(const SimTK::State& s)
{
    if(!_modelWorkingCopy) return -1;

    // Set model to whatever defaults have been updated to from the last iteration
    SimTK::State& sWorkingCopy = _modelWorkingCopy->updWorkingState();
    sWorkingCopy.setTime(s.getTime());
    _modelWorkingCopy->initStateWithoutRecreatingSystem(sWorkingCopy); 

    // update Q's and U's
    sWorkingCopy.setQ(s.getQ());
    sWorkingCopy.setU(s.getU());

    _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Velocity);
    //_modelWorkingCopy->equilibrateMuscles(sWorkingCopy);

    const Set<Actuator>& fs = _modelWorkingCopy->getActuators();

    int na = fs.getSize();
    int nacc = _accelerationIndices.getSize();

    // IPOPT
    _numericalDerivativeStepSize = 0.0001;
    _optimizerAlgorithm = "ipopt";
    _printLevel = 0;
    //_optimizationConvergenceTolerance = 1e-004;
    //_maxIterations = 2000;

    // Optimization target
    _modelWorkingCopy->setAllControllersEnabled(false);
    StaticOptimizationTarget target(sWorkingCopy,_modelWorkingCopy,na,nacc,_useMusclePhysiology);
    target.setStatesStore(_statesStore);
    target.setStatesSplineSet(_statesSplineSet);
    target.setActivationExponent(_activationExponent);
    target.setDX(_numericalDerivativeStepSize);

    // Pick optimizer algorithm
    SimTK::OptimizerAlgorithm algorithm = SimTK::InteriorPoint;
    //SimTK::OptimizerAlgorithm algorithm = SimTK::CFSQP;

    // Optimizer
    SimTK::Optimizer *optimizer = new SimTK::Optimizer(target, algorithm);

    // Optimizer options
    //cout<<"\nSetting optimizer print level to "<<_printLevel<<".\n";
    optimizer->setDiagnosticsLevel(_printLevel);
    //cout<<"Setting optimizer convergence criterion to "<<_convergenceCriterion<<".\n";
    optimizer->setConvergenceTolerance(_convergenceCriterion);
    //cout<<"Setting optimizer maximum iterations to "<<_maximumIterations<<".\n";
    optimizer->setMaxIterations(_maximumIterations);
    optimizer->useNumericalGradient(false);
    optimizer->useNumericalJacobian(false);
    if(algorithm == SimTK::InteriorPoint) {
        // Some IPOPT-specific settings
        optimizer->setLimitedMemoryHistory(500); // works well for our small systems
        optimizer->setAdvancedBoolOption("warm_start",true);
        optimizer->setAdvancedRealOption("obj_scaling_factor",1);
        optimizer->setAdvancedRealOption("nlp_scaling_max_gradient",1);
    }

    // Parameter bounds
    SimTK::Vector lowerBounds(na), upperBounds(na);
    for(int i=0,j=0;i<fs.getSize();i++) {
        ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fs.get(i));
        if (act) {
            lowerBounds(j) = act->getMinControl();
            upperBounds(j) = act->getMaxControl();
            j++;
        }
    }
    
    target.setParameterLimits(lowerBounds, upperBounds);

    _parameters = 0; // Set initial guess to zeros

    // Static optimization
    _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy,SimTK::Stage::Velocity);
    target.prepareToOptimize(sWorkingCopy, &_parameters[0]);

    //LARGE_INTEGER start;
    //LARGE_INTEGER stop;
    //LARGE_INTEGER frequency;

    //QueryPerformanceFrequency(&frequency);
    //QueryPerformanceCounter(&start);

    try {
        target.setCurrentState( &sWorkingCopy );
        optimizer->optimize(_parameters);
    }
    catch (const SimTK::Exception::Base& ex) {
        cout << ex.getMessage() << endl;
        cout << "OPTIMIZATION FAILED..." << endl;
        cout << endl;
        cout << "StaticOptimization.record:  WARN- The optimizer could not find a solution at time = " << s.getTime() << endl;
        cout << endl;

        double tolBounds = 1e-1;
        bool weakModel = false;
        string msgWeak = "The model appears too weak for static optimization.\nTry increasing the strength and/or range of the following force(s):\n";
        for(int a=0;a<na;a++) {
            Actuator* act = dynamic_cast<Actuator*>(&_forceSet->get(a));
            if( act ) {
                Muscle*  mus = dynamic_cast<Muscle*>(&_forceSet->get(a));
                if(mus==NULL) {
                    if(_parameters(a) < (lowerBounds(a)+tolBounds)) {
                        msgWeak += "   ";
                        msgWeak += act->getName();
                        msgWeak += " approaching lower bound of ";
                        ostringstream oLower;
                        oLower << lowerBounds(a);
                        msgWeak += oLower.str();
                        msgWeak += "\n";
                        weakModel = true;
                    } else if(_parameters(a) > (upperBounds(a)-tolBounds)) {
                        msgWeak += "   ";
                        msgWeak += act->getName();
                        msgWeak += " approaching upper bound of ";
                        ostringstream oUpper;
                        oUpper << upperBounds(a);
                        msgWeak += oUpper.str();
                        msgWeak += "\n";
                        weakModel = true;
                    } 
                } else {
                    if(_parameters(a) > (upperBounds(a)-tolBounds)) {
                        msgWeak += "   ";
                        msgWeak += mus->getName();
                        msgWeak += " approaching upper bound of ";
                        ostringstream o;
                        o << upperBounds(a);
                        msgWeak += o.str();
                        msgWeak += "\n";
                        weakModel = true;
                    }
                }
            }
        }
        if(weakModel) cout << msgWeak << endl;

        if(!weakModel) {
            double tolConstraints = 1e-6;
            bool incompleteModel = false;
            string msgIncomplete = "The model appears unsuitable for static optimization.\nTry appending the model with additional force(s) or locking joint(s) to reduce the following acceleration constraint violation(s):\n";
            SimTK::Vector constraints;
            target.constraintFunc(_parameters,true,constraints);
            const CoordinateSet& coordSet = _modelWorkingCopy->getCoordinateSet();
            for(int acc=0;acc<nacc;acc++) {
                if(fabs(constraints(acc)) > tolConstraints) {
                    const Coordinate& coord = coordSet.get(_accelerationIndices[acc]);
                    msgIncomplete += "   ";
                    msgIncomplete += coord.getName();
                    msgIncomplete += ": constraint violation = ";
                    ostringstream o;
                    o << constraints(acc);
                    msgIncomplete += o.str();
                    msgIncomplete += "\n";
                    incompleteModel = true;
                }
            }
            _forceReporter->step(sWorkingCopy, 1);
            if(incompleteModel) cout << msgIncomplete << endl;
        }
    }

    //QueryPerformanceCounter(&stop);
    //double duration = (double)(stop.QuadPart-start.QuadPart)/(double)frequency.QuadPart;
    //cout << "optimizer time = " << (duration*1.0e3) << " milliseconds" << endl;

    target.printPerformance(sWorkingCopy, &_parameters[0]);

    //update defaults for use in the next step

    const Set<Actuator>& actuators = _modelWorkingCopy->getActuators();
    for(int k=0; k < actuators.getSize(); ++k){
        ActivationFiberLengthMuscle *mus = dynamic_cast<ActivationFiberLengthMuscle*>(&actuators[k]);
        if(mus){
            mus->setDefaultActivation(_parameters[k]);
        }
    }

    _activationStorage->append(sWorkingCopy.getTime(),na,&_parameters[0]);

    SimTK::Vector forces(na);
    target.getActuation(const_cast<SimTK::State&>(sWorkingCopy), _parameters,forces);

    _forceReporter->step(sWorkingCopy, 1);

    return 0;
}
コード例 #11
0
/**
 * 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
    _activationStorage->reset(s.getTime());
    _forceReporter->updForceStorage().reset(s.getTime());

    // RECORD
    int status = 0;
    if(_activationStorage->getSize()<=0) {
        status = record(s);
        const Set<Actuator>& fs = _modelWorkingCopy->getActuators();
        for(int k=0;k<fs.getSize();k++) {
            ScalarActuator* act = dynamic_cast<ScalarActuator *>(&fs[k]);
            if (act){
                cout << "Bounds for " << act->getName() << ": "
                    << act->getMinControl() << " to "
                    << act->getMaxControl() << endl;
            }
            else{
                std::string msg = getConcreteClassName();
                msg += "::can only process scalar Actuator types.";
                throw Exception(msg);
            }
        }
    }

    return(status);
}
コード例 #12
0
/** get the value of the CoordinateReference */
double CoordinateReference::getValue(const SimTK::State &s) const
{
    SimTK::Vector t(1, s.getTime());
    return _coordinateValueFunction->calcValue(t);
}
コード例 #13
0
void compareSimulations(SimTK::MultibodySystem &system, SimTK::State &state, Model *osimModel, SimTK::State &osim_state, string errorMessagePrefix = "")
{
    using namespace SimTK;

    // Set the initial states for both Simbody system and OpenSim model
    Vector& qi = state.updQ();
    Vector& ui = state.updU();
    int nq_sb = initTestStates(qi, ui);
    int nq = osim_state.getNQ();

    // Push down to OpenSim "state"
        if(nq == 2*nq_sb){ //more coordinates because OpenSim model is constrained
            osim_state.updY()[0] = state.getY()[0];
            osim_state.updY()[1] = state.getY()[1];
            osim_state.updY()[nq] = state.getY()[nq_sb];
            osim_state.updY()[nq+1] = state.getY()[nq_sb+1];
        }
        else    
            osim_state.updY() = state.getY();
    

    //==========================================================================================================
    // Integrate Simbody system
    integrateSimbodySystem(system, state);

    // Simbody model final states
    qi = state.updQ();
    ui = state.updU();

    qi.dump("\nSimbody Final q's:");
    ui.dump("\nSimbody Final u's:");

    //==========================================================================================================
    // Integrate OpenSim model
    integrateOpenSimModel(osimModel, osim_state);

    // Get the state at the end of the integration from OpenSim.
    Vector& qf = osim_state.updQ();
    Vector& uf = osim_state.updU();
    cout<<"\nOpenSim Final q's:\n "<<qf<<endl;
    cout<<"\nOpenSim Final u's:\n "<<uf<<endl;

    //==========================================================================================================
    // Compare Simulation Results
    compareSimulationStates(qi, ui, qf, uf, errorMessagePrefix);
}
コード例 #14
0
/** get the values of the CoordinateReference */
void CoordinateReference::getValues(const SimTK::State &s, SimTK::Array_<double> &values) const
{
    SimTK::Vector t(1, s.getTime());
    values.resize(getNumRefs());
    values[0] = _coordinateValueFunction->calcValue(t);
}
コード例 #15
0
/**
 * Record the inverse dynamics forces.
 */
int InverseDynamics::
record(const SimTK::State& s)
{
    if(!_modelWorkingCopy) return -1;

//cout << "\nInverse Dynamics record() : \n" << endl;
    // Set model Q's and U's
    SimTK::State sWorkingCopy = _modelWorkingCopy->getWorkingState();

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

    // Having updated the model, at least re-realize Model stage!
    _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Model);

    sWorkingCopy.setTime(s.getTime());
    sWorkingCopy.setQ(s.getQ());
    sWorkingCopy.setU(s.getU());


    // Having updated the states, at least realize to velocity!
    _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Velocity);

    int nf = _numCoordinateActuators;
    int nacc = _accelerationIndices.getSize();
    // int nq = _modelWorkingCopy->getNumCoordinates();

//cout << "\nQ= " << s.getQ() << endl;
//cout << "\nU= " << s.getU() << endl;
    // Build linear constraint matrix and constant constraint vector
    SimTK::Vector f(nf), c(nacc);
    f = 0;
    computeAcceleration(sWorkingCopy, &f[0], &_constraintVector[0]);

    for(int j=0; j<nf; j++) {
        f[j] = 1;
        computeAcceleration(sWorkingCopy, &f[0], &c[0]);
        for(int i=0; i<nacc; i++) _constraintMatrix(i,j) = (c[i] - _constraintVector[i]);
        f[j] = 0;
    }

    auto coordinates = _modelWorkingCopy->getCoordinatesInMultibodyTreeOrder();

    for(int i=0; i<nacc; i++) {
        const Coordinate& coord = *coordinates[_accelerationIndices[i]];
        int ind = _statesStore->getStateIndex(coord.getSpeedName(), 0);
        if (ind < 0){
            // get the full coordinate speed state variable path name
            string fullname = coord.getStateVariableNames()[1];
            ind = _statesStore->getStateIndex(fullname, 0);
            if (ind < 0){
                string msg = "InverseDynamics::record(): \n";
                msg += "target motion for coordinate '";
                msg += coord.getName() + "' not found.";
                throw Exception(msg);
            }
        }
        Function& targetFunc = _statesSplineSet.get(ind);
        std::vector<int> firstDerivComponents(1);
        firstDerivComponents[0]=0;
        double targetAcceleration = targetFunc.calcDerivative(firstDerivComponents, SimTK::Vector(1, sWorkingCopy.getTime()));
//cout <<  coord.getName() << " t=" << sWorkingCopy.getTime() << "  acc=" << targetAcceleration << " index=" << _accelerationIndices[i] << endl; 
        _constraintVector[i] = targetAcceleration - _constraintVector[i];
    }
    //cout << "NEW Constraint Vector Adjusted = " << endl;
    //_constraintVector.dump(&t);

    // LAPACK SOLVER
    // NOTE: It destroys the matrices/vectors we pass to it, so we need to pass it copies of performanceMatrix and performanceVector (don't bother making
    // copies of _constraintMatrix/Vector since those are reinitialized each time anyway)
    int info;
    SimTK::Matrix performanceMatrixCopy = _performanceMatrix;
    SimTK::Vector performanceVectorCopy = _performanceVector;
//cout << "performanceMatrixCopy : " << performanceMatrixCopy << endl;
//cout << "performanceVectorCopy : " << performanceVectorCopy << endl;
//cout << "_constraintMatrix : " << _constraintMatrix << endl;
//cout << "_constraintVector : " << _constraintVector << endl;
//cout << "nf=" << nf << "  nacc=" << nacc << endl;
    dgglse_(nf, nf, nacc, &performanceMatrixCopy(0,0), nf, &_constraintMatrix(0,0), nacc, &performanceVectorCopy[0], &_constraintVector[0], &f[0], &_lapackWork[0], _lapackWork.size(), info);

    // Record inverse dynamics forces
    _storage->append(sWorkingCopy.getTime(),nf,&f[0]);

//cout << "\n ** f : " << f << endl << endl;

    return 0;
}
コード例 #16
0
/**
 * This method creates a SimmMotionTrial instance with the markerFile and
 * timeRange parameters. It also creates a Storage instance with the
 * coordinateFile parameter. Then it updates the coordinates and markers in
 * the model, if specified. Then it does IK to fit the model to the static
 * pose. Then it uses the current model pose to relocate all non-fixed markers
 * according to their locations in the SimmMotionTrial. Then it writes the
 * output files selected by the user.
 *
 * @param aModel the model to use for the marker placing process.
 * @return Whether the marker placing process was successful or not.
 */
bool MarkerPlacer::processModel(SimTK::State& s, Model* aModel, const string& aPathToSubject)
{
	if(!getApply()) return false;

	cout << endl << "Step 3: Placing markers on model" << endl;

	/* Load the static pose marker file, and average all the
	 * frames in the user-specified time range.
	 */
	MarkerData staticPose(aPathToSubject + _markerFileName);
	if (_timeRange.getSize()<2) 
		throw Exception("MarkerPlacer::processModel, time_range is unspecified.");

	staticPose.averageFrames(_maxMarkerMovement, _timeRange[0], _timeRange[1]);
	staticPose.convertToUnits(aModel->getLengthUnits());

	/* Delete any markers from the model that are not in the static
	 * pose marker file.
	 */
	aModel->deleteUnusedMarkers(staticPose.getMarkerNames());
	
	// Create references and WeightSets needed to initialize InverseKinemaicsSolver
	Set<MarkerWeight> markerWeightSet;
	_ikTaskSet.createMarkerWeightSet(markerWeightSet); // order in tasks file
	MarkersReference markersReference(staticPose, &markerWeightSet);
	SimTK::Array_<CoordinateReference> coordinateReferences;

	// Load the coordinate data
	// create CoordinateReferences for Coordinate Tasks
	FunctionSet *coordFunctions = NULL;
	bool haveCoordinateFile = false;
	if(_coordinateFileName != "" && _coordinateFileName != "Unassigned"){
		Storage coordinateValues(aPathToSubject + _coordinateFileName);
		aModel->getSimbodyEngine().convertDegreesToRadians(coordinateValues);
		haveCoordinateFile = true;
		coordFunctions = new GCVSplineSet(5,&coordinateValues);
	}
	
	int index = 0;
	for(int i=0; i< _ikTaskSet.getSize(); i++){
		if(IKCoordinateTask *coordTask = dynamic_cast<IKCoordinateTask *>(&_ikTaskSet[i])){
			CoordinateReference *coordRef = NULL;
			if(coordTask->getValueType() == IKCoordinateTask::FromFile){
				index = coordFunctions->getIndex(coordTask->getName(), index);
				if(index >= 0){
					coordRef = new CoordinateReference(coordTask->getName(),coordFunctions->get(index));
				}
			}
			else if((coordTask->getValueType() == IKCoordinateTask::ManualValue)){
				Constant reference(Constant(coordTask->getValue()));
				coordRef = new CoordinateReference(coordTask->getName(), reference);
			}
			else{ // assume it should be held at its current/default value
				double value = aModel->getCoordinateSet().get(coordTask->getName()).getValue(s);
				Constant reference = Constant(value);
				coordRef = new CoordinateReference(coordTask->getName(), reference);
			}

			if(coordRef == NULL)
				throw Exception("InverseKinematicsTool: value for coordinate "+coordTask->getName()+" not found.");

			coordinateReferences.push_back(*coordRef);
		
		}			
	}
	double constraintWeight = std::numeric_limits<SimTK::Real>::infinity();

	InverseKinematicsSolver* ikSol = new InverseKinematicsSolver(*aModel, markersReference, coordinateReferences, constraintWeight);
	ikSol->assemble(s);


	// Call realize Position so that the transforms are updated and  markers can be moved correctly
	aModel->getMultibodySystem().realize(s, SimTK::Stage::Position);
	// Report marker errors to assess the quality 
	int nm = markerWeightSet.getSize();
	SimTK::Array_<double> squaredMarkerErrors(nm, 0.0);
	SimTK::Array_<Vec3> markerLocations(nm, Vec3(0));
	double totalSquaredMarkerError = 0.0;
	double maxSquaredMarkerError = 0.0;
	int worst = -1;
	// Report in the same order as the marker tasks/weights
	ikSol->computeCurrentSquaredMarkerErrors(squaredMarkerErrors);
	for(int j=0; j<nm; ++j){
		totalSquaredMarkerError += squaredMarkerErrors[j];
		if(squaredMarkerErrors[j] > maxSquaredMarkerError){
			maxSquaredMarkerError = squaredMarkerErrors[j];
			worst = j;
		}
	}
	cout << "Frame at (t=" << s.getTime() << "):\t";
	cout << "total squared error = " << totalSquaredMarkerError;
	cout << ", marker error: RMS=" << sqrt(totalSquaredMarkerError/nm);
	cout << ", max=" << sqrt(maxSquaredMarkerError) << " (" << ikSol->getMarkerNameForIndex(worst) << ")" << endl;
	/* Now move the non-fixed markers on the model so that they are coincident
	 * with the measured markers in the static pose. The model is already in
	 * the proper configuration so the coordinates do not need to be changed.
	 */
	if(_moveModelMarkers) moveModelMarkersToPose(s, *aModel, staticPose);

	if (_outputStorage!= NULL){
		delete _outputStorage;
	}
	// Make a storage file containing the solved states and markers for display in GUI.
	Storage motionData;
	StatesReporter statesReporter(aModel);
	statesReporter.begin(s);
	
	_outputStorage = new Storage(statesReporter.updStatesStorage());
	_outputStorage->setName("static pose");
	//_outputStorage->print("statesReporterOutput.sto");
	Storage markerStorage;
	staticPose.makeRdStorage(*_outputStorage);
	_outputStorage->getStateVector(0)->setTime(s.getTime());
	statesReporter.updStatesStorage().addToRdStorage(*_outputStorage, s.getTime(), s.getTime());
	//_outputStorage->print("statesReporterOutputWithMarkers.sto");

	if(_printResultFiles) {
		if (!_outputModelFileNameProp.getValueIsDefault())
		{
			aModel->print(aPathToSubject + _outputModelFileName);
			cout << "Wrote model file " << _outputModelFileName << " from model " << aModel->getName() << endl;
		}

		if (!_outputMarkerFileNameProp.getValueIsDefault())
		{
			aModel->writeMarkerFile(aPathToSubject + _outputMarkerFileName);
			cout << "Wrote marker file " << _outputMarkerFileName << " from model " << aModel->getName() << endl;
		}
		
		if (!_outputMotionFileNameProp.getValueIsDefault())
		{
			_outputStorage->print(aPathToSubject + _outputMotionFileName, 
				"w", "File generated from solving marker data for model "+aModel->getName());
		}
	}


	return true;
}
コード例 #17
0
ファイル: CMC.cpp プロジェクト: bit20090138/opensim-core
/**
 * 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;
        computeControls( s, xiSet);
        _model->updAnalysisSet().setOn(false);

        // GET CONTROLS
        xiSet.getControlValues(rTI,xi);

        // OBTAIN EQUILIBRIUM
        if(i<1) {

            obtainActuatorEquilibrium(s,tiReal,0.200,xi,true);
            restoreConfiguration(s, initialState );
        }
    }

    // GET NEW STATES
    _predictor->evaluate(s, &xi[0], &forces[0]);
    
    rTI += newTargetDT;

    // CLEANUP
    setTargetDT(oldTargetDT);
    _model->updAnalysisSet().setOn(true);
    if( _verbose ) {
        cout<<"\n\n=============================================\n";
        cout<<"finish CMC.computeInitialStates: ti="<< rTI << "  q's=" << s.getQ() <<endl;
        cout<<"\nfinish CMC.computeInitialStates: ti="<< rTI << "  u's=" << s.getU() <<endl;
        cout<<"\nfinish CMC.computeInitialStates: ti="<< rTI << "  z's=" << s.getZ() <<endl;
        cout<<"=============================================\n";
    }
}
コード例 #18
0
/**
 * 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;
			_model->getMultibodySystem().getMatterSubsystem().calcAccelerationIgnoringConstraints(s_analysis, appliedMobilityForces, appliedBodyForces, ucUdot, ucA_GB) ;
			ucUdot.dump("Udots Ignoring Constraints");
			ucA_GB.dump("Body Accelerations");

			SimTK::Vector_<SimTK::SpatialVec> constraintBodyForces(_constraintSet.getSize(), SimTK::SpatialVec(SimTK::Vec3(0)));
			SimTK::Vector constraintMobilityForces(0);

			int nc = _model->getMultibodySystem().getMatterSubsystem().getNumConstraints();
			for (SimTK::ConstraintIndex cx(0); cx < nc; ++cx) {
				if (!_model->getMultibodySystem().getMatterSubsystem().isConstraintDisabled(s_analysis, cx)){
					cout << "Constraint " << cx << " enabled!" << endl;
				}
			}
			//int nMults = _model->getMultibodySystem().getMatterSubsystem().getTotalMultAlloc();

			for(int i=0; i<constraintOn.getSize(); i++) {
				if(constraintOn[i])
					_constraintSet[i].calcConstraintForces(s_analysis, constraintBodyForces, constraintMobilityForces);
			}
			constraintBodyForces.dump("Constraint Body Forces");
			constraintMobilityForces.dump("Constraint Mobility Forces");
			// ******************************* end ERROR CHECKING *******************************/
	
			for(int i=0; i<constraintOn.getSize(); i++) {
				_constraintSet.get(i).setDisabled(s_analysis, !constraintOn[i]);
				// Make sure we stay at Dynamics so each constraint can evaluate its conditions
				_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Acceleration);
			}

			// This should also push changes to defaults for unilateral conditions
			_model->setPropertiesFromState(s_analysis);

		}
		else if(_contributors[c] == "gravity"){
			// Set gravity ON
			_model->updForceSubsystem().setForceIsDisabled(s_analysis, _model->getGravityForce().getForceIndex(), false);

			//s_analysis = _model->initSystem();
			s_analysis.setTime(aT);
			s_analysis.setQ(Q);

			// zero velocity
			s_analysis.setU(SimTK::Vector(nu,0.0));
			s_analysis.setZ(s.getZ());

			// disable actuator forces
			for(int f=0; f<_model->getActuators().getSize(); f++){
				_model->updActuators().get(f).setDisabled(s_analysis, true);
			}
		}
		else if(_contributors[c] == "velocity"){		
			// Set gravity off
			_model->updForceSubsystem().setForceIsDisabled(s_analysis, _model->getGravityForce().getForceIndex(), true);

			s_analysis.setTime(aT);
			s_analysis.setQ(Q);

			// non-zero velocity
			s_analysis.setU(s.getU());
			s_analysis.setZ(s.getZ());
			
			// zero actuator forces
			for(int f=0; f<_model->getActuators().getSize(); f++){
				_model->updActuators().get(f).setDisabled(s_analysis, true);
			}
			// Set the configuration (gen. coords and speeds) of the model.
			_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Velocity);
		}
		else{ //The rest are actuators		
			// Set gravity OFF
			_model->updForceSubsystem().setForceIsDisabled(s_analysis, _model->getGravityForce().getForceIndex(), true);

			// zero actuator forces
			for(int f=0; f<_model->getActuators().getSize(); f++){
				_model->updActuators().get(f).setDisabled(s_analysis, true);
			}

			//s_analysis = _model->initSystem();
			s_analysis.setTime(aT);
			s_analysis.setQ(Q);

			// zero velocity
			SimTK::Vector U(nu,0.0);
			s_analysis.setU(U);
			s_analysis.setZ(s.getZ());
			// light up the one actuator who's contribution we are looking for
			int ai = _model->getActuators().getIndex(_contributors[c]);
			if(ai<0)
				throw Exception("InducedAcceleration: ERR- Could not find actuator '"+_contributors[c],__FILE__,__LINE__);
			
			Actuator &actuator = _model->getActuators().get(ai);
			actuator.setDisabled(s_analysis, false);
			actuator.overrideForce(s_analysis, false);
			Muscle *muscle = dynamic_cast<Muscle *>(&actuator);
			if(muscle){
				if(_computePotentialsOnly){
					muscle->overrideForce(s_analysis, true);
					muscle->setOverrideForce(s_analysis, 1.0);
				}
			}

			// Set the configuration (gen. coords and speeds) of the model.
			_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Model);
			_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Velocity);

		}// End of if to select contributor 

		// cout << "Constraint 0 is of "<< _constraintSet[0].getConcreteClassName() << " and should be " << constraintOn[0] << " and is actually " <<  (_constraintSet[0].isDisabled(s_analysis) ? "off" : "on") << endl;
		// cout << "Constraint 1 is of "<< _constraintSet[1].getConcreteClassName() << " and should be " << constraintOn[1] << " and is actually " <<  (_constraintSet[1].isDisabled(s_analysis) ? "off" : "on") << endl;

		// After setting the state of the model and applying forces
		// Compute the derivative of the multibody system (speeds and accelerations)
		_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Acceleration);

		// Sanity check that constraints hasn't totally changed the configuration of the model
		double error = (Q-s_analysis.getQ()).norm();

		// Report reaction forces for debugging
		/*
		SimTK::Vector_<SimTK::SpatialVec> constraintBodyForces(_constraintSet.getSize());
		SimTK::Vector mobilityForces(0);

		for(int i=0; i<constraintOn.getSize(); i++) {
			if(constraintOn[i])
				_constraintSet.get(i).calcConstraintForces(s_analysis, constraintBodyForces, mobilityForces);
		}*/

		// VARIABLES
		SimTK::Vec3 vec,angVec;

		// Get Accelerations for kinematics of bodies
		for(int i=0;i<_coordSet.getSize();i++) {
			double acc = _coordSet.get(i).getAccelerationValue(s_analysis);

			if(getInDegrees()) 
				acc *= SimTK_RADIAN_TO_DEGREE;	
			_coordIndAccs[i]->append(1, &acc);
		}

		// cout << "Input Body Names: "<< _bodyNames << endl;

		// Get Accelerations for kinematics of bodies
		for(int i=0;i<_bodySet.getSize();i++) {
			Body &body = _bodySet.get(i);
			// cout << "Body Name: "<< body->getName() << endl;
			const SimTK::Vec3& com = body.get_mass_center();
			
			// Get the body acceleration
			_model->getSimbodyEngine().getAcceleration(s_analysis, body, com, vec);
			_model->getSimbodyEngine().getAngularAcceleration(s_analysis, body, angVec);	

			// CONVERT TO DEGREES?
			if(getInDegrees()) 
				angVec *= SimTK_RADIAN_TO_DEGREE;	

			// FILL KINEMATICS ARRAY
			_bodyIndAccs[i]->append(3, &vec[0]);
			_bodyIndAccs[i]->append(3, &angVec[0]);
		}

		// Get Accelerations for kinematics of COM
		if(_includeCOM){
			// Get the body acceleration in ground
			vec = _model->getMultibodySystem().getMatterSubsystem().calcSystemMassCenterAccelerationInGround(s_analysis);

			// FILL KINEMATICS ARRAY
			_comIndAccs.append(3, &vec[0]);
		}

		// Get induced constraint reactions for contributor
		if(_reportConstraintReactions){
			for(int j=0; j<_constraintSet.getSize(); j++){
				_constraintReactions.append(_constraintSet[j].getRecordValues(s_analysis));
			}
		}

	} // End cycling through contributors at this time step

	// Set the accelerations of coordinates into their storages
	int nc = _coordSet.getSize();
	for(int i=0; i<nc; i++) {
		_storeInducedAccelerations[i]->append(aT, _coordIndAccs[i]->getSize(),&(_coordIndAccs[i]->get(0)));
	}

	// Set the accelerations of bodies into their storages
	int nb = _bodySet.getSize();
	for(int i=0; i<nb; i++) {
		_storeInducedAccelerations[nc+i]->append(aT, _bodyIndAccs[i]->getSize(),&(_bodyIndAccs[i]->get(0)));
	}

	// Set the accelerations of system center of mass into a storage
	if(_includeCOM){
		_storeInducedAccelerations[nc+nb]->append(aT, _comIndAccs.getSize(), &_comIndAccs[0]);
	}
	if(_reportConstraintReactions){
		_storeConstraintReactions->append(aT, _constraintReactions.getSize(), &_constraintReactions[0]);
	}

	return(0);
}
コード例 #19
0
ファイル: testContext.cpp プロジェクト: ANKELA/opensim-core
int main()
{
  try {
    int  status = 0;

    // To Retrace the steps taken by the GUI this test case follows the same call sequence:
    // new Model(file)
    // new OpenSimContext(model.initSystem(), model);
    // context.updateDisplayer(muscle)  // to display muscles
    // context.getCurrentPath(muscle)
    // context.getTransform(body)
    // context.transformPosition(body, loc, global)  // to display markers
    // context.getLocked(Coordinate)
    // context.getValue(cooridnate)
    LoadOpenSimLibrary("osimActuators");
    LoadOpenSimLibrary("osimSimulation");
    LoadOpenSimLibrary("osimJavaJNI");

    Model *model = new Model("wrist.osim");
    OpenSimContext* context = new OpenSimContext(&model->initSystem(), model);
    const ForceSet& fs = model->getForceSet();
    int n1 = fs.getNumGroups();
    const ObjectGroup* grp = fs.getGroup("wrist");
    assert(grp);
    const Array<const Object*>& members = grp->getMembers();
    int sz = members.getSize();
    ASSERT_EQUAL(sz,5,0);
    assert(members.get(0)->getName()=="ECRB");
    delete model;
    delete context;
    model = new Model("arm26_20.osim");
    context = new OpenSimContext(&model->initSystem(), model);
    // Make a copy of state contained in context ad make sure content match 
    SimTK::State stateCopy = context->getCurrentStateCopy();
    assert(context->getCurrentStateRef().toString()==stateCopy.toString());

    Array<std::string> stateNames = model->getStateVariableNames();
    OpenSim::Force* dForce=&(model->updForceSet().get("TRIlong"));
    Muscle* dTRIlong = dynamic_cast<Muscle*>(dForce);
    assert(dTRIlong);
    context->setPropertiesFromState();
    OpenSim::Thelen2003Muscle* thelenMsl = dynamic_cast<Thelen2003Muscle*>(dTRIlong);
    AbstractProperty& dProp = thelenMsl->updPropertyByName("ignore_tendon_compliance");
    
    PropertyHelper::setValueBool(true, dProp);
    cout << "Prop after is " << dProp.toString() << endl;

    bool exceptionThrown = false;
    try{// adding to the system should cause Muscle that do not handle
        // ignore_tendon_compliance to throw an exception
        context->recreateSystemKeepStage();
    }   
    catch (const std::exception& e) {
        cout << e.what() << endl;
        exceptionThrown = true;
        PropertyHelper::setValueBool(false, dProp);
        cout << "Prop reset to " << dProp.toString() << endl;
        // recreate the system so test can continue
        context->recreateSystemKeepStage();
    }

    SimTK_ASSERT_ALWAYS(exceptionThrown, "Setting ignore_tendon_compliance must throw an exception.");


    AbstractProperty& dProp2 = thelenMsl->updPropertyByName("ignore_tendon_compliance");
    cout << "Prop after create system is " << dProp2.toString() << endl;
    bool after = PropertyHelper::getValueBool(dProp);
    SimTK_ASSERT_ALWAYS(!after, "Property has wrong value!!");
    dTRIlong->updGeometryPath().updateGeometry(context->getCurrentStateRef());
    const OpenSim::Array<PathPoint*>& path = context->getCurrentPath(*dTRIlong);
    cout << "Muscle Path" << endl;
    cout << path.getSize() << endl;
    for(int i=0; i< path.getSize(); i++)
        cout << path.get(i)->getBodyName() << path.get(i)->getLocation() << endl;
    // Compare to known path 
    const OpenSim::Body& dBody = model->getBodySet().get("r_ulna_radius_hand");
    Transform xform = context->getTransform(dBody);
    cout << xform << endl;
    double flat[16];
    context->getTransformAsDouble16(xform, flat);
    // Compare to known xform
    double markerPosition[] = {.005000000000, -0.290400000000, 0.030000000000};
    double markerPositionInGround[3];
    context->transformPosition(dBody, markerPosition, markerPositionInGround);  // to display markers
    cout << "Global frame position = " << markerPositionInGround[0] <<  
        markerPositionInGround[1] << markerPositionInGround[2]<< endl;
    // Check xformed point against known position
    const Coordinate& dr_elbow_flex = model->getCoordinateSet().get("r_elbow_flex");
    bool isLocked = context->getLocked(dr_elbow_flex);
    assert(!isLocked);
    double startValue = context->getValue(dr_elbow_flex);
    cout << "Coordinate start value = " << startValue << endl;
    double length1 = context->getMuscleLength(*dTRIlong);
    cout << length1 << endl;
    ASSERT_EQUAL(.277609, length1, 1e-5);
    // Coordinate Slider
    context->setValue(dr_elbow_flex, 100*SimTK_PI/180.);
    // Get body transform, marker position and muscle path (tests wrapping as well)
    xform = context->getTransform(dBody);
    cout << "After setting coordinate to 100 deg." << endl;
    cout << xform << endl;
    // Compare to known xform
    dTRIlong->updGeometryPath().updateGeometry(context->getCurrentStateRef());
    const OpenSim::Array<PathPoint*>& newPath = context->getCurrentPath(*dTRIlong);
    // Compare to known path 
    cout << "New Muscle Path" << endl;
    cout << path.getSize() << endl;
    for(int i=0; i< path.getSize(); i++)
        cout << path.get(i)->getBodyName() << path.get(i)->getLocation() << endl;
    double length2 = context->getMuscleLength(*dTRIlong);
    cout << length2 << endl;
    ASSERT_EQUAL(.315748, length2, 1e-5);
    // Test that we can lock coordinates to specific value and make this persistant.
    Coordinate& dr_elbow_flex_mod = model->updCoordinateSet().get("r_elbow_flex");
    //dr_elbow_flex_mod.setDefaultValue(0.5);
    dr_elbow_flex_mod.setDefaultLocked(true);
    context->setValue(dr_elbow_flex_mod, 0.5);
    //model->print("wrist_locked_elbow.osim");
    context->recreateSystemKeepStage();
    const Coordinate& dr_elbow_flexNew = model->getCoordinateSet().get("r_elbow_flex");
    assert(context->getLocked(dr_elbow_flexNew));
    ASSERT_EQUAL(0.5, context->getValue(dr_elbow_flexNew), 0.000001);

    return status;
  } catch (const std::exception& e) {
      cout << "Exception: " << e.what() << endl;
      return 1;
  }
}
コード例 #20
0
/**
 * Record the kinematics.
 */
int BodyKinematics::
record(const SimTK::State& s)
{

    // Realize to Acceleration first since we'll ask for Accelerations 
    _model->getMultibodySystem().realize(s, SimTK::Stage::Acceleration);
    // VARIABLES
    double dirCos[3][3];
    SimTK::Vec3 vec,angVec;
    double Mass = 0.0;

    // GROUND BODY
    const Ground &ground = _model->getGround();

    // POSITION
    BodySet& bs = _model->updBodySet();

    for(int i=0;i<_bodyIndices.getSize();i++) {
        Body& body = bs.get(_bodyIndices[i]);
        const SimTK::Vec3& com = body.get_mass_center();
        // GET POSITIONS AND EULER ANGLES
        _model->getSimbodyEngine().getPosition(s, body,com,vec);
        _model->getSimbodyEngine().getDirectionCosines(s, body,dirCos);
        _model->getSimbodyEngine().convertDirectionCosinesToAngles(dirCos,
            &angVec[0],&angVec[1],&angVec[2]);

        // CONVERT TO DEGREES?
        if(getInDegrees()) {
            angVec[0] *= SimTK_RADIAN_TO_DEGREE;
            angVec[1] *= SimTK_RADIAN_TO_DEGREE;
            angVec[2] *= SimTK_RADIAN_TO_DEGREE;
        }           

        // FILL KINEMATICS ARRAY
        int I=6*i;
        memcpy(&_kin[I],&vec[0],3*sizeof(double));
        memcpy(&_kin[I+3],&angVec[0],3*sizeof(double));
    }

    if(_recordCenterOfMass) {
        double rP[3] = { 0.0, 0.0, 0.0 };
        for(int i=0;i<bs.getSize();i++) {
            Body& body = bs.get(i);
            const SimTK::Vec3& com = body.get_mass_center();
            _model->getSimbodyEngine().getPosition(s, body,com,vec);
            // ADD TO WHOLE BODY MASS
            Mass += body.get_mass();
            rP[0] += body.get_mass() * vec[0];
            rP[1] += body.get_mass() * vec[1];
            rP[2] += body.get_mass() * vec[2];
        }

        //COMPUTE COM OF WHOLE BODY AND ADD TO ARRAY
        rP[0] /= Mass;
        rP[1] /= Mass;
        rP[2] /= Mass;
        int I = 6*_bodyIndices.getSize();
        memcpy(&_kin[I],rP,3*sizeof(double));
    }
    
    _pStore->append(s.getTime(),_kin.getSize(),&_kin[0]);

    // VELOCITY
    for(int i=0;i<_bodyIndices.getSize();i++) {
        Body& body = bs.get(_bodyIndices[i]);
        const SimTK::Vec3& com = body.get_mass_center();
        // GET VELOCITIES AND ANGULAR VELOCITIES
        _model->getSimbodyEngine().getVelocity(s, body,com,vec);
        if(_expressInLocalFrame) {
            _model->getSimbodyEngine().transform(s, ground,vec,body,vec);
            _model->getSimbodyEngine().getAngularVelocityBodyLocal(s, body,angVec);
        } else {
            _model->getSimbodyEngine().getAngularVelocity(s, body,angVec);
        }

        // CONVERT TO DEGREES?
        if(getInDegrees()) {
            angVec[0] *= SimTK_RADIAN_TO_DEGREE;
            angVec[1] *= SimTK_RADIAN_TO_DEGREE;
            angVec[2] *= SimTK_RADIAN_TO_DEGREE;
        }           

        // FILL KINEMATICS ARRAY
        int I = 6*i;
        memcpy(&_kin[I],&vec[0],3*sizeof(double));
        memcpy(&_kin[I+3],&angVec[0],3*sizeof(double));
    }

    if(_recordCenterOfMass) {
        double rV[3] = { 0.0, 0.0, 0.0 };
        for(int i=0;i<bs.getSize();i++) {
            Body& body = bs.get(i);
            const SimTK::Vec3& com = body.get_mass_center();
            _model->getSimbodyEngine().getVelocity(s, body,com,vec);
            rV[0] += body.get_mass() * vec[0];
            rV[1] += body.get_mass() * vec[1];
            rV[2] += body.get_mass() * vec[2];
        }

        //COMPUTE VELOCITY OF COM OF WHOLE BODY AND ADD TO ARRAY
        rV[0] /= Mass;
        rV[1] /= Mass;
        rV[2] /= Mass;
        int I = 6*_bodyIndices.getSize();
        memcpy(&_kin[I],rV,3*sizeof(double));
    }

    _vStore->append(s.getTime(),_kin.getSize(),&_kin[0]);

    // ACCELERATIONS
    for(int i=0;i<_bodyIndices.getSize();i++) {
        Body& body = bs.get(_bodyIndices[i]);
        const SimTK::Vec3& com = body.get_mass_center();

        // GET ACCELERATIONS AND ANGULAR ACCELERATIONS
        _model->getSimbodyEngine().getAcceleration(s, body,com,vec);
        if(_expressInLocalFrame) {
            _model->getSimbodyEngine().transform(s, ground,vec,body,vec);
            _model->getSimbodyEngine().getAngularAccelerationBodyLocal(s, body,angVec);
        } else {
            _model->getSimbodyEngine().getAngularAcceleration(s, body,angVec);
        }

        // CONVERT TO DEGREES?
        if(getInDegrees()) {
            angVec[0] *= SimTK_RADIAN_TO_DEGREE;
            angVec[1] *= SimTK_RADIAN_TO_DEGREE;
            angVec[2] *= SimTK_RADIAN_TO_DEGREE;
        }           

        // FILL KINEMATICS ARRAY
        int I = 6*i;
        memcpy(&_kin[I],&vec[0],3*sizeof(double));
        memcpy(&_kin[I+3],&angVec[0],3*sizeof(double));
    }

    if(_recordCenterOfMass) {
        double rA[3] = { 0.0, 0.0, 0.0 };
        for(int i=0;i<bs.getSize();i++) {
            Body& body = bs.get(i);
            const SimTK::Vec3& com = body.get_mass_center();
            _model->getSimbodyEngine().getAcceleration(s, body,com,vec);
            rA[0] += body.get_mass() * vec[0];
            rA[1] += body.get_mass() * vec[1];
            rA[2] += body.get_mass() * vec[2];
        }

        //COMPUTE ACCELERATION OF COM OF WHOLE BODY AND ADD TO ARRAY
        rA[0] /= Mass;
        rA[1] /= Mass;
        rA[2] /= Mass;
        int I = 6*_bodyIndices.getSize();
        memcpy(&_kin[I],rA,3*sizeof(double));
    }

    _aStore->append(s.getTime(),_kin.getSize(),&_kin[0]);

    //printf("BodyKinematics:\taT:\t%.16f\trA[1]:\t%.16f\n",s.getTime(),rA[1]);
    return(0);
}
コード例 #21
0
 SimTK::Vector getQ(const SimTK::State& state) const {
     return state.getQ();
 }
コード例 #22
0
void testNumberOfOrientationsMismatch()
{
    cout <<
        "\ntestInverseKinematicsSolver::testNumberOfOrientationsMismatch()"
        << endl;

    std::unique_ptr<Model> leg{ constructLegWithOrientationFrames() };
    const Coordinate& coord = leg->getCoordinateSet()[0];

    SimTK::State state = leg->initSystem();
    StatesTrajectory states;

    // sample time
    double dt = 0.1;
    int N = 11;
    for (int i = 0; i < N; ++i) {
        state.updTime() = i*dt;
        coord.setValue(state, i*dt*SimTK::Pi / 3);
        states.append(state);
    }

    double err = 0.1;
    SimTK::RowVector_<SimTK::Rotation> biases(3, SimTK::Rotation());
    // bias thigh_imu
    biases[0] *= SimTK::Rotation(err, SimTK::XAxis);
    cout << "biases: " << biases << endl;

    auto orientationsTable =
            generateOrientationsDataFromModelAndStates(*leg,
                states,
                biases,
                0.0,
                true);

    SimTK::Vector_<SimTK::Rotation> unusedCol(N,
        SimTK::Rotation(0.987654321, SimTK::ZAxis));

    auto usedOrientationNames = orientationsTable.getColumnLabels();

    // add an unused orientation sensor to the given orientation data
    orientationsTable.appendColumn("unused", unusedCol);

    cout << "Before:\n" << orientationsTable << endl;

    // re-order "observed" orientation data
    SimTK::Matrix_<SimTK::Rotation> dataGutsCopy
        = orientationsTable.getMatrix();
    int last = dataGutsCopy.ncol() - 1;
    // swap first and last columns 
    orientationsTable.updMatrix()(0) = dataGutsCopy(last);
    orientationsTable.updMatrix()(last) = dataGutsCopy(0);
    auto columnNames = orientationsTable.getColumnLabels();
    orientationsTable.setColumnLabel(0, columnNames[last]);
    orientationsTable.setColumnLabel(last, columnNames[0]);
    columnNames = orientationsTable.getColumnLabels();

    // Inject NaN in "observations" of thigh_imu orientation data
    for (int i = 4; i < 7; ++i) {
        orientationsTable.updMatrix()(i, 1).scalarMultiply(SimTK::NaN);
    }

    cout << "After reorder and NaN injections:\n" << orientationsTable << endl;

    OrientationsReference orientationsRef(orientationsTable);
    int nmr = orientationsRef.getNumRefs();
    auto& osNames = orientationsRef.getNames();
    cout << osNames << endl;

    MarkersReference mRefs{};
    SimTK::Array_<CoordinateReference> coordRefs;
    // Reset the initial coordinate value
    coord.setValue(state, 0.0);
    InverseKinematicsSolver ikSolver(*leg, mRefs, orientationsRef, coordRefs);
    double tol = 1e-4;
    ikSolver.setAccuracy(tol);
    ikSolver.assemble(state);

    int nos = ikSolver.getNumOrientationSensorsInUse();

    SimTK::Array_<double> orientationErrors(nos);
    for (double t : orientationsRef.getTimes()) {
        state.updTime() = t;
        ikSolver.track(state);

        //get the  orientation errors
        ikSolver.computeCurrentOrientationErrors(orientationErrors);
        int nose = orientationErrors.size();

        SimTK_ASSERT_ALWAYS(nose == nos,
            "InverseKinematicsSolver failed to account "
            "for unused orientations reference (observation).");

        cout << "time: " << state.getTime() << " |";
        auto namesIter = usedOrientationNames.begin();
        for (int j = 0; j < nose; ++j) {
            const auto& orientationName = 
                ikSolver.getOrientationSensorNameForIndex(j);

            cout << " " << orientationName << " error = " << orientationErrors[j];

            SimTK_ASSERT_ALWAYS(*namesIter++ != "unused",
                "InverseKinematicsSolver failed to ignore "
                "unused orientation reference (observation).");

            if (orientationName == "thigh_imu") {//should see error on biased marker
                SimTK_ASSERT_ALWAYS(abs(orientationErrors[j]) <= err,
                    "InverseKinematicsSolver mangled marker order.");
            }
            else { // other markers should be minimally affected
                SimTK_ASSERT_ALWAYS(orientationErrors[j] <= tol,
                    "InverseKinematicsSolver mangled marker order.");
            }
        }
        cout << endl;
    }
}
コード例 #23
0
 void computeStateVariableDerivatives(const SimTK::State& s) const override {
     double deriv = exp(-2.0*s.getTime());
     setStateVariableDerivativeValue(s, "subState", deriv);
 }
コード例 #24
0
ファイル: Manager.cpp プロジェクト: cpizzolato/opensim-core
bool Manager::doIntegration(SimTK::State& s, int step, double dtFirst ) {

    if(!_integ) {
        throw Exception("Manager::doIntegration(): "
                "Integrator has not been set. Construct the Manager "
                "with an integrator, or call Manager::setIntegrator().");
    }

    // CLEAR ANY INTERRUPT
    // Halts must arrive during an integration.
    clearHalt();

    double dt/*,dtPrev*/,tReal;
    double time =_ti;
    dt=dtFirst;
    if(dt>_dtMax) dt = _dtMax;
    //dtPrev=dt;

    // CHECK SPECIFIED DT STEPPING
    
    if(_specifiedDT) {
        if(_tArray.getSize()<=0) {
            string msg="IntegRKF.integrate: ERR- specified dt stepping not";
            msg += "possible-- empty time array.";
            throw( Exception(msg) );
        }
        double first = _tArray[0];
        double last = _tArray.getLast();
        if((getTimeArrayStep(_ti)<0) || (_ti<first) || (_tf>last)) {
            string msg="IntegRKF.integrate: ERR- specified dt stepping not";
            msg +="possible-- time array does not cover the requested";
            msg +=" integration interval.";
            throw(Exception(msg));
        }
    }

    // RECORD FIRST TIME STEP
    if(!_specifiedDT) {
        resetTimeAndDTArrays(time);
        if(_tArray.getSize()<=0) {
            _tArray.append(time);
        }
    }
    bool fixedStep = false;
    double fixedStepSize;
    if( _constantDT || _specifiedDT) fixedStep = true;

    // If _system is has been set we should be integrating a CMC system
    // not the model's system.
    const SimTK::System& sys = _system ? *_system 
                                       : _model->getMultibodySystem();

    // Only initialize a TimeStepper if it hasn't been done yet
    if (_timeStepper == NULL) initializeTimeStepper(sys, s);

    SimTK::Integrator::SuccessfulStepStatus status;

    if( fixedStep ) {
        dt = getFixedStepSize(getTimeArrayStep(_ti));
    } else {
        _integ->setReturnEveryInternalStep(true); 
    }

    if( s.getTime()+dt >= _tf ) dt = _tf - s.getTime();
   
    // We need to be at a valid stage to initialize the controls, but only when 
    // we are integrating the complete model system, not the CMC system. This 
    // is very ugly and a cleaner solution is required- aseth
    if(_system == NULL)
        sys.realize(s, SimTK::Stage::Velocity); // this is multibody system 
    initialize(s, dt);  

    if( fixedStep){
        s.updTime() = time;
        sys.realize(s, SimTK::Stage::Acceleration);

        if(_performAnalyses)_model->updAnalysisSet().step(s, step);
        tReal = s.getTime();
        if( _writeToStorage ) {
            SimTK::Vector stateValues = _model->getStateVariableValues(s);
            StateVector vec;
            vec.setStates(tReal, stateValues);
            getStateStorage().append(vec);
            if(_model->isControlled())
                _controllerSet->storeControls(s,step);
        }
    }

    double stepToTime = _tf;

    // LOOP
    while( time  < _tf ) {
        if( fixedStep ){
              fixedStepSize = getNextTimeArrayTime( time ) - time;
             if( fixedStepSize + time  >= _tf )  fixedStepSize = _tf - time;
             _integ->setFixedStepSize( fixedStepSize );
             stepToTime = time + fixedStepSize; 
        }

        // stepTo() does not return if it fails. However, the final step
        // is returned once as an ordinary return; by the time we get
        // EndOfSimulation status we have already seen the state and don't
        // need to record it again.
        status = _timeStepper->stepTo(stepToTime);

        if( status != SimTK::Integrator::EndOfSimulation ) {
            const SimTK::State& s =  _integ->getState();
            if(_performAnalyses)_model->updAnalysisSet().step(s,step);
            tReal = s.getTime();
            if( _writeToStorage) {
                SimTK::Vector stateValues = _model->getStateVariableValues(s);
                StateVector vec;
                vec.setStates(tReal, stateValues);
                getStateStorage().append(vec);
                if(_model->isControlled())
                    _controllerSet->storeControls(s, step);
            }
            step++;
        }
        else
            halt();
        
        time = _integ->getState().getTime();
        // CHECK FOR INTERRUPT
        if(checkHalt()) break;
    }
    finalize(_integ->updAdvancedState() );
    s = _integ->getState();

    // CLEAR ANY INTERRUPT
    clearHalt();

    return true;
}
コード例 #25
0
/** get the acceleration value of the CoordinateReference */
double CoordinateReference::getAccelerationValue(const SimTK::State &s) const
{
    SimTK::Vector t(1, s.getTime());
    std::vector<int> order(2, 0);
    return _coordinateValueFunction->calcDerivative(order, t);
}
コード例 #26
0
ファイル: IKSolverParallel.cpp プロジェクト: modenaxe/rtosim
    void IKSolverParallel::operator()(){

        SimTK::State s = model_.initSystem();
        bool localRunCondition(true);
        std::vector<double> sortedMarkerWeights;
        for (auto it : markerNames_)
            sortedMarkerWeights.push_back(markerWeights_[it]);
        //I may need to use a raw pointer because of OpenSim..
        unique_ptr<MarkersReferenceFromQueue> markerReference(new MarkersReferenceFromQueue(inputThreadPoolJobs_, markerNames_, sortedMarkerWeights));

        OpenSim::Set<OpenSim::MarkerWeight> osimMarkerWeights;
        for (auto it : markerNames_)
            osimMarkerWeights.adoptAndAppend(new OpenSim::MarkerWeight(it, markerWeights_[it]));
        markerReference->setMarkerWeightSet(osimMarkerWeights);
        SimTK::Array_<OpenSim::CoordinateReference> coordinateRefs;

        double previousTime(0.);
        double currentTime(0.);
        OpenSim::InverseKinematicsSolver ikSolver(model_, *markerReference, coordinateRefs, contraintWeight_);
        ikSolver.setAccuracy(sovlerAccuracy_);
        doneWithSubscriptions_.wait();
        bool isAssembled(false);
        while (!isAssembled) {
            try {

                ikSolver.assemble(s);
                isAssembled = true;
            }
            catch (...){
                std::cerr << "Time " << s.getTime() << " Model not assembled" << std::endl;
                markerReference->purgeCurrentFrame();
                isAssembled = false;
            }
        }

        SimTK::State defaultState(s);
        currentTime = markerReference->getCurrentTime();
        s.updTime() = currentTime;
        previousTime = currentTime;
        pushState(s);
        unsigned ct = 0;
        //     auto start = std::chrono::system_clock::now();
        //init the stats, so we can start measuring the frame processing time correctly

        while (localRunCondition) {
            if (!markerReference->isEndOfData()){
                try{
                    stopWatch_.init();
                    ikSolver.track(s);
                    stopWatch_.log();
                    if (!isWithinRom(s))
                        s = defaultState;
                }
                catch (...) {
                    s = defaultState;
                }
                SimTK::Array_<double> markerErrors;
                ikSolver.computeCurrentMarkerErrors(markerErrors);
                currentTime = markerReference->getCurrentTime();
                s.updTime() = currentTime;
                previousTime = currentTime;
                pushState(s);
                defaultState = s;
                ++ct;
            }
            else {
                localRunCondition = false;
                outputGeneralisedCoordinatesQueue_.push(rtosim::EndOfData::get<GeneralisedCoordinatesFrame>());
            }
        }
#ifdef RTOSIM_DEBUG
        cout << " IKSolver " << std::this_thread::get_id() << " is closing\n";
#endif
        doneWithExecution_.wait();
    }
コード例 #27
0
/**
 * 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);
            coord.setSpeedValue(constrainedState, qu[nq+j]);
        }
        _model->assemble(constrainedState);
        for (int j = 0; j < nq; j++) {
            Coordinate& coord = coordinateSet.get(j);
            qu[j] = coord.getValue(constrainedState);
            qu[nq+j] = coord.getSpeedValue(constrainedState);
        }
        rQComplete->append(time,nq,&qu[0]);
        rUComplete->append(time,nu,&qu[nq]);
    }
    
    delete qStore;
    
    // Compute storage object for simulation
    // Need to set column labels before converting rad->deg
    rQComplete->setColumnLabels(columnLabels);
    rUComplete->setColumnLabels(speedLabels);
    // Convert back to degrees
    convertRadiansToDegrees(*rQComplete);
    convertRadiansToDegrees(*rUComplete);
}
コード例 #28
0
ファイル: CMC.cpp プロジェクト: bit20090138/opensim-core
 void handleEvent (SimTK::State& s, Real accuracy, bool& terminate) const override {
     terminate = false;
     _controller->computeControls( s, _controller->updControlSet() );
     _controller->setTargetTime(s.getTime() + _controller->getTargetDT());
 }
コード例 #29
0
/**
 * This method is called at the beginning of an analysis so that any
 * necessary initializations may be performed.
 *
 * This method should be overridden in the child class.  It is
 * included here so that the child class will not have to implement it if it
 * is not necessary.
 *
 * @param s state of system
 *
 * @return -1 on error, 0 otherwise.
 */
int InverseDynamics::begin(const SimTK::State& s )
{
    if(!proceed()) return(0);

    SimTK::Matrix massMatrix;
    _model->getMatterSubsystem().calcM(s, massMatrix);

    //massMatrix.dump("mass matrix is:");
    // Check that m is full rank
    try {
        SimTK::FactorLU lu(massMatrix);
    } catch (const SimTK::Exception::Base&) {
        throw Exception("InverseDynamics: ERROR- mass matrix is singular  ",__FILE__,__LINE__);
    }
    // enable the line below when simmath is fixed
    //bool singularMassMatrix = lu.isSingular();

    //cout << "Mass matrix is " << (singularMassMatrix?"singular":"Non-singular");

    // Make a working copy of the model
    delete _modelWorkingCopy;
    _modelWorkingCopy = _model->clone();
    _modelWorkingCopy->updAnalysisSet().setSize(0);

    // Replace model force set with only generalized forces
    if(_model) {
        SimTK::State& sWorkingCopyTemp = _modelWorkingCopy->initSystem();
        // 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();
            _numCoordinateActuators = _modelWorkingCopy->getActuators().getSize();
        } 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);
            _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());
            }
        }
        _modelWorkingCopy->setAllControllersEnabled(false);
        _ownsForceSet = false;

        SimTK::State& sWorkingCopy = _modelWorkingCopy->initSystem();

        // Gather indices into speed set corresponding to the unconstrained degrees of freedom (for which we will set acceleration constraints)
        _accelerationIndices.setSize(0);
        auto coordinates = _modelWorkingCopy->getCoordinatesInMultibodyTreeOrder();
        for(size_t i=0u; i<coordinates.size(); ++i) {
            const Coordinate& coord = *coordinates[i];
            if(!coord.isConstrained(sWorkingCopy)) {
                _accelerationIndices.append(static_cast<int>(i));
            }
        }

        _dydt.setSize(_modelWorkingCopy->getNumCoordinates() + _modelWorkingCopy->getNumSpeeds());

        int nf = _numCoordinateActuators;
        int nacc = _accelerationIndices.getSize();

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

        // Realize to velocity in case there are any velocity dependent forces
        _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Velocity);

        _constraintMatrix.resize(nacc,nf);
        _constraintVector.resize(nacc);

        _performanceMatrix.resize(nf,nf);
        _performanceMatrix = 0;
        for(int i=0,j=0; i<nf; i++) {
            ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->get(i));
            if( act ) {
                act->setActuation(sWorkingCopy, 1);
                _performanceMatrix(j,j) = act->getStress(sWorkingCopy);
                j++;
             }
        }

        _performanceVector.resize(nf);
        _performanceVector = 0;

        int lwork = nf + nf + nacc;
        _lapackWork.resize(lwork);
    }

    _statesSplineSet=GCVSplineSet(5,_statesStore);

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

    deleteStorage();
    allocateStorage();

    // RESET STORAGE
    _storage->reset(s.getTime());

    // RECORD
    int status = 0;
    if(_storage->getSize()<=0) {
        status = record(s);
    }

    return(status);
}
コード例 #30
0
/**
 * Record the MuscleAnalysis quantities.
 */
int MuscleAnalysis::record(const SimTK::State& s)
{
    if(_model==NULL) return -1;
    if (!getOn()) return -1;

    // MAKE SURE ALL ACTUATION QUANTITIES ARE VALID
    // COMPUTE DERIVATIVES
    // ----------------------------------
    // TIME NORMALIZATION
    double tReal = s.getTime();
    // ----------------------------------
    // LOOP THROUGH MUSCLES
    int nm = _muscleArray.getSize();

    double nan = SimTK::NaN;
    // Angles and lengths
    Array<double> penang(nan,nm);
    Array<double> len(nan,nm), tlen(nan,nm);
    Array<double> fiblen(nan,nm), normfiblen(nan,nm);

    // Muscle velocity information
    Array<double> fibVel(nan,nm), normFibVel(nan,nm);
    Array<double> penAngVel(nan,nm);

    // Muscle component forces
    Array<double> force(nan, nm), fibforce(nan, nm);
    Array<double> actfibforce(nan,nm), passfibforce(nan,nm);
    Array<double> actfibforcealongten(nan,nm), passfibforcealongten(nan,nm);

    // Muscle and component powers
    Array<double> fibActivePower(nan,nm), fibPassivePower(nan,nm),
                  tendonPower(nan,nm), muscPower(nan,nm);

    double sysMass = _model->getMatterSubsystem().calcSystemMass(s);
    bool hasMass = sysMass > SimTK::Eps;

    // Just warn once per instant
    bool lengthWarning = false;
    bool forceWarning = false;
    bool dynamicsWarning = false;

    for(int i=0; i<nm; ++i) {
        try{
            len[i] = _muscleArray[i]->getLength(s);
            tlen[i] = _muscleArray[i]->getTendonLength(s);
            fiblen[i] = _muscleArray[i]->getFiberLength(s);
            normfiblen[i] = _muscleArray[i]->getNormalizedFiberLength(s);
            penang[i] = _muscleArray[i]->getPennationAngle(s);
        }
        catch (const std::exception& e) {
            if(!lengthWarning){
                cout << "WARNING- MuscleAnalysis::record() unable to evaluate ";
                cout << "muscle length at time " << s.getTime() << " for reason: ";
                cout << e.what() << endl;
                lengthWarning = true;
            }
            continue;
        }

        try{
            // Compute muscle forces that are dependent on Positions, Velocities
            // so that later quantities are valid and setForce is called
            _muscleArray[i]->computeActuation(s);
            force[i] = _muscleArray[i]->getActuation(s);
            fibforce[i] = _muscleArray[i]->getFiberForce(s);
            actfibforce[i] = _muscleArray[i]->getActiveFiberForce(s);
            passfibforce[i] = _muscleArray[i]->getPassiveFiberForce(s);
            actfibforcealongten[i] = _muscleArray[i]->getActiveFiberForceAlongTendon(s);
            passfibforcealongten[i] = _muscleArray[i]->getPassiveFiberForceAlongTendon(s);
        }
        catch (const std::exception& e) {
            if(!forceWarning){
                cout << "WARNING- MuscleAnalysis::record() unable to evaluate ";
                cout << "muscle forces at time " << s.getTime() << " for reason: ";
                cout << e.what() << endl;
                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;
}