/** * 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"; } }
bool Manager::doIntegration(SimTK::State& s, int step, double dtFirst ) { if(!_integ) { throw Exception("Manager::doIntegration(): " "Integrator has not been set. Construct the Manager " "with an integrator, or call Manager::setIntegrator()."); } // CLEAR ANY INTERRUPT // Halts must arrive during an integration. clearHalt(); double dt/*,dtPrev*/,tReal; double time =_ti; dt=dtFirst; if(dt>_dtMax) dt = _dtMax; //dtPrev=dt; // CHECK SPECIFIED DT STEPPING if(_specifiedDT) { if(_tArray.getSize()<=0) { string msg="IntegRKF.integrate: ERR- specified dt stepping not"; msg += "possible-- empty time array."; throw( Exception(msg) ); } double first = _tArray[0]; double last = _tArray.getLast(); if((getTimeArrayStep(_ti)<0) || (_ti<first) || (_tf>last)) { string msg="IntegRKF.integrate: ERR- specified dt stepping not"; msg +="possible-- time array does not cover the requested"; msg +=" integration interval."; throw(Exception(msg)); } } // RECORD FIRST TIME STEP if(!_specifiedDT) { resetTimeAndDTArrays(time); if(_tArray.getSize()<=0) { _tArray.append(time); } } bool fixedStep = false; double fixedStepSize; if( _constantDT || _specifiedDT) fixedStep = true; // If _system is has been set we should be integrating a CMC system // not the model's system. const SimTK::System& sys = _system ? *_system : _model->getMultibodySystem(); // Only initialize a TimeStepper if it hasn't been done yet if (_timeStepper == NULL) initializeTimeStepper(sys, s); SimTK::Integrator::SuccessfulStepStatus status; if( fixedStep ) { dt = getFixedStepSize(getTimeArrayStep(_ti)); } else { _integ->setReturnEveryInternalStep(true); } if( s.getTime()+dt >= _tf ) dt = _tf - s.getTime(); // We need to be at a valid stage to initialize the controls, but only when // we are integrating the complete model system, not the CMC system. This // is very ugly and a cleaner solution is required- aseth if(_system == NULL) sys.realize(s, SimTK::Stage::Velocity); // this is multibody system initialize(s, dt); if( fixedStep){ s.updTime() = time; sys.realize(s, SimTK::Stage::Acceleration); if(_performAnalyses)_model->updAnalysisSet().step(s, step); tReal = s.getTime(); if( _writeToStorage ) { SimTK::Vector stateValues = _model->getStateVariableValues(s); StateVector vec; vec.setStates(tReal, stateValues); getStateStorage().append(vec); if(_model->isControlled()) _controllerSet->storeControls(s,step); } } double stepToTime = _tf; // LOOP while( time < _tf ) { if( fixedStep ){ fixedStepSize = getNextTimeArrayTime( time ) - time; if( fixedStepSize + time >= _tf ) fixedStepSize = _tf - time; _integ->setFixedStepSize( fixedStepSize ); stepToTime = time + fixedStepSize; } // stepTo() does not return if it fails. However, the final step // is returned once as an ordinary return; by the time we get // EndOfSimulation status we have already seen the state and don't // need to record it again. status = _timeStepper->stepTo(stepToTime); if( status != SimTK::Integrator::EndOfSimulation ) { const SimTK::State& s = _integ->getState(); if(_performAnalyses)_model->updAnalysisSet().step(s,step); tReal = s.getTime(); if( _writeToStorage) { SimTK::Vector stateValues = _model->getStateVariableValues(s); StateVector vec; vec.setStates(tReal, stateValues); getStateStorage().append(vec); if(_model->isControlled()) _controllerSet->storeControls(s, step); } step++; } else halt(); time = _integ->getState().getTime(); // CHECK FOR INTERRUPT if(checkHalt()) break; } finalize(_integ->updAdvancedState() ); s = _integ->getState(); // CLEAR ANY INTERRUPT clearHalt(); return true; }
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(); }
void testNumberOfOrientationsMismatch() { cout << "\ntestInverseKinematicsSolver::testNumberOfOrientationsMismatch()" << endl; std::unique_ptr<Model> leg{ constructLegWithOrientationFrames() }; const Coordinate& coord = leg->getCoordinateSet()[0]; SimTK::State state = leg->initSystem(); StatesTrajectory states; // sample time double dt = 0.1; int N = 11; for (int i = 0; i < N; ++i) { state.updTime() = i*dt; coord.setValue(state, i*dt*SimTK::Pi / 3); states.append(state); } double err = 0.1; SimTK::RowVector_<SimTK::Rotation> biases(3, SimTK::Rotation()); // bias thigh_imu biases[0] *= SimTK::Rotation(err, SimTK::XAxis); cout << "biases: " << biases << endl; auto orientationsTable = generateOrientationsDataFromModelAndStates(*leg, states, biases, 0.0, true); SimTK::Vector_<SimTK::Rotation> unusedCol(N, SimTK::Rotation(0.987654321, SimTK::ZAxis)); auto usedOrientationNames = orientationsTable.getColumnLabels(); // add an unused orientation sensor to the given orientation data orientationsTable.appendColumn("unused", unusedCol); cout << "Before:\n" << orientationsTable << endl; // re-order "observed" orientation data SimTK::Matrix_<SimTK::Rotation> dataGutsCopy = orientationsTable.getMatrix(); int last = dataGutsCopy.ncol() - 1; // swap first and last columns orientationsTable.updMatrix()(0) = dataGutsCopy(last); orientationsTable.updMatrix()(last) = dataGutsCopy(0); auto columnNames = orientationsTable.getColumnLabels(); orientationsTable.setColumnLabel(0, columnNames[last]); orientationsTable.setColumnLabel(last, columnNames[0]); columnNames = orientationsTable.getColumnLabels(); // Inject NaN in "observations" of thigh_imu orientation data for (int i = 4; i < 7; ++i) { orientationsTable.updMatrix()(i, 1).scalarMultiply(SimTK::NaN); } cout << "After reorder and NaN injections:\n" << orientationsTable << endl; OrientationsReference orientationsRef(orientationsTable); int nmr = orientationsRef.getNumRefs(); auto& osNames = orientationsRef.getNames(); cout << osNames << endl; MarkersReference mRefs{}; SimTK::Array_<CoordinateReference> coordRefs; // Reset the initial coordinate value coord.setValue(state, 0.0); InverseKinematicsSolver ikSolver(*leg, mRefs, orientationsRef, coordRefs); double tol = 1e-4; ikSolver.setAccuracy(tol); ikSolver.assemble(state); int nos = ikSolver.getNumOrientationSensorsInUse(); SimTK::Array_<double> orientationErrors(nos); for (double t : orientationsRef.getTimes()) { state.updTime() = t; ikSolver.track(state); //get the orientation errors ikSolver.computeCurrentOrientationErrors(orientationErrors); int nose = orientationErrors.size(); SimTK_ASSERT_ALWAYS(nose == nos, "InverseKinematicsSolver failed to account " "for unused orientations reference (observation)."); cout << "time: " << state.getTime() << " |"; auto namesIter = usedOrientationNames.begin(); for (int j = 0; j < nose; ++j) { const auto& orientationName = ikSolver.getOrientationSensorNameForIndex(j); cout << " " << orientationName << " error = " << orientationErrors[j]; SimTK_ASSERT_ALWAYS(*namesIter++ != "unused", "InverseKinematicsSolver failed to ignore " "unused orientation reference (observation)."); if (orientationName == "thigh_imu") {//should see error on biased marker SimTK_ASSERT_ALWAYS(abs(orientationErrors[j]) <= err, "InverseKinematicsSolver mangled marker order."); } else { // other markers should be minimally affected SimTK_ASSERT_ALWAYS(orientationErrors[j] <= tol, "InverseKinematicsSolver mangled marker order."); } } cout << endl; } }
void testNumberOfMarkersMismatch() { cout << "\ntestInverseKinematicsSolver::testNumberOfMarkersMismatch()" << endl; std::unique_ptr<Model> pendulum{ constructPendulumWithMarkers() }; const Coordinate& coord = pendulum->getCoordinateSet()[0]; SimTK::State state = pendulum->initSystem(); StatesTrajectory states; // sample time double dt = 0.1; int N = 11; for (int i = 0; i < N; ++i) { state.updTime() = i*dt; coord.setValue(state, i*dt*SimTK::Pi / 3); states.append(state); } double err = 0.05; SimTK::RowVector_<SimTK::Vec3> biases(3, SimTK::Vec3(0)); // bias m0 biases[0] += SimTK::Vec3(0, err, 0); cout << "biases: " << biases << endl; auto markerTable = generateMarkerDataFromModelAndStates(*pendulum, states, biases, 0.0, true); SimTK::Vector_<SimTK::Vec3> unusedCol(N, SimTK::Vec3(0.987654321)); auto usedMarkerNames = markerTable.getColumnLabels(); // add an unused marker to the marker data markerTable.appendColumn("unused", unusedCol); cout << "Before:\n" << markerTable << endl; // re-order "observed" marker data SimTK::Matrix_<SimTK::Vec3> dataGutsCopy = markerTable.getMatrix(); int last = dataGutsCopy.ncol() - 1; // swap first and last columns markerTable.updMatrix()(0) = dataGutsCopy(last); markerTable.updMatrix()(last) = dataGutsCopy(0); auto columnNames = markerTable.getColumnLabels(); markerTable.setColumnLabel(0, columnNames[last]); markerTable.setColumnLabel(last, columnNames[0]); columnNames = markerTable.getColumnLabels(); // Inject NaN in "observations" of "mR" marker data for (int i = 4; i < 7; ++i) { markerTable.updMatrix()(i, 1) = SimTK::NaN; } cout << "After reorder and NaN injections:\n" << markerTable << endl; MarkersReference markersRef(markerTable); int nmr = markersRef.getNumRefs(); auto& markerNames = markersRef.getNames(); cout << markerNames << endl; SimTK::Array_<CoordinateReference> coordRefs; // Reset the initial coordinate value coord.setValue(state, 0.0); InverseKinematicsSolver ikSolver(*pendulum, markersRef, coordRefs); double tol = 1e-4; ikSolver.setAccuracy(tol); ikSolver.assemble(state); int nm = ikSolver.getNumMarkersInUse(); SimTK::Array_<double> markerErrors(nm); for (unsigned i = 0; i < markersRef.getNumFrames(); ++i) { state.updTime() = i*dt; ikSolver.track(state); //get the marker errors ikSolver.computeCurrentMarkerErrors(markerErrors); int nme = markerErrors.size(); SimTK_ASSERT_ALWAYS(nme == nm, "InverseKinematicsSolver failed to account " "for unused marker reference (observation)."); cout << "time: " << state.getTime() << " |"; auto namesIter = usedMarkerNames.begin(); for (int j = 0; j < nme; ++j) { const auto& markerName = ikSolver.getMarkerNameForIndex(j); cout << " " << markerName << " error = " << markerErrors[j]; SimTK_ASSERT_ALWAYS( *namesIter++ != "unused", "InverseKinematicsSolver failed to ignore " "unused marker reference (observation)."); if (markerName == "m0") {//should see error on biased marker SimTK_ASSERT_ALWAYS(abs(markerErrors[j]-err) <= tol, "InverseKinematicsSolver mangled marker order."); } else { // other markers should be minimally affected SimTK_ASSERT_ALWAYS(markerErrors[j] <= tol, "InverseKinematicsSolver mangled marker order."); } } cout << endl; } }
void testTrackWithUpdateMarkerWeights() { cout << "\ntestInverseKinematicsSolver::testTrackWithUpdateMarkerWeights()" << endl; std::unique_ptr<Model> pendulum{ constructPendulumWithMarkers() }; Coordinate& coord = pendulum->getCoordinateSet()[0]; SimTK::State state = pendulum->initSystem(); StatesTrajectory states; // sample time double dt = 0.01; for (int i = 0; i < 101; ++i) { state.updTime()=i*dt; coord.setValue(state, SimTK::Pi / 3); states.append(state); } SimTK::RowVector_<SimTK::Vec3> biases(3, SimTK::Vec3(0)); MarkersReference markersRef(generateMarkerDataFromModelAndStates(*pendulum, states, biases, 0.02, true)); auto& markerNames = markersRef.getNames(); for (const auto& name : markerNames) { markersRef.updMarkerWeightSet().adoptAndAppend( new MarkerWeight(name, 1.0)); } SimTK::Array_<CoordinateReference> coordRefs; // Reset the initial coordinate value coord.setValue(state, 0.0); InverseKinematicsSolver ikSolver(*pendulum, markersRef, coordRefs); ikSolver.setAccuracy(1e-6); ikSolver.assemble(state); SimTK::Array_<double> markerWeights; markersRef.getWeights(state, markerWeights); SimTK::Array_<double> leftMarkerWeightedErrors; double previousErr = 0.1; for (unsigned i = 0; i < markersRef.getNumFrames(); ++i) { state.updTime() = i*dt; // increment the weight of the left marker each time markerWeights[2] = 0.1*i+1; ikSolver.updateMarkerWeights(markerWeights); ikSolver.track(state); if (i>0 && (i % 10 == 0)) { //get the marker errors ikSolver.computeCurrentMarkerErrors(leftMarkerWeightedErrors); cout << "time: " << state.getTime() << " | " << markerNames[2] << "(weight = " << markerWeights[2] << ") error = " << leftMarkerWeightedErrors[2] << endl; // increasing the marker weight (marker[2] = "mL") should cause // that marker error to decrease SimTK_ASSERT_ALWAYS( leftMarkerWeightedErrors[2] < previousErr, "InverseKinematicsSolver track failed to lower 'left' " "marker error when marker weight was increased."); previousErr = leftMarkerWeightedErrors[2]; } } }
//============================================================================= // HELPER //============================================================================= void AnalyzeTool::run(SimTK::State& s, Model &aModel, int iInitial, int iFinal, const Storage &aStatesStore, bool aSolveForEquilibrium) { AnalysisSet& analysisSet = aModel.updAnalysisSet(); for(int i=0;i<analysisSet.getSize();i++) { analysisSet.get(i).setStatesStore(aStatesStore); } // TODO: some sort of filtering or something to make derivatives smoother? GCVSplineSet statesSplineSet(5,&aStatesStore); // PERFORM THE ANALYSES double tPrev=0.0,t=0.0,dt=0.0; int ny = s.getNY(); Array<double> dydt(0.0,ny); Array<double> yFromStorage(0.0,ny); const Array<string>& labels = aStatesStore.getColumnLabels(); int numOpenSimStates = labels.getSize()-1; SimTK::Vector stateData; stateData.resize(numOpenSimStates); for(int i=iInitial;i<=iFinal;i++) { tPrev = t; aStatesStore.getTime(i,s.updTime()); // time t = s.getTime(); aModel.setAllControllersEnabled(true); aStatesStore.getData(i,numOpenSimStates,&stateData[0]); // states // Get data into local Vector and assign to State using common utility // to handle internal (non-OpenSim) states that may exist Array<std::string> stateNames = aStatesStore.getColumnLabels(); for (int j=0; j<stateData.size(); ++j){ // storage labels included time at index 0 so +1 to skip aModel.setStateVariableValue(s, stateNames[j+1], stateData[j]); } // Adjust configuration to match constraints and other goals aModel.assemble(s); // equilibrateMuscles before realization as it may affect forces if(aSolveForEquilibrium){ try{// might not be able to equilibrate if model is in // a non-physical pose. For example, a pose where the // muscle length is shorter than the tendon slack-length. // the muscle will throw an Exception in this case. aModel.equilibrateMuscles(s); } catch (const std::exception& e) { cout << "WARNING- AnalyzeTool::run() unable to equilibrate muscles "; cout << "at time = " << t <<"." << endl; cout << "Reason: " << e.what() << endl; } } // Make sure model is at least ready to provide kinematics aModel.getMultibodySystem().realize(s, SimTK::Stage::Velocity); if(i==iInitial) { analysisSet.begin(s); } else if(i==iFinal) { analysisSet.end(s); // Step } else { analysisSet.step(s,i); } } }