Beispiel #1
1
/**
 * 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);
}
//==========================================================================================================
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()