/**
* Compute the controls for a simulation.
*
* This method alters the control set in order to control the simulation.
*/
void CMC::
computeControls(SimTK::State& s, ControlSet &controlSet)
{
// CONTROLS SHOULD BE RECOMPUTED- NEED A NEW TARGET TIME
_tf = s.getTime() + _targetDT;
int i,j;
// TURN ANALYSES OFF
_model->updAnalysisSet().setOn(false);
// TIME STUFF
double tiReal = s.getTime();
double tfReal = _tf;
cout<<"CMC.computeControls: t = "<<s.getTime()<<endl;
if(_verbose) {
cout<<"\n\n----------------------------------\n";
cout<<"integration step size = "<<_targetDT<<", target time = "<<_tf<<endl;
}
// SET CORRECTIONS
int nq = _model->getNumCoordinates();
int nu = _model->getNumSpeeds();
FunctionSet *qSet = _predictor->getCMCActSubsys()->getCoordinateTrajectories();
FunctionSet *uSet = _predictor->getCMCActSubsys()->getSpeedTrajectories();
Array<double> qDesired(0.0,nq),uDesired(0.0,nu);
qSet->evaluate(qDesired,0,tiReal);
if(uSet!=NULL) {
uSet->evaluate(uDesired,0,tiReal);
} else {
qSet->evaluate(uDesired,1,tiReal);
}
Array<double> qCorrection(0.0,nq),uCorrection(0.0,nu);
const Vector& q = s.getQ();
const Vector& u = s.getU();
for(i=0;i<nq;i++) qCorrection[i] = q[i] - qDesired[i];
for(i=0;i<nu;i++) uCorrection[i] = u[i] - uDesired[i];
_predictor->getCMCActSubsys()->setCoordinateCorrections(&qCorrection[0]);
_predictor->getCMCActSubsys()->setSpeedCorrections(&uCorrection[0]);
if( _verbose ) {
cout << "\n=============================" << endl;
cout << "\nCMC:computeControls" << endl;
cout << "\nq's = " << s.getQ() << endl;
cout << "\nu's = " << s.getU() << endl;
cout << "\nz's = " << s.getZ() << endl;
cout<<"\nqDesired:"<<qDesired << endl;
cout<<"\nuDesired:"<<uDesired << endl;
cout<<"\nQCorrections:"<<qCorrection << endl;
cout<<"\nUCorrections:"<<uCorrection << endl;
}
// realize to Velocity because some tasks (eg. CMC_Point) need to be
// at velocity to compute errors
_model->getMultibodySystem().realize(s, Stage::Velocity );
// ERRORS
_taskSet->computeErrors(s, tiReal);
_taskSet->recordErrorsAsLastErrors();
Array<double> &pErr = _taskSet->getPositionErrors();
Array<double> &vErr = _taskSet->getVelocityErrors();
if(_verbose) cout<<"\nErrors at time "<<s.getTime()<<":"<<endl;
int e=0;
for(i=0;i<_taskSet->getSize();i++) {
TrackingTask& task = _taskSet->get(i);
if(_verbose) {
for(j=0;j<task.getNumTaskFunctions();j++) {
cout<<task.getName()<<": ";
cout<<"pErr="<<pErr[e]<<" vErr="<<vErr[e]<<endl;
e++;
}
}
}
double *err = new double[pErr.getSize()];
for(i=0;i<pErr.getSize();i++) err[i] = pErr[i];
_pErrStore->append(tiReal,pErr.getSize(),err);
for(i=0;i<vErr.getSize();i++) err[i] = vErr[i];
_vErrStore->append(tiReal,vErr.getSize(),err);
// COMPUTE DESIRED ACCELERATIONS
_taskSet->computeDesiredAccelerations(s, tiReal,tfReal);
// Set the weight of the stress term in the optimization target based on this sigmoid-function-blending
// Note that if no task limits are set then by default the weight will be 1.
// TODO: clean this up -- currently using dynamic_casts to make sure we're not using fast target, etc.
if(dynamic_cast<ActuatorForceTarget*>(_target)) {
double relativeTau = 0.1;
ActuatorForceTarget *realTarget = dynamic_cast<ActuatorForceTarget*>(_target);
Array<double> &pErr = _taskSet->getPositionErrors();
double stressTermWeight = 1;
for(i=0;i<_taskSet->getSize();i++) {
if(dynamic_cast<CMC_Joint*>(&_taskSet->get(i))) {
CMC_Joint& jointTask = dynamic_cast<CMC_Joint&>(_taskSet->get(i));
if(jointTask.getLimit()) {
double w = ForwardTool::SigmaDn(jointTask.getLimit() * relativeTau, jointTask.getLimit(), fabs(pErr[i]));
if(_verbose) cout << "Task " << i << ": err=" << pErr[i] << ", limit=" << jointTask.getLimit() << ", sigmoid=" << w << endl;
stressTermWeight = min(stressTermWeight, w);
}
}
}
if(_verbose) cout << "Setting stress term weight to " << stressTermWeight << " (relativeTau was " << relativeTau << ")" << std::endl;
realTarget->setStressTermWeight(stressTermWeight);
for(i=0;i<vErr.getSize();i++) err[i] = vErr[i];
_stressTermWeightStore->append(tiReal,1,&stressTermWeight);
}
// SET BOUNDS ON CONTROLS
int N = _predictor->getNX();
Array<double> xmin(0.0,N),xmax(1.0,N);
for(i=0;i<N;i++) {
Control& x = controlSet.get(i);
xmin[i] = x.getControlValueMin(tiReal);
xmax[i] = x.getControlValueMax(tiReal);
}
if(_verbose) {
cout<<"\nxmin:\n"<<xmin<<endl;
cout<<"\nxmax:\n"<<xmax<<endl;
}
// COMPUTE BOUNDS ON MUSCLE FORCES
Array<double> zero(0.0,N);
Array<double> fmin(0.0,N),fmax(0.0,N);
_predictor->setInitialTime(tiReal);
_predictor->setFinalTime(tfReal);
_predictor->setTargetForces(&zero[0]);
_predictor->evaluate(s, &xmin[0], &fmin[0]);
_predictor->evaluate(s, &xmax[0], &fmax[0]);
SimTK::State newState = _predictor->getCMCActSubsys()->getCompleteState();
if(_verbose) {
cout<<endl<<endl;
cout<<"\ntiReal = "<<tiReal<<" tfReal = "<<tfReal<<endl;
cout<<"Min forces:\n";
cout<<fmin<<endl;
cout<<"Max forces:\n";
cout<<fmax<<endl;
}
// Print actuator force range if range is small
double range;
for(i=0;i<N;i++) {
range = fmax[i] - fmin[i];
if(range<1.0) {
cout << "CMC::computeControls WARNING- small force range for "
<< getActuatorSet()[i].getName()
<< " ("<<fmin[i]<<" to "<<fmax[i]<<")\n" << endl;
// if the force range is so small it means the control value, x,
// is inconsequential and we might as well choose the smallest control
// value possible, or else the RootSolver will choose the last value
// it used to evaluate the force, which will be the maximum control
// value. In other words, if the fiber length is so short that no level
// of activation can produce force, the RootSolver gets the same answer
// for force if it uses xmin or:: xmax, but since it uses xmax last
// it returns xmax as the control value. Make xmax = xmin to avoid that.
xmax[i] = xmin[i];
}
}
// SOLVE STATIC OPTIMIZATION FOR DESIRED ACTUATOR FORCES
SimTK::Vector lowerBounds(N), upperBounds(N);
for(i=0;i<N;i++) {
if(fmin[i]<fmax[i]) {
lowerBounds[i] = fmin[i];
upperBounds[i] = fmax[i];
} else {
lowerBounds[i] = fmax[i];
upperBounds[i] = fmin[i];
}
}
_target->setParameterLimits(lowerBounds, upperBounds);
// OPTIMIZER ERROR TRAP
_f.setSize(N);
if(!_target->prepareToOptimize(newState, &_f[0])) {
// No direct solution, need to run optimizer
Vector fVector(N,&_f[0],true);
try {
_optimizer->optimize(fVector);
}
catch (const SimTK::Exception::Base& ex) {
cout << ex.getMessage() << endl;
cout << "OPTIMIZATION FAILED..." << endl;
cout<<endl;
ostringstream msg;
msg << "CMC.computeControls: ERROR- Optimizer could not find a solution." << endl;
msg << "Unable to find a feasible solution at time = " << s.getTime() << "." << endl;
msg << "Model cannot generate the forces necessary to achieve the target acceleration." << endl;
msg << "Possible issues: 1. not all model degrees-of-freedom are actuated, " << endl;
msg << "2. there are tracking tasks for locked coordinates, and/or" << endl;
msg << "3. there are unnecessary control constraints on reserve/residual actuators." << endl;
cout<<"\n"<<msg.str()<<endl<<endl;
throw(new OpenSim::Exception(msg.str(), __FILE__,__LINE__));
}
} else {
// Got a direct solution, don't need to run optimizer
}
if(_verbose) _target->printPerformance(&_f[0]);
if(_verbose) {
cout<<"\nDesired actuator forces:\n";
cout<<_f<<endl;
}
// ROOT SOLVE FOR EXCITATIONS
_predictor->setTargetForces(&_f[0]);
RootSolver rootSolver(_predictor);
Array<double> tol(4.0e-3,N);
Array<double> fErrors(0.0,N);
Array<double> controls(0.0,N);
controls = rootSolver.solve(s, xmin,xmax,tol);
if(_verbose) {
cout<<"\n\nXXX t=" << _tf << " Controls:" <<controls<<endl;
}
// FILTER OSCILLATIONS IN CONTROL VALUES
if(_useCurvatureFilter) FilterControls(s, controlSet,_targetDT,controls,_verbose);
// SET EXCITATIONS
controlSet.setControlValues(_tf,&controls[0]);
_model->updAnalysisSet().setOn(true);
}
/**
* This method is called at the beginning of an analysis so that any
* necessary initializations may be performed.
*
*
* This method should be overridden in the child class. It is
* included here so that the child class will not have to implement it if it
* is not necessary.
*
* @param state system State
* @param aStep Step number of the integration.
*
* @return -1 on error, 0 otherwise.
*/
int ForceReporter::
begin(SimTK::State& s)
{
if(!proceed()) return(0);
tidyForceNames();
// LABELS
constructColumnLabels(s);
// RESET STORAGE
_forceStore.reset(s.getTime());
// RECORD
int status = 0;
if(_forceStore.getSize()<=0) {
status = record(s);
}
return(status);
}
/**
* This method is called at the beginning of an analysis so that any
* necessary initializations may be performed.
*
* This method is meant to be called at the begining of an integration in
* Model::integBeginCallback() and has the same argument list.
*
* @param s SimTK:State
*
* @return -1 on error, 0 otherwise.
*/
int InducedAccelerations::begin(SimTK::State &s)
{
if(!proceed()) return(0);
initialize(s);
// RESET STORAGES
for(int i = 0; i<_storeInducedAccelerations.getSize(); i++){
_storeInducedAccelerations[i]->reset(s.getTime());
}
cout << "Performing Induced Accelerations Analysis" << endl;
// RECORD
int status = 0;
status = record(s);
return(status);
}
/**
* Record the actuation quantities.
*/
int Actuation::
record(const SimTK::State& s)
{
if (_model == NULL) return(-1);
// MAKE SURE ALL ACTUATION QUANTITIES ARE VALID
_model->getMultibodySystem().realize(s, SimTK::Stage::Dynamics);
// TIME NORMALIZATION
double tReal = s.getTime();
// FORCE
const Set<Actuator>& fSet = _model->getActuators();
for (int i = 0, iact = 0; i<fSet.getSize(); i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
if (!fSet.get(i).get_isDisabled())
_fsp[iact++] = act->getActuation(s);
}
_forceStore->append(tReal, _na, _fsp);
// SPEED
for (int i = 0, iact = 0; i<fSet.getSize(); i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
if (!fSet.get(i).get_isDisabled())
_fsp[iact++] = act->getSpeed(s);
}
_speedStore->append(tReal, _na, _fsp);
// POWER
for (int i = 0, iact = 0; i<fSet.getSize(); i++) {
if (!fSet.get(i).get_isDisabled())
_fsp[iact++] = fSet.get(i).getPower(s);
}
_powerStore->append(tReal, _na, _fsp);
return(0);
}
/**
* Record the ForceReporter quantities.
*/
int ForceReporter::record(const SimTK::State& s)
{
if(_model==NULL) return(-1);
// MAKE SURE ALL ForceReporter QUANTITIES ARE VALID
_model->getMultibodySystem().realize(s, SimTK::Stage::Dynamics );
StateVector nextRow = StateVector(s.getTime());
// NUMBER OF Forces
const ForceSet& forces = _model->getForceSet(); // This does not contain gravity
int nf = forces.getSize();
for(int i=0;i<nf;i++) {
// If body force we need to record six values for torque+force
// If muscle we record one scalar
OpenSim::Force& nextForce = (OpenSim::Force&)forces[i];
if (nextForce.isDisabled(s)) continue;
Array<double> values = nextForce.getRecordValues(s);
nextRow.getData().append(values);
}
if(_includeConstraintForces){
// NUMBER OF Constraints
const ConstraintSet& constraints = _model->getConstraintSet(); // This does not contain gravity
int nc = constraints.getSize();
for(int i=0;i<nc;i++) {
OpenSim::Constraint& nextConstraint = (OpenSim::Constraint&)constraints[i];
if (nextConstraint.isDisabled(s)) continue;
Array<double> values = nextConstraint.getRecordValues(s);
nextRow.getData().append(values);
}
}
_forceStore.append(nextRow);
return(0);
}
/**
* Record the kinematics.
*/
int PointKinematics::
record(const SimTK::State& s)
{
const SimbodyEngine& de = _model->getSimbodyEngine();
// VARIABLES
SimTK::Vec3 vec;
const double& time = s.getTime();
// POSITION
de.getPosition(s, *_body,_point,vec);
if(_relativeToBody){
de.transformPosition(s, _model->getGround(), vec, *_relativeToBody, vec);
}
_pStore->append(time, vec);
// VELOCITY
de.getVelocity(s, *_body,_point,vec);
if(_relativeToBody){
de.transform(s, _model->getGround(), vec, *_relativeToBody, vec);
}
_vStore->append(time, vec);
// ACCELERATIONS
_model->getMultibodySystem().realize(s, SimTK::Stage::Acceleration);
de.getAcceleration(s, *_body,_point,vec);
if(_relativeToBody){
de.transform(s, _model->getGround(), vec, *_relativeToBody, vec);
}
_aStore->append(time, vec);
return(0);
}
double getSomething(const SimTK::State& state) const {
const_cast<Foo *>(this)->m_ctr++;
m_mutableCtr++;
return state.getTime();
}
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;
}
}
bool State::isConsistent(const SimTK::State& otherState) const {
if (getNumSubsystems() != otherState.getNumSubsystems())
return false;
// State variables.
if (getNQ() != otherState.getNQ())
return false;
if (getNU() != otherState.getNU())
return false;
if (getNZ() != otherState.getNZ())
return false;
// Constraints.
if (getNQErr() != otherState.getNQErr())
return false;
if (getNUErr() != otherState.getNUErr())
return false;
if (getNUDotErr() != otherState.getNUDotErr())
return false;
// NMultipliers should be the same as NUDotErr, but we leave this check
// here in case they diverge in the future.
if (getNMultipliers() != otherState.getNMultipliers())
return false;
// Events.
if (getNEventTriggers() != otherState.getNEventTriggers())
return false;
// Per-subsystem quantities.
// TODO we could get rid of the total-over-subsystems checks above, but
// those checks would let us exit earlier.
for (SimTK::SubsystemIndex isub(0); isub < getNumSubsystems();
++isub) {
if (getNQ(isub) != otherState.getNQ(isub))
return false;
if (getNU(isub) != otherState.getNU(isub))
return false;
if (getNZ(isub) != otherState.getNZ(isub))
return false;
if (getNQErr(isub) != otherState.getNQErr(isub))
return false;
if (getNUErr(isub) != otherState.getNUErr(isub))
return false;
if (getNUDotErr(isub) != otherState.getNUDotErr(isub))
return false;
// NMultipliers should be the same as NUDotErr, but we leave this check
// here in case they diverge in the future.
if (getNMultipliers(isub) != otherState.getNMultipliers(isub))
return false;
for(SimTK::Stage stage = SimTK::Stage::LowestValid;
stage <= SimTK::Stage::HighestRuntime; ++stage) {
if (getNEventTriggersByStage(isub, stage) !=
otherState.getNEventTriggersByStage(isub, stage))
return false;
}
}
return true;
}
/**
* Record the results.
*/
int StaticOptimization::
record(const SimTK::State& s)
{
if(!_modelWorkingCopy) return -1;
// Set model to whatever defaults have been updated to from the last iteration
SimTK::State& sWorkingCopy = _modelWorkingCopy->updWorkingState();
sWorkingCopy.setTime(s.getTime());
_modelWorkingCopy->initStateWithoutRecreatingSystem(sWorkingCopy);
// update Q's and U's
sWorkingCopy.setQ(s.getQ());
sWorkingCopy.setU(s.getU());
_modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Velocity);
//_modelWorkingCopy->equilibrateMuscles(sWorkingCopy);
const Set<Actuator>& fs = _modelWorkingCopy->getActuators();
int na = fs.getSize();
int nacc = _accelerationIndices.getSize();
// IPOPT
_numericalDerivativeStepSize = 0.0001;
_optimizerAlgorithm = "ipopt";
_printLevel = 0;
//_optimizationConvergenceTolerance = 1e-004;
//_maxIterations = 2000;
// Optimization target
_modelWorkingCopy->setAllControllersEnabled(false);
StaticOptimizationTarget target(sWorkingCopy,_modelWorkingCopy,na,nacc,_useMusclePhysiology);
target.setStatesStore(_statesStore);
target.setStatesSplineSet(_statesSplineSet);
target.setActivationExponent(_activationExponent);
target.setDX(_numericalDerivativeStepSize);
// Pick optimizer algorithm
SimTK::OptimizerAlgorithm algorithm = SimTK::InteriorPoint;
//SimTK::OptimizerAlgorithm algorithm = SimTK::CFSQP;
// Optimizer
SimTK::Optimizer *optimizer = new SimTK::Optimizer(target, algorithm);
// Optimizer options
//cout<<"\nSetting optimizer print level to "<<_printLevel<<".\n";
optimizer->setDiagnosticsLevel(_printLevel);
//cout<<"Setting optimizer convergence criterion to "<<_convergenceCriterion<<".\n";
optimizer->setConvergenceTolerance(_convergenceCriterion);
//cout<<"Setting optimizer maximum iterations to "<<_maximumIterations<<".\n";
optimizer->setMaxIterations(_maximumIterations);
optimizer->useNumericalGradient(false);
optimizer->useNumericalJacobian(false);
if(algorithm == SimTK::InteriorPoint) {
// Some IPOPT-specific settings
optimizer->setLimitedMemoryHistory(500); // works well for our small systems
optimizer->setAdvancedBoolOption("warm_start",true);
optimizer->setAdvancedRealOption("obj_scaling_factor",1);
optimizer->setAdvancedRealOption("nlp_scaling_max_gradient",1);
}
// Parameter bounds
SimTK::Vector lowerBounds(na), upperBounds(na);
for(int i=0,j=0;i<fs.getSize();i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fs.get(i));
if (act) {
lowerBounds(j) = act->getMinControl();
upperBounds(j) = act->getMaxControl();
j++;
}
}
target.setParameterLimits(lowerBounds, upperBounds);
_parameters = 0; // Set initial guess to zeros
// Static optimization
_modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy,SimTK::Stage::Velocity);
target.prepareToOptimize(sWorkingCopy, &_parameters[0]);
//LARGE_INTEGER start;
//LARGE_INTEGER stop;
//LARGE_INTEGER frequency;
//QueryPerformanceFrequency(&frequency);
//QueryPerformanceCounter(&start);
try {
target.setCurrentState( &sWorkingCopy );
optimizer->optimize(_parameters);
}
catch (const SimTK::Exception::Base& ex) {
cout << ex.getMessage() << endl;
cout << "OPTIMIZATION FAILED..." << endl;
cout << endl;
cout << "StaticOptimization.record: WARN- The optimizer could not find a solution at time = " << s.getTime() << endl;
cout << endl;
double tolBounds = 1e-1;
bool weakModel = false;
string msgWeak = "The model appears too weak for static optimization.\nTry increasing the strength and/or range of the following force(s):\n";
for(int a=0;a<na;a++) {
Actuator* act = dynamic_cast<Actuator*>(&_forceSet->get(a));
if( act ) {
Muscle* mus = dynamic_cast<Muscle*>(&_forceSet->get(a));
if(mus==NULL) {
if(_parameters(a) < (lowerBounds(a)+tolBounds)) {
msgWeak += " ";
msgWeak += act->getName();
msgWeak += " approaching lower bound of ";
ostringstream oLower;
oLower << lowerBounds(a);
msgWeak += oLower.str();
msgWeak += "\n";
weakModel = true;
} else if(_parameters(a) > (upperBounds(a)-tolBounds)) {
msgWeak += " ";
msgWeak += act->getName();
msgWeak += " approaching upper bound of ";
ostringstream oUpper;
oUpper << upperBounds(a);
msgWeak += oUpper.str();
msgWeak += "\n";
weakModel = true;
}
} else {
if(_parameters(a) > (upperBounds(a)-tolBounds)) {
msgWeak += " ";
msgWeak += mus->getName();
msgWeak += " approaching upper bound of ";
ostringstream o;
o << upperBounds(a);
msgWeak += o.str();
msgWeak += "\n";
weakModel = true;
}
}
}
}
if(weakModel) cout << msgWeak << endl;
if(!weakModel) {
double tolConstraints = 1e-6;
bool incompleteModel = false;
string msgIncomplete = "The model appears unsuitable for static optimization.\nTry appending the model with additional force(s) or locking joint(s) to reduce the following acceleration constraint violation(s):\n";
SimTK::Vector constraints;
target.constraintFunc(_parameters,true,constraints);
const CoordinateSet& coordSet = _modelWorkingCopy->getCoordinateSet();
for(int acc=0;acc<nacc;acc++) {
if(fabs(constraints(acc)) > tolConstraints) {
const Coordinate& coord = coordSet.get(_accelerationIndices[acc]);
msgIncomplete += " ";
msgIncomplete += coord.getName();
msgIncomplete += ": constraint violation = ";
ostringstream o;
o << constraints(acc);
msgIncomplete += o.str();
msgIncomplete += "\n";
incompleteModel = true;
}
}
_forceReporter->step(sWorkingCopy, 1);
if(incompleteModel) cout << msgIncomplete << endl;
}
}
//QueryPerformanceCounter(&stop);
//double duration = (double)(stop.QuadPart-start.QuadPart)/(double)frequency.QuadPart;
//cout << "optimizer time = " << (duration*1.0e3) << " milliseconds" << endl;
target.printPerformance(sWorkingCopy, &_parameters[0]);
//update defaults for use in the next step
const Set<Actuator>& actuators = _modelWorkingCopy->getActuators();
for(int k=0; k < actuators.getSize(); ++k){
ActivationFiberLengthMuscle *mus = dynamic_cast<ActivationFiberLengthMuscle*>(&actuators[k]);
if(mus){
mus->setDefaultActivation(_parameters[k]);
}
}
_activationStorage->append(sWorkingCopy.getTime(),na,&_parameters[0]);
SimTK::Vector forces(na);
target.getActuation(const_cast<SimTK::State&>(sWorkingCopy), _parameters,forces);
_forceReporter->step(sWorkingCopy, 1);
return 0;
}
/**
* This method is called at the beginning of an analysis so that any
* necessary initializations may be performed.
*
* This method is meant to be called at the beginning of an integration
*
* @param s Current state .
*
* @return -1 on error, 0 otherwise.
*/
int StaticOptimization::
begin(SimTK::State& s )
{
if(!proceed()) return(0);
// Make a working copy of the model
delete _modelWorkingCopy;
_modelWorkingCopy = _model->clone();
_modelWorkingCopy->initSystem();
// Replace model force set with only generalized forces
if(_model) {
SimTK::State& sWorkingCopyTemp = _modelWorkingCopy->updWorkingState();
// Update the _forceSet we'll be computing inverse dynamics for
if(_ownsForceSet) delete _forceSet;
if(_useModelForceSet) {
// Set pointer to model's internal force set
_forceSet = &_modelWorkingCopy->updForceSet();
_ownsForceSet = false;
} else {
ForceSet& as = _modelWorkingCopy->updForceSet();
// Keep a copy of forces that are not muscles to restore them back.
ForceSet* saveForces = as.clone();
// Generate an force set consisting of a coordinate actuator for every unconstrained degree of freedom
_forceSet = CoordinateActuator::CreateForceSetOfCoordinateActuatorsForModel(sWorkingCopyTemp,*_modelWorkingCopy,1,false);
_ownsForceSet = false;
_modelWorkingCopy->setAllControllersEnabled(false);
_numCoordinateActuators = _forceSet->getSize();
// Copy whatever forces that are not muscles back into the model
for(int i=0; i<saveForces->getSize(); i++){
const Force& f=saveForces->get(i);
if ((dynamic_cast<const Muscle*>(&saveForces->get(i)))==NULL)
as.append(saveForces->get(i).clone());
}
}
SimTK::State& sWorkingCopy = _modelWorkingCopy->initSystem();
// Set modeling options for Actuators to be overriden
for(int i=0; i<_forceSet->getSize(); i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->get(i));
if( act ) {
act->overrideActuation(sWorkingCopy, true);
}
}
sWorkingCopy.setTime(s.getTime());
sWorkingCopy.setQ(s.getQ());
sWorkingCopy.setU(s.getU());
sWorkingCopy.setZ(s.getZ());
_modelWorkingCopy->getMultibodySystem().realize(s,SimTK::Stage::Velocity);
_modelWorkingCopy->equilibrateMuscles(sWorkingCopy);
// Gather indices into speed set corresponding to the unconstrained degrees of freedom
// (for which we will set acceleration constraints)
_accelerationIndices.setSize(0);
const CoordinateSet& coordSet = _model->getCoordinateSet();
for(int i=0; i<coordSet.getSize(); i++) {
const Coordinate& coord = coordSet.get(i);
if(!coord.isConstrained(sWorkingCopy)) {
Array<int> inds = _statesStore->
getColumnIndicesForIdentifier(coord.getName()) ;
_accelerationIndices.append(inds[0]);
}
}
int na = _forceSet->getSize();
int nacc = _accelerationIndices.getSize();
if(na < nacc)
throw(Exception("StaticOptimization: ERROR- over-constrained "
"system -- need at least as many forces as there are degrees of freedom.\n") );
_forceReporter.reset(new ForceReporter(_modelWorkingCopy));
_forceReporter->begin(sWorkingCopy);
_forceReporter->updForceStorage().reset();
_parameters.resize(_modelWorkingCopy->getNumControls());
_parameters = 0;
}
_statesSplineSet=GCVSplineSet(5,_statesStore);
// DESCRIPTION AND LABELS
constructDescription();
constructColumnLabels();
deleteStorage();
allocateStorage();
// RESET STORAGE
_activationStorage->reset(s.getTime());
_forceReporter->updForceStorage().reset(s.getTime());
// RECORD
int status = 0;
if(_activationStorage->getSize()<=0) {
status = record(s);
const Set<Actuator>& fs = _modelWorkingCopy->getActuators();
for(int k=0;k<fs.getSize();k++) {
ScalarActuator* act = dynamic_cast<ScalarActuator *>(&fs[k]);
if (act){
cout << "Bounds for " << act->getName() << ": "
<< act->getMinControl() << " to "
<< act->getMaxControl() << endl;
}
else{
std::string msg = getConcreteClassName();
msg += "::can only process scalar Actuator types.";
throw Exception(msg);
}
}
}
return(status);
}
/** get the value of the CoordinateReference */
double CoordinateReference::getValue(const SimTK::State &s) const
{
SimTK::Vector t(1, s.getTime());
return _coordinateValueFunction->calcValue(t);
}
void compareSimulations(SimTK::MultibodySystem &system, SimTK::State &state, Model *osimModel, SimTK::State &osim_state, string errorMessagePrefix = "")
{
using namespace SimTK;
// Set the initial states for both Simbody system and OpenSim model
Vector& qi = state.updQ();
Vector& ui = state.updU();
int nq_sb = initTestStates(qi, ui);
int nq = osim_state.getNQ();
// Push down to OpenSim "state"
if(nq == 2*nq_sb){ //more coordinates because OpenSim model is constrained
osim_state.updY()[0] = state.getY()[0];
osim_state.updY()[1] = state.getY()[1];
osim_state.updY()[nq] = state.getY()[nq_sb];
osim_state.updY()[nq+1] = state.getY()[nq_sb+1];
}
else
osim_state.updY() = state.getY();
//==========================================================================================================
// Integrate Simbody system
integrateSimbodySystem(system, state);
// Simbody model final states
qi = state.updQ();
ui = state.updU();
qi.dump("\nSimbody Final q's:");
ui.dump("\nSimbody Final u's:");
//==========================================================================================================
// Integrate OpenSim model
integrateOpenSimModel(osimModel, osim_state);
// Get the state at the end of the integration from OpenSim.
Vector& qf = osim_state.updQ();
Vector& uf = osim_state.updU();
cout<<"\nOpenSim Final q's:\n "<<qf<<endl;
cout<<"\nOpenSim Final u's:\n "<<uf<<endl;
//==========================================================================================================
// Compare Simulation Results
compareSimulationStates(qi, ui, qf, uf, errorMessagePrefix);
}
/** get the values of the CoordinateReference */
void CoordinateReference::getValues(const SimTK::State &s, SimTK::Array_<double> &values) const
{
SimTK::Vector t(1, s.getTime());
values.resize(getNumRefs());
values[0] = _coordinateValueFunction->calcValue(t);
}
/**
* Record the inverse dynamics forces.
*/
int InverseDynamics::
record(const SimTK::State& s)
{
if(!_modelWorkingCopy) return -1;
//cout << "\nInverse Dynamics record() : \n" << endl;
// Set model Q's and U's
SimTK::State sWorkingCopy = _modelWorkingCopy->getWorkingState();
// Set modeling options for Actuators to be overridden
for(int i=0; i<_forceSet->getSize(); i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->get(i));
if( act ) {
act->overrideActuation(sWorkingCopy, true);
}
}
// Having updated the model, at least re-realize Model stage!
_modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Model);
sWorkingCopy.setTime(s.getTime());
sWorkingCopy.setQ(s.getQ());
sWorkingCopy.setU(s.getU());
// Having updated the states, at least realize to velocity!
_modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Velocity);
int nf = _numCoordinateActuators;
int nacc = _accelerationIndices.getSize();
// int nq = _modelWorkingCopy->getNumCoordinates();
//cout << "\nQ= " << s.getQ() << endl;
//cout << "\nU= " << s.getU() << endl;
// Build linear constraint matrix and constant constraint vector
SimTK::Vector f(nf), c(nacc);
f = 0;
computeAcceleration(sWorkingCopy, &f[0], &_constraintVector[0]);
for(int j=0; j<nf; j++) {
f[j] = 1;
computeAcceleration(sWorkingCopy, &f[0], &c[0]);
for(int i=0; i<nacc; i++) _constraintMatrix(i,j) = (c[i] - _constraintVector[i]);
f[j] = 0;
}
auto coordinates = _modelWorkingCopy->getCoordinatesInMultibodyTreeOrder();
for(int i=0; i<nacc; i++) {
const Coordinate& coord = *coordinates[_accelerationIndices[i]];
int ind = _statesStore->getStateIndex(coord.getSpeedName(), 0);
if (ind < 0){
// get the full coordinate speed state variable path name
string fullname = coord.getStateVariableNames()[1];
ind = _statesStore->getStateIndex(fullname, 0);
if (ind < 0){
string msg = "InverseDynamics::record(): \n";
msg += "target motion for coordinate '";
msg += coord.getName() + "' not found.";
throw Exception(msg);
}
}
Function& targetFunc = _statesSplineSet.get(ind);
std::vector<int> firstDerivComponents(1);
firstDerivComponents[0]=0;
double targetAcceleration = targetFunc.calcDerivative(firstDerivComponents, SimTK::Vector(1, sWorkingCopy.getTime()));
//cout << coord.getName() << " t=" << sWorkingCopy.getTime() << " acc=" << targetAcceleration << " index=" << _accelerationIndices[i] << endl;
_constraintVector[i] = targetAcceleration - _constraintVector[i];
}
//cout << "NEW Constraint Vector Adjusted = " << endl;
//_constraintVector.dump(&t);
// LAPACK SOLVER
// NOTE: It destroys the matrices/vectors we pass to it, so we need to pass it copies of performanceMatrix and performanceVector (don't bother making
// copies of _constraintMatrix/Vector since those are reinitialized each time anyway)
int info;
SimTK::Matrix performanceMatrixCopy = _performanceMatrix;
SimTK::Vector performanceVectorCopy = _performanceVector;
//cout << "performanceMatrixCopy : " << performanceMatrixCopy << endl;
//cout << "performanceVectorCopy : " << performanceVectorCopy << endl;
//cout << "_constraintMatrix : " << _constraintMatrix << endl;
//cout << "_constraintVector : " << _constraintVector << endl;
//cout << "nf=" << nf << " nacc=" << nacc << endl;
dgglse_(nf, nf, nacc, &performanceMatrixCopy(0,0), nf, &_constraintMatrix(0,0), nacc, &performanceVectorCopy[0], &_constraintVector[0], &f[0], &_lapackWork[0], _lapackWork.size(), info);
// Record inverse dynamics forces
_storage->append(sWorkingCopy.getTime(),nf,&f[0]);
//cout << "\n ** f : " << f << endl << endl;
return 0;
}
/**
* 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(SimTK::State& s, 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());
// 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++){
if(IKCoordinateTask *coordTask = dynamic_cast<IKCoordinateTask *>(&_ikTaskSet[i])){
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("InverseKinematicsTool: value for coordinate "+coordTask->getName()+" not found.");
coordinateReferences.push_back(*coordRef);
}
}
double constraintWeight = std::numeric_limits<SimTK::Real>::infinity();
InverseKinematicsSolver* ikSol = new InverseKinematicsSolver(*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){
totalSquaredMarkerError += squaredMarkerErrors[j];
if(squaredMarkerErrors[j] > maxSquaredMarkerError){
maxSquaredMarkerError = squaredMarkerErrors[j];
worst = j;
}
}
cout << "Frame at (t=" << s.getTime() << "):\t";
cout << "total squared error = " << totalSquaredMarkerError;
cout << ", marker error: RMS=" << sqrt(totalSquaredMarkerError/nm);
cout << ", max=" << sqrt(maxSquaredMarkerError) << " (" << ikSol->getMarkerNameForIndex(worst) << ")" << endl;
/* Now move the non-fixed markers on the model so that they are coincident
* with the measured markers in the static pose. The model is already in
* the proper configuration so the coordinates do not need to be changed.
*/
if(_moveModelMarkers) moveModelMarkersToPose(s, *aModel, staticPose);
if (_outputStorage!= NULL){
delete _outputStorage;
}
// Make a storage file containing the solved states and markers for display in GUI.
Storage motionData;
StatesReporter statesReporter(aModel);
statesReporter.begin(s);
_outputStorage = new Storage(statesReporter.updStatesStorage());
_outputStorage->setName("static pose");
//_outputStorage->print("statesReporterOutput.sto");
Storage markerStorage;
staticPose.makeRdStorage(*_outputStorage);
_outputStorage->getStateVector(0)->setTime(s.getTime());
statesReporter.updStatesStorage().addToRdStorage(*_outputStorage, s.getTime(), s.getTime());
//_outputStorage->print("statesReporterOutputWithMarkers.sto");
if(_printResultFiles) {
if (!_outputModelFileNameProp.getValueIsDefault())
{
aModel->print(aPathToSubject + _outputModelFileName);
cout << "Wrote model file " << _outputModelFileName << " from model " << aModel->getName() << endl;
}
if (!_outputMarkerFileNameProp.getValueIsDefault())
{
aModel->writeMarkerFile(aPathToSubject + _outputMarkerFileName);
cout << "Wrote marker file " << _outputMarkerFileName << " from model " << aModel->getName() << endl;
}
if (!_outputMotionFileNameProp.getValueIsDefault())
{
_outputStorage->print(aPathToSubject + _outputMotionFileName,
"w", "File generated from solving marker data for model "+aModel->getName());
}
}
return true;
}
/**
* 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";
}
}
/**
* Compute and record the results.
*
* This method, for the purpose of example, records the position and
* orientation of each body in the model. You will need to customize it
* to perform your analysis.
*
* @param aT Current time in the simulation.
* @param aX Current values of the controls.
* @param aY Current values of the states: includes generalized coords and speeds
*/
int InducedAccelerations::record(const SimTK::State& s)
{
int nu = _model->getNumSpeeds();
double aT = s.getTime();
cout << "time = " << aT << endl;
SimTK::Vector Q = s.getQ();
// Reset Accelerations for coordinates at this time step
for(int i=0;i<_coordSet.getSize();i++) {
_coordIndAccs[i]->setSize(0);
}
// Reset Accelerations for bodies at this time step
for(int i=0;i<_bodySet.getSize();i++) {
_bodyIndAccs[i]->setSize(0);
}
// Reset Accelerations for system center of mass at this time step
_comIndAccs.setSize(0);
_constraintReactions.setSize(0);
SimTK::State s_analysis = _model->getWorkingState();
_model->initStateWithoutRecreatingSystem(s_analysis);
// Just need to set current time and position to determine state of constraints
s_analysis.setTime(aT);
s_analysis.setQ(Q);
// Check the external forces and determine if contact constraints should be applied at this time
// and turn constraint on if it should be.
Array<bool> constraintOn = applyContactConstraintAccordingToExternalForces(s_analysis);
// Hang on to a state that has the right flags for contact constraints turned on/off
_model->setPropertiesFromState(s_analysis);
// Use this state for the remainder of this step (record)
s_analysis = _model->getMultibodySystem().realizeTopology();
// DO NOT recreate the system, will lose location of constraint
_model->initStateWithoutRecreatingSystem(s_analysis);
// Cycle through the force contributors to the system acceleration
for(int c=0; c< _contributors.getSize(); c++){
//cout << "Solving for contributor: " << _contributors[c] << endl;
// Need to be at the dynamics stage to disable a force
_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Dynamics);
if(_contributors[c] == "total"){
// Set gravity ON
_model->getGravityForce().enable(s_analysis);
//Use same conditions on constraints
s_analysis.setTime(aT);
// Set the configuration (gen. coords and speeds) of the model.
s_analysis.setQ(Q);
s_analysis.setU(s.getU());
s_analysis.setZ(s.getZ());
//Make sure all the actuators are on!
for(int f=0; f<_model->getActuators().getSize(); f++){
_model->updActuators().get(f).setDisabled(s_analysis, false);
}
// Get to the point where we can evaluate unilateral constraint conditions
_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Acceleration);
/* *********************************** ERROR CHECKING *******************************
SimTK::Vec3 pcom =_model->getMultibodySystem().getMatterSubsystem().calcSystemMassCenterLocationInGround(s_analysis);
SimTK::Vec3 vcom =_model->getMultibodySystem().getMatterSubsystem().calcSystemMassCenterVelocityInGround(s_analysis);
SimTK::Vec3 acom =_model->getMultibodySystem().getMatterSubsystem().calcSystemMassCenterAccelerationInGround(s_analysis);
SimTK::Matrix M;
_model->getMultibodySystem().getMatterSubsystem().calcM(s_analysis, M);
cout << "mass matrix: " << M << endl;
SimTK::Inertia sysInertia = _model->getMultibodySystem().getMatterSubsystem().calcSystemCentralInertiaInGround(s_analysis);
cout << "system inertia: " << sysInertia << endl;
SimTK::SpatialVec sysMomentum =_model->getMultibodySystem().getMatterSubsystem().calcSystemMomentumAboutGroundOrigin(s_analysis);
cout << "system momentum: " << sysMomentum << endl;
const SimTK::Vector &appliedMobilityForces = _model->getMultibodySystem().getMobilityForces(s_analysis, SimTK::Stage::Dynamics);
appliedMobilityForces.dump("All Applied Mobility Forces");
// Get all applied body forces like those from conact
const SimTK::Vector_<SimTK::SpatialVec>& appliedBodyForces = _model->getMultibodySystem().getRigidBodyForces(s_analysis, SimTK::Stage::Dynamics);
appliedBodyForces.dump("All Applied Body Forces");
SimTK::Vector ucUdot;
SimTK::Vector_<SimTK::SpatialVec> ucA_GB;
_model->getMultibodySystem().getMatterSubsystem().calcAccelerationIgnoringConstraints(s_analysis, appliedMobilityForces, appliedBodyForces, ucUdot, ucA_GB) ;
ucUdot.dump("Udots Ignoring Constraints");
ucA_GB.dump("Body Accelerations");
SimTK::Vector_<SimTK::SpatialVec> constraintBodyForces(_constraintSet.getSize(), SimTK::SpatialVec(SimTK::Vec3(0)));
SimTK::Vector constraintMobilityForces(0);
int nc = _model->getMultibodySystem().getMatterSubsystem().getNumConstraints();
for (SimTK::ConstraintIndex cx(0); cx < nc; ++cx) {
if (!_model->getMultibodySystem().getMatterSubsystem().isConstraintDisabled(s_analysis, cx)){
cout << "Constraint " << cx << " enabled!" << endl;
}
}
//int nMults = _model->getMultibodySystem().getMatterSubsystem().getTotalMultAlloc();
for(int i=0; i<constraintOn.getSize(); i++) {
if(constraintOn[i])
_constraintSet[i].calcConstraintForces(s_analysis, constraintBodyForces, constraintMobilityForces);
}
constraintBodyForces.dump("Constraint Body Forces");
constraintMobilityForces.dump("Constraint Mobility Forces");
// ******************************* end ERROR CHECKING *******************************/
for(int i=0; i<constraintOn.getSize(); i++) {
_constraintSet.get(i).setDisabled(s_analysis, !constraintOn[i]);
// Make sure we stay at Dynamics so each constraint can evaluate its conditions
_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Acceleration);
}
// This should also push changes to defaults for unilateral conditions
_model->setPropertiesFromState(s_analysis);
}
else if(_contributors[c] == "gravity"){
// Set gravity ON
_model->updForceSubsystem().setForceIsDisabled(s_analysis, _model->getGravityForce().getForceIndex(), false);
//s_analysis = _model->initSystem();
s_analysis.setTime(aT);
s_analysis.setQ(Q);
// zero velocity
s_analysis.setU(SimTK::Vector(nu,0.0));
s_analysis.setZ(s.getZ());
// disable actuator forces
for(int f=0; f<_model->getActuators().getSize(); f++){
_model->updActuators().get(f).setDisabled(s_analysis, true);
}
}
else if(_contributors[c] == "velocity"){
// Set gravity off
_model->updForceSubsystem().setForceIsDisabled(s_analysis, _model->getGravityForce().getForceIndex(), true);
s_analysis.setTime(aT);
s_analysis.setQ(Q);
// non-zero velocity
s_analysis.setU(s.getU());
s_analysis.setZ(s.getZ());
// zero actuator forces
for(int f=0; f<_model->getActuators().getSize(); f++){
_model->updActuators().get(f).setDisabled(s_analysis, true);
}
// Set the configuration (gen. coords and speeds) of the model.
_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Velocity);
}
else{ //The rest are actuators
// Set gravity OFF
_model->updForceSubsystem().setForceIsDisabled(s_analysis, _model->getGravityForce().getForceIndex(), true);
// zero actuator forces
for(int f=0; f<_model->getActuators().getSize(); f++){
_model->updActuators().get(f).setDisabled(s_analysis, true);
}
//s_analysis = _model->initSystem();
s_analysis.setTime(aT);
s_analysis.setQ(Q);
// zero velocity
SimTK::Vector U(nu,0.0);
s_analysis.setU(U);
s_analysis.setZ(s.getZ());
// light up the one actuator who's contribution we are looking for
int ai = _model->getActuators().getIndex(_contributors[c]);
if(ai<0)
throw Exception("InducedAcceleration: ERR- Could not find actuator '"+_contributors[c],__FILE__,__LINE__);
Actuator &actuator = _model->getActuators().get(ai);
actuator.setDisabled(s_analysis, false);
actuator.overrideForce(s_analysis, false);
Muscle *muscle = dynamic_cast<Muscle *>(&actuator);
if(muscle){
if(_computePotentialsOnly){
muscle->overrideForce(s_analysis, true);
muscle->setOverrideForce(s_analysis, 1.0);
}
}
// Set the configuration (gen. coords and speeds) of the model.
_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Model);
_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Velocity);
}// End of if to select contributor
// cout << "Constraint 0 is of "<< _constraintSet[0].getConcreteClassName() << " and should be " << constraintOn[0] << " and is actually " << (_constraintSet[0].isDisabled(s_analysis) ? "off" : "on") << endl;
// cout << "Constraint 1 is of "<< _constraintSet[1].getConcreteClassName() << " and should be " << constraintOn[1] << " and is actually " << (_constraintSet[1].isDisabled(s_analysis) ? "off" : "on") << endl;
// After setting the state of the model and applying forces
// Compute the derivative of the multibody system (speeds and accelerations)
_model->getMultibodySystem().realize(s_analysis, SimTK::Stage::Acceleration);
// Sanity check that constraints hasn't totally changed the configuration of the model
double error = (Q-s_analysis.getQ()).norm();
// Report reaction forces for debugging
/*
SimTK::Vector_<SimTK::SpatialVec> constraintBodyForces(_constraintSet.getSize());
SimTK::Vector mobilityForces(0);
for(int i=0; i<constraintOn.getSize(); i++) {
if(constraintOn[i])
_constraintSet.get(i).calcConstraintForces(s_analysis, constraintBodyForces, mobilityForces);
}*/
// VARIABLES
SimTK::Vec3 vec,angVec;
// Get Accelerations for kinematics of bodies
for(int i=0;i<_coordSet.getSize();i++) {
double acc = _coordSet.get(i).getAccelerationValue(s_analysis);
if(getInDegrees())
acc *= SimTK_RADIAN_TO_DEGREE;
_coordIndAccs[i]->append(1, &acc);
}
// cout << "Input Body Names: "<< _bodyNames << endl;
// Get Accelerations for kinematics of bodies
for(int i=0;i<_bodySet.getSize();i++) {
Body &body = _bodySet.get(i);
// cout << "Body Name: "<< body->getName() << endl;
const SimTK::Vec3& com = body.get_mass_center();
// Get the body acceleration
_model->getSimbodyEngine().getAcceleration(s_analysis, body, com, vec);
_model->getSimbodyEngine().getAngularAcceleration(s_analysis, body, angVec);
// CONVERT TO DEGREES?
if(getInDegrees())
angVec *= SimTK_RADIAN_TO_DEGREE;
// FILL KINEMATICS ARRAY
_bodyIndAccs[i]->append(3, &vec[0]);
_bodyIndAccs[i]->append(3, &angVec[0]);
}
// Get Accelerations for kinematics of COM
if(_includeCOM){
// Get the body acceleration in ground
vec = _model->getMultibodySystem().getMatterSubsystem().calcSystemMassCenterAccelerationInGround(s_analysis);
// FILL KINEMATICS ARRAY
_comIndAccs.append(3, &vec[0]);
}
// Get induced constraint reactions for contributor
if(_reportConstraintReactions){
for(int j=0; j<_constraintSet.getSize(); j++){
_constraintReactions.append(_constraintSet[j].getRecordValues(s_analysis));
}
}
} // End cycling through contributors at this time step
// Set the accelerations of coordinates into their storages
int nc = _coordSet.getSize();
for(int i=0; i<nc; i++) {
_storeInducedAccelerations[i]->append(aT, _coordIndAccs[i]->getSize(),&(_coordIndAccs[i]->get(0)));
}
// Set the accelerations of bodies into their storages
int nb = _bodySet.getSize();
for(int i=0; i<nb; i++) {
_storeInducedAccelerations[nc+i]->append(aT, _bodyIndAccs[i]->getSize(),&(_bodyIndAccs[i]->get(0)));
}
// Set the accelerations of system center of mass into a storage
if(_includeCOM){
_storeInducedAccelerations[nc+nb]->append(aT, _comIndAccs.getSize(), &_comIndAccs[0]);
}
if(_reportConstraintReactions){
_storeConstraintReactions->append(aT, _constraintReactions.getSize(), &_constraintReactions[0]);
}
return(0);
}
int main()
{
try {
int status = 0;
// To Retrace the steps taken by the GUI this test case follows the same call sequence:
// new Model(file)
// new OpenSimContext(model.initSystem(), model);
// context.updateDisplayer(muscle) // to display muscles
// context.getCurrentPath(muscle)
// context.getTransform(body)
// context.transformPosition(body, loc, global) // to display markers
// context.getLocked(Coordinate)
// context.getValue(cooridnate)
LoadOpenSimLibrary("osimActuators");
LoadOpenSimLibrary("osimSimulation");
LoadOpenSimLibrary("osimJavaJNI");
Model *model = new Model("wrist.osim");
OpenSimContext* context = new OpenSimContext(&model->initSystem(), model);
const ForceSet& fs = model->getForceSet();
int n1 = fs.getNumGroups();
const ObjectGroup* grp = fs.getGroup("wrist");
assert(grp);
const Array<const Object*>& members = grp->getMembers();
int sz = members.getSize();
ASSERT_EQUAL(sz,5,0);
assert(members.get(0)->getName()=="ECRB");
delete model;
delete context;
model = new Model("arm26_20.osim");
context = new OpenSimContext(&model->initSystem(), model);
// Make a copy of state contained in context ad make sure content match
SimTK::State stateCopy = context->getCurrentStateCopy();
assert(context->getCurrentStateRef().toString()==stateCopy.toString());
Array<std::string> stateNames = model->getStateVariableNames();
OpenSim::Force* dForce=&(model->updForceSet().get("TRIlong"));
Muscle* dTRIlong = dynamic_cast<Muscle*>(dForce);
assert(dTRIlong);
context->setPropertiesFromState();
OpenSim::Thelen2003Muscle* thelenMsl = dynamic_cast<Thelen2003Muscle*>(dTRIlong);
AbstractProperty& dProp = thelenMsl->updPropertyByName("ignore_tendon_compliance");
PropertyHelper::setValueBool(true, dProp);
cout << "Prop after is " << dProp.toString() << endl;
bool exceptionThrown = false;
try{// adding to the system should cause Muscle that do not handle
// ignore_tendon_compliance to throw an exception
context->recreateSystemKeepStage();
}
catch (const std::exception& e) {
cout << e.what() << endl;
exceptionThrown = true;
PropertyHelper::setValueBool(false, dProp);
cout << "Prop reset to " << dProp.toString() << endl;
// recreate the system so test can continue
context->recreateSystemKeepStage();
}
SimTK_ASSERT_ALWAYS(exceptionThrown, "Setting ignore_tendon_compliance must throw an exception.");
AbstractProperty& dProp2 = thelenMsl->updPropertyByName("ignore_tendon_compliance");
cout << "Prop after create system is " << dProp2.toString() << endl;
bool after = PropertyHelper::getValueBool(dProp);
SimTK_ASSERT_ALWAYS(!after, "Property has wrong value!!");
dTRIlong->updGeometryPath().updateGeometry(context->getCurrentStateRef());
const OpenSim::Array<PathPoint*>& path = context->getCurrentPath(*dTRIlong);
cout << "Muscle Path" << endl;
cout << path.getSize() << endl;
for(int i=0; i< path.getSize(); i++)
cout << path.get(i)->getBodyName() << path.get(i)->getLocation() << endl;
// Compare to known path
const OpenSim::Body& dBody = model->getBodySet().get("r_ulna_radius_hand");
Transform xform = context->getTransform(dBody);
cout << xform << endl;
double flat[16];
context->getTransformAsDouble16(xform, flat);
// Compare to known xform
double markerPosition[] = {.005000000000, -0.290400000000, 0.030000000000};
double markerPositionInGround[3];
context->transformPosition(dBody, markerPosition, markerPositionInGround); // to display markers
cout << "Global frame position = " << markerPositionInGround[0] <<
markerPositionInGround[1] << markerPositionInGround[2]<< endl;
// Check xformed point against known position
const Coordinate& dr_elbow_flex = model->getCoordinateSet().get("r_elbow_flex");
bool isLocked = context->getLocked(dr_elbow_flex);
assert(!isLocked);
double startValue = context->getValue(dr_elbow_flex);
cout << "Coordinate start value = " << startValue << endl;
double length1 = context->getMuscleLength(*dTRIlong);
cout << length1 << endl;
ASSERT_EQUAL(.277609, length1, 1e-5);
// Coordinate Slider
context->setValue(dr_elbow_flex, 100*SimTK_PI/180.);
// Get body transform, marker position and muscle path (tests wrapping as well)
xform = context->getTransform(dBody);
cout << "After setting coordinate to 100 deg." << endl;
cout << xform << endl;
// Compare to known xform
dTRIlong->updGeometryPath().updateGeometry(context->getCurrentStateRef());
const OpenSim::Array<PathPoint*>& newPath = context->getCurrentPath(*dTRIlong);
// Compare to known path
cout << "New Muscle Path" << endl;
cout << path.getSize() << endl;
for(int i=0; i< path.getSize(); i++)
cout << path.get(i)->getBodyName() << path.get(i)->getLocation() << endl;
double length2 = context->getMuscleLength(*dTRIlong);
cout << length2 << endl;
ASSERT_EQUAL(.315748, length2, 1e-5);
// Test that we can lock coordinates to specific value and make this persistant.
Coordinate& dr_elbow_flex_mod = model->updCoordinateSet().get("r_elbow_flex");
//dr_elbow_flex_mod.setDefaultValue(0.5);
dr_elbow_flex_mod.setDefaultLocked(true);
context->setValue(dr_elbow_flex_mod, 0.5);
//model->print("wrist_locked_elbow.osim");
context->recreateSystemKeepStage();
const Coordinate& dr_elbow_flexNew = model->getCoordinateSet().get("r_elbow_flex");
assert(context->getLocked(dr_elbow_flexNew));
ASSERT_EQUAL(0.5, context->getValue(dr_elbow_flexNew), 0.000001);
return status;
} catch (const std::exception& e) {
cout << "Exception: " << e.what() << endl;
return 1;
}
}
/**
* Record the kinematics.
*/
int BodyKinematics::
record(const SimTK::State& s)
{
// Realize to Acceleration first since we'll ask for Accelerations
_model->getMultibodySystem().realize(s, SimTK::Stage::Acceleration);
// VARIABLES
double dirCos[3][3];
SimTK::Vec3 vec,angVec;
double Mass = 0.0;
// GROUND BODY
const Ground &ground = _model->getGround();
// POSITION
BodySet& bs = _model->updBodySet();
for(int i=0;i<_bodyIndices.getSize();i++) {
Body& body = bs.get(_bodyIndices[i]);
const SimTK::Vec3& com = body.get_mass_center();
// GET POSITIONS AND EULER ANGLES
_model->getSimbodyEngine().getPosition(s, body,com,vec);
_model->getSimbodyEngine().getDirectionCosines(s, body,dirCos);
_model->getSimbodyEngine().convertDirectionCosinesToAngles(dirCos,
&angVec[0],&angVec[1],&angVec[2]);
// CONVERT TO DEGREES?
if(getInDegrees()) {
angVec[0] *= SimTK_RADIAN_TO_DEGREE;
angVec[1] *= SimTK_RADIAN_TO_DEGREE;
angVec[2] *= SimTK_RADIAN_TO_DEGREE;
}
// FILL KINEMATICS ARRAY
int I=6*i;
memcpy(&_kin[I],&vec[0],3*sizeof(double));
memcpy(&_kin[I+3],&angVec[0],3*sizeof(double));
}
if(_recordCenterOfMass) {
double rP[3] = { 0.0, 0.0, 0.0 };
for(int i=0;i<bs.getSize();i++) {
Body& body = bs.get(i);
const SimTK::Vec3& com = body.get_mass_center();
_model->getSimbodyEngine().getPosition(s, body,com,vec);
// ADD TO WHOLE BODY MASS
Mass += body.get_mass();
rP[0] += body.get_mass() * vec[0];
rP[1] += body.get_mass() * vec[1];
rP[2] += body.get_mass() * vec[2];
}
//COMPUTE COM OF WHOLE BODY AND ADD TO ARRAY
rP[0] /= Mass;
rP[1] /= Mass;
rP[2] /= Mass;
int I = 6*_bodyIndices.getSize();
memcpy(&_kin[I],rP,3*sizeof(double));
}
_pStore->append(s.getTime(),_kin.getSize(),&_kin[0]);
// VELOCITY
for(int i=0;i<_bodyIndices.getSize();i++) {
Body& body = bs.get(_bodyIndices[i]);
const SimTK::Vec3& com = body.get_mass_center();
// GET VELOCITIES AND ANGULAR VELOCITIES
_model->getSimbodyEngine().getVelocity(s, body,com,vec);
if(_expressInLocalFrame) {
_model->getSimbodyEngine().transform(s, ground,vec,body,vec);
_model->getSimbodyEngine().getAngularVelocityBodyLocal(s, body,angVec);
} else {
_model->getSimbodyEngine().getAngularVelocity(s, body,angVec);
}
// CONVERT TO DEGREES?
if(getInDegrees()) {
angVec[0] *= SimTK_RADIAN_TO_DEGREE;
angVec[1] *= SimTK_RADIAN_TO_DEGREE;
angVec[2] *= SimTK_RADIAN_TO_DEGREE;
}
// FILL KINEMATICS ARRAY
int I = 6*i;
memcpy(&_kin[I],&vec[0],3*sizeof(double));
memcpy(&_kin[I+3],&angVec[0],3*sizeof(double));
}
if(_recordCenterOfMass) {
double rV[3] = { 0.0, 0.0, 0.0 };
for(int i=0;i<bs.getSize();i++) {
Body& body = bs.get(i);
const SimTK::Vec3& com = body.get_mass_center();
_model->getSimbodyEngine().getVelocity(s, body,com,vec);
rV[0] += body.get_mass() * vec[0];
rV[1] += body.get_mass() * vec[1];
rV[2] += body.get_mass() * vec[2];
}
//COMPUTE VELOCITY OF COM OF WHOLE BODY AND ADD TO ARRAY
rV[0] /= Mass;
rV[1] /= Mass;
rV[2] /= Mass;
int I = 6*_bodyIndices.getSize();
memcpy(&_kin[I],rV,3*sizeof(double));
}
_vStore->append(s.getTime(),_kin.getSize(),&_kin[0]);
// ACCELERATIONS
for(int i=0;i<_bodyIndices.getSize();i++) {
Body& body = bs.get(_bodyIndices[i]);
const SimTK::Vec3& com = body.get_mass_center();
// GET ACCELERATIONS AND ANGULAR ACCELERATIONS
_model->getSimbodyEngine().getAcceleration(s, body,com,vec);
if(_expressInLocalFrame) {
_model->getSimbodyEngine().transform(s, ground,vec,body,vec);
_model->getSimbodyEngine().getAngularAccelerationBodyLocal(s, body,angVec);
} else {
_model->getSimbodyEngine().getAngularAcceleration(s, body,angVec);
}
// CONVERT TO DEGREES?
if(getInDegrees()) {
angVec[0] *= SimTK_RADIAN_TO_DEGREE;
angVec[1] *= SimTK_RADIAN_TO_DEGREE;
angVec[2] *= SimTK_RADIAN_TO_DEGREE;
}
// FILL KINEMATICS ARRAY
int I = 6*i;
memcpy(&_kin[I],&vec[0],3*sizeof(double));
memcpy(&_kin[I+3],&angVec[0],3*sizeof(double));
}
if(_recordCenterOfMass) {
double rA[3] = { 0.0, 0.0, 0.0 };
for(int i=0;i<bs.getSize();i++) {
Body& body = bs.get(i);
const SimTK::Vec3& com = body.get_mass_center();
_model->getSimbodyEngine().getAcceleration(s, body,com,vec);
rA[0] += body.get_mass() * vec[0];
rA[1] += body.get_mass() * vec[1];
rA[2] += body.get_mass() * vec[2];
}
//COMPUTE ACCELERATION OF COM OF WHOLE BODY AND ADD TO ARRAY
rA[0] /= Mass;
rA[1] /= Mass;
rA[2] /= Mass;
int I = 6*_bodyIndices.getSize();
memcpy(&_kin[I],rA,3*sizeof(double));
}
_aStore->append(s.getTime(),_kin.getSize(),&_kin[0]);
//printf("BodyKinematics:\taT:\t%.16f\trA[1]:\t%.16f\n",s.getTime(),rA[1]);
return(0);
}
SimTK::Vector getQ(const SimTK::State& state) const {
return state.getQ();
}
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 computeStateVariableDerivatives(const SimTK::State& s) const override {
double deriv = exp(-2.0*s.getTime());
setStateVariableDerivativeValue(s, "subState", deriv);
}
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;
}
/** get the acceleration value of the CoordinateReference */
double CoordinateReference::getAccelerationValue(const SimTK::State &s) const
{
SimTK::Vector t(1, s.getTime());
std::vector<int> order(2, 0);
return _coordinateValueFunction->calcDerivative(order, t);
}
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();
}
/**
* From a potentially partial specification of the generalized coordinates,
* form a complete storage of the generalized coordinates (q's) and
* generalized speeds (u's).
*
* @param aQIn Storage containing the q's or a subset of the q's. Rotational
* q's should be in degrees.
* @param rQComplete Storage containing all the q's. If q's were not
* in aQIn, the values are set to 0.0. When a q is constrained, its value
* is altered to be consistent with the constraint. The caller is responsible
* for deleting the memory associated with this storage.
* @param rUComplete Storage containing all the u's. The generalized speeds
* are obtained by spline fitting the q's and differentiating the splines.
* When a u is constrained, its value is altered to be consistent with the
* constraint. The caller is responsible for deleting the memory
* associated with this storage.
*/
void SimbodyEngine::
formCompleteStorages( const SimTK::State& s, const OpenSim::Storage &aQIn,
OpenSim::Storage *&rQComplete,OpenSim::Storage *&rUComplete) const
{
int i;
int nq = _model->getNumCoordinates();
int nu = _model->getNumSpeeds();
// Get coordinate file indices
Array<string> columnLabels, speedLabels, coordStateNames;
columnLabels.append("time");
speedLabels = columnLabels;
Array<int> index(-1,nq);
const CoordinateSet& coordinateSet = _model->getCoordinateSet();
int sizeCoordSet = coordinateSet.getSize();
for(i=0;i<sizeCoordSet;i++) {
Coordinate& coord = coordinateSet.get(i);
string prefix = coord.getJoint().getName() + "/" + coord.getName() + "/";
coordStateNames = coord.getStateVariableNames();
columnLabels.append(prefix+coordStateNames[0]);
speedLabels.append(prefix+coordStateNames[1]);
int fix = aQIn.getStateIndex(coord.getName());
if (fix < 0) {
fix = aQIn.getStateIndex(columnLabels[i+1]);
}
index[i] = fix;
if(index[i]<0) {
string msg = "Model::formCompleteStorages(): WARNING- Did not find column ";
msg += coordStateNames[0];
msg += " in storage object.\n";
cout << msg << endl;
}
}
// Extract Coordinates
double time;
Array<double> data(0.0);
Array<double> q(0.0,nq);
Storage *qStore = new Storage();
qStore->setInDegrees(aQIn.isInDegrees());
qStore->setName("GeneralizedCoordinates");
qStore->setColumnLabels(columnLabels);
int size = aQIn.getSize();
StateVector *vector;
int j;
for(i=0;i<size;i++) {
vector = aQIn.getStateVector(i);
data = vector->getData();
time = vector->getTime();
for(j=0;j<nq;j++) {
q[j] = 0.0;
if(index[j]<0) continue;
q[j] = data[index[j]];
}
qStore->append(time,nq,&q[0]);
}
// Convert to radians
if (aQIn.isInDegrees())
convertDegreesToRadians(*qStore);
// Compute generalized speeds
GCVSplineSet tempQset(5,qStore);
Storage *uStore = tempQset.constructStorage(1);
// Compute constraints
Array<double> qu(0.0,nq+nu);
rQComplete = new Storage();
rUComplete = new Storage();
State constrainedState = s;
_model->getMultibodySystem().realize(constrainedState, s.getSystemStage());
for(i=0;i<size;i++) {
qStore->getTime(i,time);
qStore->getData(i,nq,&qu[0]);
uStore->getData(i,nq,&qu[nq]);
for (int j = 0; j < nq; j++) {
Coordinate& coord = coordinateSet.get(j);
coord.setValue(constrainedState, qu[j], false);
coord.setSpeedValue(constrainedState, qu[nq+j]);
}
_model->assemble(constrainedState);
for (int j = 0; j < nq; j++) {
Coordinate& coord = coordinateSet.get(j);
qu[j] = coord.getValue(constrainedState);
qu[nq+j] = coord.getSpeedValue(constrainedState);
}
rQComplete->append(time,nq,&qu[0]);
rUComplete->append(time,nu,&qu[nq]);
}
delete qStore;
// Compute storage object for simulation
// Need to set column labels before converting rad->deg
rQComplete->setColumnLabels(columnLabels);
rUComplete->setColumnLabels(speedLabels);
// Convert back to degrees
convertRadiansToDegrees(*rQComplete);
convertRadiansToDegrees(*rUComplete);
}
void handleEvent (SimTK::State& s, Real accuracy, bool& terminate) const override {
terminate = false;
_controller->computeControls( s, _controller->updControlSet() );
_controller->setTargetTime(s.getTime() + _controller->getTargetDT());
}
/**
* This method is called at the beginning of an analysis so that any
* necessary initializations may be performed.
*
* This method should be overridden in the child class. It is
* included here so that the child class will not have to implement it if it
* is not necessary.
*
* @param s state of system
*
* @return -1 on error, 0 otherwise.
*/
int InverseDynamics::begin(const SimTK::State& s )
{
if(!proceed()) return(0);
SimTK::Matrix massMatrix;
_model->getMatterSubsystem().calcM(s, massMatrix);
//massMatrix.dump("mass matrix is:");
// Check that m is full rank
try {
SimTK::FactorLU lu(massMatrix);
} catch (const SimTK::Exception::Base&) {
throw Exception("InverseDynamics: ERROR- mass matrix is singular ",__FILE__,__LINE__);
}
// enable the line below when simmath is fixed
//bool singularMassMatrix = lu.isSingular();
//cout << "Mass matrix is " << (singularMassMatrix?"singular":"Non-singular");
// Make a working copy of the model
delete _modelWorkingCopy;
_modelWorkingCopy = _model->clone();
_modelWorkingCopy->updAnalysisSet().setSize(0);
// Replace model force set with only generalized forces
if(_model) {
SimTK::State& sWorkingCopyTemp = _modelWorkingCopy->initSystem();
// Update the _forceSet we'll be computing inverse dynamics for
if(_ownsForceSet) delete _forceSet;
if(_useModelForceSet) {
// Set pointer to model's internal force set
_forceSet = &_modelWorkingCopy->updForceSet();
_numCoordinateActuators = _modelWorkingCopy->getActuators().getSize();
} else {
ForceSet& as = _modelWorkingCopy->updForceSet();
// Keep a copy of forces that are not muscles to restore them back.
ForceSet* saveForces = as.clone();
// Generate an force set consisting of a coordinate actuator for every unconstrained degree of freedom
_forceSet = CoordinateActuator::CreateForceSetOfCoordinateActuatorsForModel(sWorkingCopyTemp,*_modelWorkingCopy,1,false);
_numCoordinateActuators = _forceSet->getSize();
// Copy whatever forces that are not muscles back into the model
for(int i=0; i<saveForces->getSize(); i++){
// const Force& f=saveForces->get(i);
if ((dynamic_cast<const Muscle*>(&saveForces->get(i)))==NULL)
as.append(saveForces->get(i).clone());
}
}
_modelWorkingCopy->setAllControllersEnabled(false);
_ownsForceSet = false;
SimTK::State& sWorkingCopy = _modelWorkingCopy->initSystem();
// Gather indices into speed set corresponding to the unconstrained degrees of freedom (for which we will set acceleration constraints)
_accelerationIndices.setSize(0);
auto coordinates = _modelWorkingCopy->getCoordinatesInMultibodyTreeOrder();
for(size_t i=0u; i<coordinates.size(); ++i) {
const Coordinate& coord = *coordinates[i];
if(!coord.isConstrained(sWorkingCopy)) {
_accelerationIndices.append(static_cast<int>(i));
}
}
_dydt.setSize(_modelWorkingCopy->getNumCoordinates() + _modelWorkingCopy->getNumSpeeds());
int nf = _numCoordinateActuators;
int nacc = _accelerationIndices.getSize();
if(nf < nacc)
throw(Exception("InverseDynamics: ERROR- over-constrained system -- need at least as many forces as there are degrees of freedom.\n"));
// Realize to velocity in case there are any velocity dependent forces
_modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Velocity);
_constraintMatrix.resize(nacc,nf);
_constraintVector.resize(nacc);
_performanceMatrix.resize(nf,nf);
_performanceMatrix = 0;
for(int i=0,j=0; i<nf; i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->get(i));
if( act ) {
act->setActuation(sWorkingCopy, 1);
_performanceMatrix(j,j) = act->getStress(sWorkingCopy);
j++;
}
}
_performanceVector.resize(nf);
_performanceVector = 0;
int lwork = nf + nf + nacc;
_lapackWork.resize(lwork);
}
_statesSplineSet=GCVSplineSet(5,_statesStore);
// DESCRIPTION AND LABELS
constructDescription();
constructColumnLabels();
deleteStorage();
allocateStorage();
// RESET STORAGE
_storage->reset(s.getTime());
// RECORD
int status = 0;
if(_storage->getSize()<=0) {
status = record(s);
}
return(status);
}
/**
* Record the MuscleAnalysis quantities.
*/
int MuscleAnalysis::record(const SimTK::State& s)
{
if(_model==NULL) return -1;
if (!getOn()) return -1;
// MAKE SURE ALL ACTUATION QUANTITIES ARE VALID
// COMPUTE DERIVATIVES
// ----------------------------------
// TIME NORMALIZATION
double tReal = s.getTime();
// ----------------------------------
// LOOP THROUGH MUSCLES
int nm = _muscleArray.getSize();
double nan = SimTK::NaN;
// Angles and lengths
Array<double> penang(nan,nm);
Array<double> len(nan,nm), tlen(nan,nm);
Array<double> fiblen(nan,nm), normfiblen(nan,nm);
// Muscle velocity information
Array<double> fibVel(nan,nm), normFibVel(nan,nm);
Array<double> penAngVel(nan,nm);
// Muscle component forces
Array<double> force(nan, nm), fibforce(nan, nm);
Array<double> actfibforce(nan,nm), passfibforce(nan,nm);
Array<double> actfibforcealongten(nan,nm), passfibforcealongten(nan,nm);
// Muscle and component powers
Array<double> fibActivePower(nan,nm), fibPassivePower(nan,nm),
tendonPower(nan,nm), muscPower(nan,nm);
double sysMass = _model->getMatterSubsystem().calcSystemMass(s);
bool hasMass = sysMass > SimTK::Eps;
// Just warn once per instant
bool lengthWarning = false;
bool forceWarning = false;
bool dynamicsWarning = false;
for(int i=0; i<nm; ++i) {
try{
len[i] = _muscleArray[i]->getLength(s);
tlen[i] = _muscleArray[i]->getTendonLength(s);
fiblen[i] = _muscleArray[i]->getFiberLength(s);
normfiblen[i] = _muscleArray[i]->getNormalizedFiberLength(s);
penang[i] = _muscleArray[i]->getPennationAngle(s);
}
catch (const std::exception& e) {
if(!lengthWarning){
cout << "WARNING- MuscleAnalysis::record() unable to evaluate ";
cout << "muscle length at time " << s.getTime() << " for reason: ";
cout << e.what() << endl;
lengthWarning = true;
}
continue;
}
try{
// Compute muscle forces that are dependent on Positions, Velocities
// so that later quantities are valid and setForce is called
_muscleArray[i]->computeActuation(s);
force[i] = _muscleArray[i]->getActuation(s);
fibforce[i] = _muscleArray[i]->getFiberForce(s);
actfibforce[i] = _muscleArray[i]->getActiveFiberForce(s);
passfibforce[i] = _muscleArray[i]->getPassiveFiberForce(s);
actfibforcealongten[i] = _muscleArray[i]->getActiveFiberForceAlongTendon(s);
passfibforcealongten[i] = _muscleArray[i]->getPassiveFiberForceAlongTendon(s);
}
catch (const std::exception& e) {
if(!forceWarning){
cout << "WARNING- MuscleAnalysis::record() unable to evaluate ";
cout << "muscle forces at time " << s.getTime() << " for reason: ";
cout << e.what() << endl;
forceWarning = true;
}
continue;
}
}
// Cannot compute system dynamics without mass
if(hasMass){
// state derivatives (activation rate and fiber velocity) evaluated at dynamics
_model->getMultibodySystem().realize(s,SimTK::Stage::Dynamics);
for(int i=0; i<nm; ++i) {
try{
//Velocities
fibVel[i] = _muscleArray[i]->getFiberVelocity(s);
normFibVel[i] = _muscleArray[i]->getNormalizedFiberVelocity(s);
penAngVel[i] = _muscleArray[i]->getPennationAngularVelocity(s);
//Powers
fibActivePower[i] = _muscleArray[i]->getFiberActivePower(s);
fibPassivePower[i] = _muscleArray[i]->getFiberPassivePower(s);
tendonPower[i] = _muscleArray[i]->getTendonPower(s);
muscPower[i] = _muscleArray[i]->getMusclePower(s);
}
catch (const std::exception& e) {
if(!dynamicsWarning){
cout << "WARNING- MuscleAnalysis::record() unable to evaluate ";
cout << "muscle forces at time " << s.getTime() << " for reason: ";
cout << e.what() << endl;
dynamicsWarning = true;
}
continue;
}
}
}
else {
if(!dynamicsWarning){
cout << "WARNING- MuscleAnalysis::record() unable to evaluate ";
cout << "muscle dynamics at time " << s.getTime() << " because ";
cout << "model has no mass and system dynamics cannot be computed." << endl;
dynamicsWarning = true;
}
}
// APPEND TO STORAGE
_pennationAngleStore->append(tReal,penang.getSize(),&penang[0]);
_lengthStore->append(tReal,len.getSize(),&len[0]);
_fiberLengthStore->append(tReal,fiblen.getSize(),&fiblen[0]);
_normalizedFiberLengthStore
->append(tReal,normfiblen.getSize(),&normfiblen[0]);
_tendonLengthStore->append(tReal,tlen.getSize(),&tlen[0]);
_fiberVelocityStore->append(tReal,fibVel.getSize(),&fibVel[0]);
_normFiberVelocityStore->append(tReal,normFibVel.getSize(),&normFibVel[0]);
_pennationAngularVelocityStore
->append(tReal,penAngVel.getSize(),&penAngVel[0]);
_forceStore->append(tReal,force.getSize(),&force[0]);
_fiberForceStore->append(tReal,fibforce.getSize(),&fibforce[0]);
_activeFiberForceStore->append(tReal,actfibforce.getSize(),&actfibforce[0]);
_passiveFiberForceStore
->append(tReal,passfibforce.getSize(),&passfibforce[0]);
_activeFiberForceAlongTendonStore
->append(tReal,actfibforcealongten.getSize(),&actfibforcealongten[0]);
_passiveFiberForceAlongTendonStore
->append(tReal,passfibforcealongten.getSize(),&passfibforcealongten[0]);
_fiberActivePowerStore
->append(tReal,fibActivePower.getSize(),&fibActivePower[0]);
_fiberPassivePowerStore
->append(tReal,fibPassivePower.getSize(),&fibPassivePower[0]);
_tendonPowerStore->append(tReal,tendonPower.getSize(),&tendonPower[0]);
_musclePowerStore->append(tReal,muscPower.getSize(),&muscPower[0]);
if (_computeMoments){
// LOOP OVER ACTIVE MOMENT ARM STORAGE OBJECTS
Coordinate *q = NULL;
Storage *maStore=NULL, *mStore=NULL;
int nq = _momentArmStorageArray.getSize();
Array<double> ma(0.0,nm),m(0.0,nm);
for(int i=0; i<nq; i++) {
q = _momentArmStorageArray[i]->q;
maStore = _momentArmStorageArray[i]->momentArmStore;
mStore = _momentArmStorageArray[i]->momentStore;
// bool locked = q->getLocked(s);
_model->getMultibodySystem().realize(s, s.getSystemStage());
// LOOP OVER MUSCLES
for(int j=0; j<nm; j++) {
ma[j] = _muscleArray[j]->computeMomentArm(s,*q);
m[j] = ma[j] * force[j];
}
maStore->append(s.getTime(),nm,&ma[0]);
mStore->append(s.getTime(),nm,&m[0]);
}
}
return 0;
}
