void Controller::pauseState(const CtrlEvent *evt) {
    switch(CtrlEvent_FSMSIGnal(evt)) {

		case OMX_SETSTATE_FSMSIG: 
			DEBUGLOG(("(Ctrl %x) Pause - setState %d\n", this, evt->args.cmd.param)) ;
#ifdef OSTTRACE
			OstTraceFiltInst1(TRACE_FLOW, "ENS_HOST: Controller::pauseState OMX_SETSTATE_FSMSIG %d", evt->args.cmd.param);
#endif
			{
				OMX_STATETYPE newOMXState = (OMX_STATETYPE)evt->args.cmd.param ;			
	
				if (newOMXState == OMX_StateExecuting) {
					this->state = (FSM_State) &Controller::pauseToExecutingState ;					
				} else if (newOMXState == OMX_StateIdle) {
					this->state = (FSM_State) &Controller::pauseToIdleState ;
				} else { ARMNMF_DBC_ASSERT(0); }
				
				forwardStateChange(newOMXState) ;
			}
			break;
		   
		default:
			defaultState(evt, OMX_StatePause) ;
			break;
    }
} /* pauseState */
Пример #2
0
    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();
    }