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