/**
* 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);
}
/**
* Assemble the model such that it satisfies configuration goals and constraints
* The input state is used to initialize the assembly and then is updated to
* return the resulting assembled configuration.
*/
void AssemblySolver::assemble(SimTK::State &state)
{
// Make a working copy of the state that will be used to set the internal
// state of the solver. This is necessary because we may wish to disable
// redundant constraints, but do not want this to effect the state of
// constraints the user expects
SimTK::State s = state;
// Make sure goals are up-to-date.
setupGoals(s);
// Let assembler perform some internal setup
_assembler->initialize(s);
/* TODO: Useful to include through debug message/log in the future
printf("UNASSEMBLED CONFIGURATION (normerr=%g, maxerr=%g, cost=%g)\n",
_assembler->calcCurrentErrorNorm(),
max(abs(_assembler->getInternalState().getQErr())),
_assembler->calcCurrentGoal());
cout << "Model numQs: " << _assembler->getInternalState().getNQ()
<< " Assembler num freeQs: " << _assembler->getNumFreeQs() << endl;
*/
try{
// Now do the assembly and return the updated state.
_assembler->assemble();
// Update the q's in the state passed in
_assembler->updateFromInternalState(s);
state.updQ() = s.getQ();
state.updU() = s.getU();
// Get model coordinates
const CoordinateSet& modelCoordSet = getModel().getCoordinateSet();
// Make sure the locks in original state are restored
for(int i=0; i< modelCoordSet.getSize(); ++i){
bool isLocked = modelCoordSet[i].getLocked(state);
if(isLocked)
modelCoordSet[i].setLocked(state, isLocked);
}
/* TODO: Useful to include through debug message/log in the future
printf("ASSEMBLED CONFIGURATION (acc=%g tol=%g normerr=%g, maxerr=%g, cost=%g)\n",
_assembler->getAccuracyInUse(), _assembler->getErrorToleranceInUse(),
_assembler->calcCurrentErrorNorm(), max(abs(_assembler->getInternalState().getQErr())),
_assembler->calcCurrentGoal());
printf("# initializations=%d\n", _assembler->getNumInitializations());
printf("# assembly steps: %d\n", _assembler->getNumAssemblySteps());
printf(" evals: goal=%d grad=%d error=%d jac=%d\n",
_assembler->getNumGoalEvals(), _assembler->getNumGoalGradientEvals(),
_assembler->getNumErrorEvals(), _assembler->getNumErrorJacobianEvals());
*/
}
catch (const std::exception& ex)
{
std::string msg = "AssemblySolver::assemble() Failed: ";
msg += ex.what();
throw Exception(msg);
}
}
void IKSolverParallel::pushState(const SimTK::State& s) {
GeneralisedCoordinatesData currentData(nCoordinates_);
std::vector<double> q(nCoordinates_);
SimTK::Vector stateQ(s.getQ());
for (unsigned i(0); i < nCoordinates_; ++i)
q[i] = stateQ[i];
currentData.setQ(q);
outputGeneralisedCoordinatesQueue_.push({ s.getTime(), currentData });
}
bool IKSolverParallel::isWithinRom(const SimTK::State& s) const {
bool isInRom(true);
auto q(s.getQ());
for (unsigned i(0); i < nCoordinates_; ++i) {
auto rangeMax(model_.getCoordinateSet().get(i).getRangeMax());
auto rangeMin(model_.getCoordinateSet().get(i).getRangeMin());
if (q[i] > rangeMax || q[i] < rangeMin) {
isInRom = false;
std::cout << coordinateNames_[i] << " is outside its range of motion" << std::endl;
}
}
return isInRom;
}
/**
* 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;
}
SimTK::Vector getQ(const SimTK::State& state) const {
return state.getQ();
}
/**
* 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);
}
/**
* 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;
}
/**
* 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);
}