Esempio n. 1
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";
    }
}
Esempio n. 2
0
//==========================================================================================================
void testControlSetControllerOnBlock()
{
    using namespace SimTK;

    // Create a new OpenSim model
    Model osimModel;
    osimModel.setName("osimModel");

    // Get the ground body
    OpenSim::Body& ground = osimModel.getGroundBody();

    // Create a 20 kg, 0.1 m^3 block body
    double blockMass = 20.0, blockSideLength = 0.1;
    Vec3 blockMassCenter(0), groundOrigin(0), blockInGround(0, blockSideLength/2, 0);
    Inertia blockIntertia = Inertia::brick(blockSideLength, blockSideLength, blockSideLength);

    OpenSim::Body block("block", blockMass, blockMassCenter, blockMass*blockIntertia);

    //Create a free joint with 6 degrees-of-freedom
    SimTK::Vec3 noRotation(0);
    SliderJoint blockToGround("",ground, blockInGround, noRotation, block, blockMassCenter, noRotation);
    // Create 6 coordinates (degrees-of-freedom) between the ground and block
    CoordinateSet& jointCoordinateSet = blockToGround.upd_CoordinateSet();
    double posRange[2] = {-1, 1};
    jointCoordinateSet[0].setName("xTranslation");
    jointCoordinateSet[0].setMotionType(Coordinate::Translational);
    jointCoordinateSet[0].setRange(posRange);

    // Add the block body to the model
    osimModel.addBody(&block);

    // Define a single coordinate actuator.
    CoordinateActuator actuator(jointCoordinateSet[0].getName());
    actuator.setName("actuator");

    // Add the actuator to the model
    osimModel.addForce(&actuator);

    double initialTime = 0;
    double finalTime = 1.0;

    // Define the initial and final control values
    double controlForce[1] = {100};
    // Create two control signals
    ControlLinear control;
    control.setName("actuator");
    // Create a control set and add the controls to the set
    ControlSet actuatorControls;
    actuatorControls.adoptAndAppend(&control);
    actuatorControls.setMemoryOwner(false);
    actuatorControls.setControlValues(initialTime, controlForce);
    actuatorControls.setControlValues(finalTime, controlForce);
    // Create a control set controller that simply applies controls from a ControlSet
    ControlSetController actuatorController;
    // Make a copy and set it on the ControlSetController as it takes ownership of the
    // ControlSet passed in
    actuatorController.setControlSet((ControlSet*)Object::SafeCopy(&actuatorControls));

    // add the controller to the model
    osimModel.addController(&actuatorController);

    // Initialize the system and get the state representing the state system
    SimTK::State& si = osimModel.initSystem();

    // Specify zero slider joint kinematic states
    CoordinateSet &coordinates = osimModel.updCoordinateSet();
    coordinates[0].setValue(si, 0.0);    // x translation
    coordinates[0].setSpeedValue(si, 0.0);			 // x speed

    // Create the integrator and manager for the simulation.
    double accuracy = 1.0e-3;
    SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
    integrator.setAccuracy(accuracy);
    Manager manager(osimModel, integrator);

    // Integrate from initial time to final time
    manager.setInitialTime(initialTime);
    manager.setFinalTime(finalTime);
    std::cout<<"\n\nIntegrating from "<<initialTime<<" to "<<finalTime<<std::endl;
    manager.integrate(si);

    si.getQ().dump("Final position:");
    double x_err = fabs(coordinates[0].getValue(si) - 0.5*(controlForce[0]/blockMass)*finalTime*finalTime);
    ASSERT(x_err <= accuracy, __FILE__, __LINE__, "ControlSetControllerOnBlock failed to produce the expected motion.");

    // Save the simulation results
    Storage states(manager.getStateStorage());
    states.print("block_push.sto");

    osimModel.disownAllComponents();
}// end of testControlSetControllerOnBlock()