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