Ejemplo n.º 1
0
/**
 * Filter the controls.  This method was introduced as a means of attempting
 * to reduce the sizes of residuals.  Unfortunately, the approach was
 * generally unsuccessful because the desired accelerations were not
 * achieved.
 *
 * @param aControlSet Set of controls computed by CMC.
 * @param aDT Current time window.
 * @param rControls Array of filtered controls.
 */
void CMC::
FilterControls(const SimTK::State& s, const ControlSet &aControlSet,double aDT,
               OpenSim::Array<double> &rControls,bool aVerbosePrinting)
{
    if(aDT <= SimTK::Zero) {
        if(aVerbosePrinting) cout<<"\nCMC.filterControls: aDT is practically 0.0, skipping!\n\n";
        return;
    }

    if(aVerbosePrinting) cout<<"\n\nFiltering controls to limit curvature...\n";

    int i;
    int size = rControls.getSize();
    Array<double> x0(0.0,size),x1(0.0,size),x2(0.0,size);

    // SET TIMES
    double t0,t1/*,t2*/;
    // t2 = s.getTime();
    t1 = s.getTime() - aDT;
    t0 = t1 - aDT;

    // SET CONTROL VALUES
    x2 = rControls;
    aControlSet.getControlValues(t1,x1);
    aControlSet.getControlValues(t0,x0);

    // LOOP OVER CONTROLS
    double m1,m2;
    double curvature;
    double thresholdCurvature = 2.0 * 0.05 / (aDT * aDT);
    
    //double thresholdSlopeDiff = 0.2 / aDT;
    for(i=0;i<size;i++) {
        m2 = (x2[i]-x1[i]) / aDT;
        m1 = (x1[i]-x0[i]) / aDT;

                
        curvature = (m2 - m1) / aDT;
        curvature = fabs(curvature);

        if(curvature<=thresholdCurvature) continue;
    
//      diff = fabs(m2) - fabs(m1);
//      cout<<"thresholdSlopeDiff="<<thresholdSlopeDiff<<"  slopeDiff="<<diff<<endl;
//      if(diff>thresholdSlopeDiff) continue;
        
        

        // ALTER CONTROL VALUE
        rControls[i] = (3.0*x2[i] + 2.0*x1[i] + x0[i]) / 6.0;

        // PRINT
        if(aVerbosePrinting) cout<<aControlSet[i].getName()<<": old="<<x2[i]<<" new="<<rControls[i]<<endl;
    }

    if(aVerbosePrinting) cout<<endl<<endl;
}
Ejemplo n.º 2
0
/**
 * 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";
    }
}