C++ FunctionSet::getIndex方法代码示例

/**
 * Set the velocity functions for the tasks.  Functions are set based on the
 * correspondence of the function and the task.  For example,
 * a task with the name "x" will search for a function or functions
 * with the name "x".  For tasks that require 3 functions, such
 * as CMC_Point tasks, the assumption is that there will be three
 * consecutive functions named "x" in the function set.  If the correct
 * number of functions is not found, the task is disabled.
 *
 * @param aFuncSet Function set.
 * @return Pointer to the previous function set.
 */
void CMC_TaskSet::
setFunctionsForVelocity(FunctionSet &aFuncSet)
{
    // LOOP THROUGH TRACK OBJECTS
    int i,j,iFunc=0;
    int nTrk;
    string name;
    Function *f[3];

    const CoordinateSet& coords = getModel()->getCoordinateSet(); 

    for(i=0;i<getSize();i++) {

        // OBJECT
        TrackingTask& ttask = get(i);

        // If CMC_Task process same way as pre 2.0.2
        if (dynamic_cast<CMC_Task*>(&ttask)==NULL) 
            continue;

        CMC_Task& task = dynamic_cast<CMC_Task&>(ttask);

        // NAME
        name = task.getName();
        if(name.empty()) continue;

        const Coordinate& coord = coords.get(name);

        // FIND FUNCTION(S)
        f[0] = f[1] = f[2] = NULL;
        nTrk = task.getNumTaskFunctions();
        iFunc = aFuncSet.getIndex(coord.getSpeedName(),iFunc);

        if (iFunc < 0){
            name = coord.getJoint().getName() + "/" + coord.getSpeedName();
            iFunc = aFuncSet.getIndex(name, iFunc);
            if (iFunc < 0){
                string msg = "CMC_TaskSet::setFunctionsForVelocity: function for task '";
                msg += name + " not found.";
                throw Exception(msg);
            }
        }

        for(j=0;j<nTrk;j++) {
            try {
                f[j] = &aFuncSet.get(iFunc);
            } catch(const Exception& x) {
                x.print(cout);
            }
            if(f[j]==NULL) break;
        }
        task.setTaskFunctionsForVelocity(f[0],f[1],f[2]);
    }
}

/**
 * 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(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());

    // Construct the system and get the working state when done changing the model
    SimTK::State& s = aModel->initSystem();
    
    // 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++){
        IKCoordinateTask *coordTask = dynamic_cast<IKCoordinateTask *>(&_ikTaskSet[i]);
        if (coordTask && coordTask->getApply()){
            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("MarkerPlacer: value for coordinate "+coordTask->getName()+" not found.");

            // We have a valid coordinate reference so now set its weight according to the task
            coordRef->setWeight(coordTask->getWeight());
            coordinateReferences.push_back(*coordRef);      
        }           
    }
    double constraintWeight = std::numeric_limits<SimTK::Real>::infinity();

    InverseKinematicsSolver ikSol(*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){
//.........这里部分代码省略.........

/**
 * Set the functions for the tasks.  Functions are set based on the
 * correspondence of the function and the task.  For example,
 * a task with the name "x" will search for a function or functions
 * with the name "x".  For tasks that require 3 functions, such
 * as CMC_Point tasks, the assumption is that there will be three
 * consecutive functions named "x" in the function set.  If the correct
 * number of functions is not found, the task is disabled.
 *
 * @param aFuncSet Function set.
 * @return Pointer to the previous function set.
 */
void CMC_TaskSet::
setFunctions(FunctionSet &aFuncSet)
{
    // LOOP THROUGH TRACK OBJECTS
    int i,j,iFunc=0;
    int nTrk;
    string name;
    Function *f[3];
    for(i=0;i<getSize();i++) {

        // OBJECT
        TrackingTask& ttask = get(i);

        if (dynamic_cast<StateTrackingTask*>(&ttask)!=NULL) {
            StateTrackingTask& sTask = dynamic_cast<StateTrackingTask&>(ttask);
            if (aFuncSet.contains(sTask.getName())){
                sTask.setTaskFunctions(&aFuncSet.get(sTask.getName()));
            }
            else{
                cout << "State tracking task " << sTask.getName() 
                    << "has no data to track and will be ignored" << std::endl;
            }
            continue;
        }
        // If CMC_Task process same way as pre 2.0.2
        if (dynamic_cast<CMC_Task*>(&ttask)==NULL) 
            continue;

        CMC_Task& task = dynamic_cast<CMC_Task&>(ttask);
        // NAME
        name = task.getName();
        if(name.empty()) continue;

        // FIND FUNCTION(S)
        f[0] = f[1] = f[2] = NULL;
        nTrk = task.getNumTaskFunctions();
        iFunc = aFuncSet.getIndex(name,iFunc);
        if (iFunc < 0){
            const Coordinate& coord = _model->getCoordinateSet().get(name);
            name = coord.getJoint().getName() + "/" + name + "/value";
            iFunc = aFuncSet.getIndex(name, iFunc);
            if (iFunc < 0){
                string msg = "CMC_TaskSet::setFunctionsForVelocity: function for task '";
                msg += name + " not found.";
                throw Exception(msg);
            }
        }

        for(j=0;j<nTrk;j++) {
            try {
                f[j] = &aFuncSet.get(iFunc);
            } catch(const Exception& x) {
                x.print(cout);
            }
            if(f[j]==NULL) break;
            if(name == f[j]->getName()) {
                iFunc++;
            } else {
                f[j] = NULL;
                break;
            }
        }
        task.setTaskFunctions(f[0],f[1],f[2]);
    }
}

//.........这里部分代码省略.........
    /* Delete any markers from the model that are not in the static
     * pose marker file.
     */
    aModel->deleteUnusedMarkers(staticPose->getMarkerNames());

    // Construct the system and get the working state when done changing the model
    SimTK::State& s = aModel->initSystem();
    s.updTime() = _timeRange[0];
    
    // Create references and WeightSets needed to initialize InverseKinemaicsSolver
    Set<MarkerWeight> markerWeightSet;
    _ikTaskSet.createMarkerWeightSet(markerWeightSet); // order in tasks file
    // MarkersReference takes ownership of marker data (staticPose)
    MarkersReference markersReference(staticPoseTable, &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++){
        IKCoordinateTask *coordTask = dynamic_cast<IKCoordinateTask *>(&_ikTaskSet[i]);
        if (coordTask && coordTask->getApply()){
            std::unique_ptr<CoordinateReference> coordRef{};
            if(coordTask->getValueType() == IKCoordinateTask::FromFile){
                index = coordFunctions->getIndex(coordTask->getName(), index);
                if(index >= 0){
                    coordRef.reset(new CoordinateReference(coordTask->getName(),coordFunctions->get(index)));
                }
            }
            else if((coordTask->getValueType() == IKCoordinateTask::ManualValue)){
                Constant reference(Constant(coordTask->getValue()));
                coordRef.reset(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.reset(new CoordinateReference(coordTask->getName(), reference));
            }

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

            // We have a valid coordinate reference so now set its weight according to the task
            coordRef->setWeight(coordTask->getWeight());
            coordinateReferences.push_back(*coordRef);      
        }           
    }
    double constraintWeight = std::numeric_limits<SimTK::Real>::infinity();

    InverseKinematicsSolver ikSol(*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();