//==============================================================================
// CONSTRUCTION AND DESTRUCTION
//==============================================================================
bool ActuatorForceTargetFast::
prepareToOptimize(SimTK::State& s, double *x)
{
// Keep around a "copy" of the state so we can use it in objective function
// in cases where we're tracking states
_saveState = s;
#ifdef USE_LINEAR_CONSTRAINT_MATRIX
int nf = _controller->getActuatorSet().getSize();
int nc = getNumConstraints();
_constraintMatrix.resize(nc,nf);
_constraintVector.resize(nc);
Vector f(nf), c(nc);
// Build linear constraint matrix and constant constraint vector
f = 0;
computeConstraintVector(s, f, _constraintVector);
for(int j=0; j<nf; j++) {
f[j] = 1;
computeConstraintVector(s, f, c);
_constraintMatrix(j) = (c - _constraintVector);
f[j] = 0;
}
#endif
// use temporary copy of state because computeIsokineticForceAssumingInfinitelyStiffTendon
// will change the muscle states. This is necessary ONLY in the case of deprecated muscles
SimTK::State tempState = s;
double activation = 1.0;
getController()->getModel().getMultibodySystem().realize( tempState, SimTK::Stage::Dynamics );
// COMPUTE MAX ISOMETRIC FORCE
const Set<Actuator>& fSet = _controller->getActuatorSet();
double fOpt = SimTK::NaN;
getController()->getModel().getMultibodySystem().realize(tempState, SimTK::Stage::Dynamics );
for(int i=0 ; i<fSet.getSize(); ++i) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
Muscle* mus = dynamic_cast<Muscle*>(act);
if(mus==NULL) {
fOpt = act->getOptimalForce();
}
else{
fOpt = mus->calcInextensibleTendonActiveFiberForce(tempState,
activation);
}
if( std::fabs(fOpt) < SimTK::TinyReal )
fOpt = SimTK::TinyReal;
_recipOptForceSquared[i] = 1.0 / (fOpt*fOpt);
}
// return false to indicate that we still need to proceed with optimization (did not do a lapack direct solve)
return false;
}
/**
* Compute all constraints given x.
*/
void ActuatorForceTargetFast::
computeConstraintVector(SimTK::State& s, const Vector &x,Vector &c) const
{
CMC_TaskSet& taskSet = _controller->updTaskSet();
const Set<Actuator>& fSet = _controller->getActuatorSet();
int nf = fSet.getSize();
// Now override the actuator forces with computed active force
// (from static optimization) but also include the passive force
// contribution of muscles when applying forces to the model
for(int i=0;i<nf;i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
act->overrideActuation(s, true);
act->setOverrideActuation(s, x[i]);
}
_controller->getModel().getMultibodySystem().realize(s, SimTK::Stage::Acceleration );
taskSet.computeAccelerations(s);
Array<double> &w = taskSet.getWeights();
Array<double> &aDes = taskSet.getDesiredAccelerations();
Array<double> &a = taskSet.getAccelerations();
// CONSTRAINTS
for(int i=0; i<getNumConstraints(); i++)
c[i]=w[i]*(aDes[i]-a[i]);
// reset the actuator control
for(int i=0;i<fSet.getSize();i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
act->overrideActuation(s, false);
}
_controller->getModel().getMultibodySystem().realizeModel(s);
}
//=============================================================================
// ACCELERATION
//=============================================================================
//
void StaticOptimizationTarget::
computeAcceleration(SimTK::State& s, const SimTK::Vector ¶meters,SimTK::Vector &rAccel) const
{
double time = s.getTime();
const ForceSet& fs = _model->getForceSet();
for(int i=0,j=0;i<fs.getSize();i++) {
ScalarActuator *act = dynamic_cast<ScalarActuator*>(&fs.get(i));
if( act ) {
act->setOverrideActuation(s, parameters[j] * _optimalForce[j]);
j++;
}
}
_model->getMultibodySystem().realize(s,SimTK::Stage::Acceleration);
SimTK::Vector udot = _model->getMatterSubsystem().getUDot(s);
for(int i=0; i<_accelerationIndices.getSize(); i++)
rAccel[i] = udot[_accelerationIndices[i]];
//QueryPerformanceCounter(&stop);
//double duration = (double)(stop.QuadPart-start.QuadPart)/(double)frequency.QuadPart;
//std::cout << "computeAcceleration time = " << (duration*1.0e3) << " milliseconds" << std::endl;
// 1.45 ms
}
/**
* Get the names of the states of the actuators.
*
* @param rNames Array of names.
*/
void ForceSet::
getStateVariableNames(OpenSim::Array<std::string> &rNames) const
{
for(int i=0;i<getSize();i++) {
ScalarActuator *act = dynamic_cast<ScalarActuator*>(&get(i));
if(act) {
rNames.append(act->getStateVariableNames());
}
}
}
/**
* Get an optimal force.
*/
void StaticOptimizationTarget::
getActuation(SimTK::State& s, const SimTK::Vector ¶meters, SimTK::Vector &forces)
{
//return(_optimalForce[aIndex]);
const ForceSet& fs = _model->getForceSet();
SimTK::Vector tempAccel(getNumConstraints());
computeAcceleration(s, parameters, tempAccel);
for(int i=0,j=0;i<fs.getSize();i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fs.get(i));
if( act )forces(j++) = act->getActuation(s);
}
}
//==============================================================================
// CONSTRUCTION
//==============================================================================
bool StaticOptimizationTarget::
prepareToOptimize(SimTK::State& s, double *x)
{
// COMPUTE MAX ISOMETRIC FORCE
const ForceSet& fSet = _model->getForceSet();
for(int i=0, j=0;i<fSet.getSize();i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet.get(i));
if( act ) {
double fOpt;
Muscle *mus = dynamic_cast<Muscle*>(&fSet.get(i));
if( mus ) {
//ActivationFiberLengthMuscle *aflmus = dynamic_cast<ActivationFiberLengthMuscle*>(mus);
if(mus && _useMusclePhysiology) {
_model->setAllControllersEnabled(true);
fOpt = mus->calcInextensibleTendonActiveFiberForce(s, 1.0);
_model->setAllControllersEnabled(false);
} else {
fOpt = mus->getMaxIsometricForce();
}
} else {
fOpt = act->getOptimalForce();
}
_optimalForce[j++] = fOpt;
}
}
#ifdef USE_LINEAR_CONSTRAINT_MATRIX
//cout<<"Computing linear constraint matrix..."<<endl;
int np = getNumParameters();
int nc = getNumConstraints();
_constraintMatrix.resize(nc,np);
_constraintVector.resize(nc);
Vector pVector(np), cVector(nc);
// Build linear constraint matrix and constant constraint vector
pVector = 0;
computeConstraintVector(s, pVector,_constraintVector);
for(int p=0; p<np; p++) {
pVector[p] = 1;
computeConstraintVector(s, pVector, cVector);
for(int c=0; c<nc; c++) _constraintMatrix(c,p) = (cVector[c] - _constraintVector[c]);
pVector[p] = 0;
}
#endif
// return false to indicate that we still need to proceed with optimization
return false;
}
void StaticOptimizationTarget::
computeActuatorAreas(const SimTK::State& s )
{
// COMPUTE ACTUATOR AREAS
ForceSet& forceSet = _model->updForceSet();
for(int i=0, j=0;i<forceSet.getSize();i++) {
ScalarActuator *act = dynamic_cast<ScalarActuator*>(&forceSet.get(i));
if( act ) {
act->setActuation(s, 1.0);
_recipAreaSquared[j] = act->getStress(s);
_recipAreaSquared[j] *= _recipAreaSquared[j];
j++;
}
}
}
/**
* Constructor.
*
* @param aNX Number of controls.
* @param aController Parent controller.
*/
ActuatorForceTargetFast::
ActuatorForceTargetFast(SimTK::State& s, int aNX,CMC *aController):
OptimizationTarget(aNX), _controller(aController)
{
// NUMBER OF CONTROLS
if(getNumParameters()<=0) {
throw(Exception("ActuatorForceTargetFast: ERROR- no controls.\n"));
}
// ALLOCATE STATE ARRAYS
int ny = _controller->getModel().getNumStateVariables();
int nq = _controller->getModel().getNumCoordinates();
int nu = _controller->getModel().getNumSpeeds();
int na = _controller->getActuatorSet().getSize();
_y.setSize(ny);
_dydt.setSize(ny);
_dqdt.setSize(nq);
_dudt.setSize(nu);
_recipAreaSquared.setSize(na);
_recipOptForceSquared.setSize(na);
_recipAvgActForceRangeSquared.setSize(na);
int nConstraints = _controller->getTaskSet().getNumActiveTaskFunctions();
// NUMBERS OF CONSTRAINTS
// There are only linear equality constraints.
setNumEqualityConstraints(nConstraints);
setNumLinearEqualityConstraints(nConstraints);
// DERIVATIVE PERTURBATION SIZES;
setDX(1.0e-6);
// COMPUTE ACTUATOR AREAS
Array<double> f(1.0,na);
const Set<Actuator>& fSet = _controller->getActuatorSet();
for(int i=0,j=0;i<fSet.getSize();i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
Muscle* musc = dynamic_cast<Muscle *>(act);
if(musc)
_recipAreaSquared[j] = f[j]/musc->getMaxIsometricForce();
else
_recipAreaSquared[j] = f[j]/act->getOptimalForce();
_recipAreaSquared[j] *= _recipAreaSquared[j];
j++;
}
}
//=============================================================================
// ANALYSIS
//=============================================================================
//
void InverseDynamics::
computeAcceleration(SimTK::State& s, double *aF,double *rAccel) const
{
for(int i=0,j=0; i<_forceSet->getSize(); i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->get(i));
if( act ) {
act->setOverrideActuation(s, aF[j++]);
}
}
// NEED TO APPLY OTHER FORCES (e.g. Prescribed) FROM ORIGINAL MODEL
_modelWorkingCopy->getMultibodySystem().realize(s,SimTK::Stage::Acceleration);
SimTK::Vector udot = _modelWorkingCopy->getMatterSubsystem().getUDot(s);
for(int i=0; i<_accelerationIndices.getSize(); i++)
rAccel[i] = udot[_accelerationIndices[i]];
}
/**
* Compute the Force.
*/
SimTK::Vector ActuatorForceProbe::computeProbeInputs(const State& s) const
{
int nA = getActuatorNames().size();
SimTK::Vector TotalF(getNumProbeInputs());
TotalF = 0;
// Loop through each actuator in the list of actuator_names.
for (int i = 0; i<nA; ++i)
{
// Get the Actuator force.
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_model->getActuators()[_actuatorIndex[i]]);
const double Ftmp = act->getForce(s);
// Append to output vector.
if (getSumForcesTogether())
TotalF(0) += std::pow(Ftmp, getExponent());
else
TotalF(i) = std::pow(Ftmp, getExponent());
}
return TotalF;
}
/**
* Evaluate the vector function.
*
* @param s SimTK::State.
* @param aF Array of actuator force differences.
*/
void VectorFunctionForActuators::
evaluate( const SimTK::State& s, double *aX, double *rF)
{
int i;
int N = getNX();
CMC& controller= dynamic_cast<CMC&>(_model->updControllerSet().get("CMC" ));
controller.updControlSet().setControlValues(_tf, aX);
// create a Manager that will integrate just the actuator subsystem and use only the
// CMC controller
Manager manager(*_model, *_integrator);
manager.setInitialTime(_ti);
manager.setFinalTime(_tf);
manager.setSystem( _CMCActuatorSystem );
// tell the mangager to not call the analayses or write to storage
// while the CMCSubsystem is being integrated.
manager.setPerformAnalyses(false);
manager.setWriteToStorage(false);
SimTK::State& actSysState = _CMCActuatorSystem->updDefaultState();
getCMCActSubsys()->updZ(actSysState) = _model->getMultibodySystem()
.getDefaultSubsystem().getZ(s);
actSysState.setTime(_ti);
// Integration
manager.integrate(actSysState, 0.000001);
const Set<Actuator>& forceSet = controller.getActuatorSet();
// Vector function values
int j = 0;
for(i=0;i<N;i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&forceSet[i]);
rF[j] = act->getActuation(getCMCActSubsys()->getCompleteState()) - _f[j];
j++;
}
}
/**
* 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);
}
void ActuatorForceTarget::
computePerformanceVectors(SimTK::State& s, const Vector &aF, Vector &rAccelPerformanceVector, Vector &rForcePerformanceVector)
{
const Set<Actuator> &fSet = _controller->getActuatorSet();
for(int i=0;i<fSet.getSize();i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
act->setOverrideActuation(s, aF[i]);
act->overrideActuation(s, true);
}
_controller->getModel().getMultibodySystem().realize(s, SimTK::Stage::Acceleration );
CMC_TaskSet& taskSet = _controller->updTaskSet();
taskSet.computeAccelerations(s);
Array<double> &w = taskSet.getWeights();
Array<double> &aDes = taskSet.getDesiredAccelerations();
Array<double> &a = taskSet.getAccelerations();
// PERFORMANCE
double sqrtStressTermWeight = sqrt(_stressTermWeight);
for(int i=0;i<fSet.getSize();i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
rForcePerformanceVector[i] = sqrtStressTermWeight * act->getStress(s);
}
int nacc = aDes.getSize();
for(int i=0;i<nacc;i++) rAccelPerformanceVector[i] = sqrt(w[i]) * (a[i] - aDes[i]);
// reset the actuator control
for(int i=0;i<fSet.getSize();i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
act->overrideActuation(s, false);
}
}
/**
* 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;
}
// for adding any components to the model
void CMC::extendAddToSystem( SimTK::MultibodySystem& system) const
{
Super::extendAddToSystem(system);
// add event handler for updating controls for next window
CMC* mutableThis = const_cast<CMC *>(this);
ComputeControlsEventHandler* computeControlsHandler =
new ComputeControlsEventHandler(mutableThis);
system.updDefaultSubsystem().addEventHandler(computeControlsHandler );
const Set<Actuator>& fSet = getActuatorSet();
int nActs = fSet.getSize();
mutableThis->_controlSetIndices.setSize(nActs);
// Create the control set that will hold the controls computed by CMC
mutableThis->_controlSet.setName(_model->getName());
mutableThis->_controlSet.setSize(0);
// Define the control set used to specify control bounds and to hold
// the computed control values from the CMC algorithm
double xmin =0, xmax=0;
std::string actName = "";
for(int i=0; i < nActs; ++i ) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
//Actuator& act = getActuatorSet().get(i);
ControlLinear *control = new ControlLinear();
control->setName(act->getName() + ".excitation" );
xmin = act->getMinControl();
if (xmin ==-SimTK::Infinity)
xmin =-MAX_CONTROLS_FOR_RRA;
xmax = act->getMaxControl();
if (xmax ==SimTK::Infinity)
xmax =MAX_CONTROLS_FOR_RRA;
Muscle *musc = dynamic_cast<Muscle *>(act);
// if controlling muscles, CMC requires that the control be constant (i.e. piecewise constant or use steps)
// since it uses this assumption to rootsolve for the required controls over the CMC time-window.
if(musc){
control->setUseSteps(true);
if(xmin < MIN_CMC_CONTROL_VALUE){
cout << "CMC::Warning: CMC cannot compute controls for muscles with muscle controls < " << MIN_CMC_CONTROL_VALUE <<".\n" <<
"The minimum control limit for muscle '" << musc->getName() << "' has been reset to " << MIN_CMC_CONTROL_VALUE <<"." <<endl;
xmin = MIN_CMC_CONTROL_VALUE;
}
if(xmax < MAX_CMC_CONTROL_VALUE){
cout << "CMC::Warning: CMC cannot compute controls for muscles with muscle controls > " << MAX_CMC_CONTROL_VALUE <<".\n" <<
"The maximum control limit for muscle '" << musc->getName() << "' has been reset to " << MAX_CMC_CONTROL_VALUE << "." << endl;
xmax = MAX_CMC_CONTROL_VALUE;
}
}
control->setDefaultParameterMin(xmin);
control->setDefaultParameterMax(xmax);
mutableThis->_controlSet.adoptAndAppend(control);
mutableThis->_controlSetIndices.set(i, i);
}
mutableThis->setNumControls(_controlSet.getSize());
}
/**
* 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 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);
}
/* Solve for the induced accelerations (udot_f) for a Force in the model
identified by its name. */
const SimTK::Vector& InducedAccelerationsSolver::solve(const SimTK::State& s,
const string& forceName,
bool computeActuatorPotentialOnly,
SimTK::Vector_<SimTK::SpatialVec>* constraintReactions)
{
int nu = _modelCopy.getNumSpeeds();
double aT = s.getTime();
SimTK::State& s_solver = _modelCopy.updWorkingState();
//_modelCopy.initStateWithoutRecreatingSystem(s_solver);
// Just need to set current time and kinematics to determine state of constraints
s_solver.setTime(aT);
s_solver.updQ()=s.getQ();
s_solver.updU()=s.getU();
// 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_solver);
// Hang on to a state that has the right flags for contact constraints turned on/off
_modelCopy.setPropertiesFromState(s_solver);
// Use this state for the remainder of this step (record)
s_solver = _modelCopy.getMultibodySystem().realizeTopology();
// DO NOT recreate the system, will lose location of constraint
_modelCopy.initStateWithoutRecreatingSystem(s_solver);
//cout << "Solving for contributor: " << _contributors[c] << endl;
// Need to be at the dynamics stage to disable a force
s_solver.setTime(aT);
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Dynamics);
if(forceName == "total"){
// Set gravity ON
_modelCopy.getGravityForce().enable(s_solver);
//Use same conditions on constraints
s_solver.updU() = s.getU();
s_solver.updU() = s.getZ();
//Make sure all the actuators are on!
for(int f=0; f<_modelCopy.getActuators().getSize(); f++){
_modelCopy.updActuators().get(f).setDisabled(s_solver, false);
}
// Get to the point where we can evaluate unilateral constraint conditions
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Acceleration);
/* *********************************** ERROR CHECKING *******************************
SimTK::Vec3 pcom =_modelCopy.getMultibodySystem().getMatterSubsystem().calcSystemMassCenterLocationInGround(s_solver);
SimTK::Vec3 vcom =_modelCopy.getMultibodySystem().getMatterSubsystem().calcSystemMassCenterVelocityInGround(s_solver);
SimTK::Vec3 acom =_modelCopy.getMultibodySystem().getMatterSubsystem().calcSystemMassCenterAccelerationInGround(s_solver);
SimTK::Matrix M;
_modelCopy.getMultibodySystem().getMatterSubsystem().calcM(s_solver, M);
cout << "mass matrix: " << M << endl;
SimTK::Inertia sysInertia = _modelCopy.getMultibodySystem().getMatterSubsystem().calcSystemCentralInertiaInGround(s_solver);
cout << "system inertia: " << sysInertia << endl;
SimTK::SpatialVec sysMomentum =_modelCopy.getMultibodySystem().getMatterSubsystem().calcSystemMomentumAboutGroundOrigin(s_solver);
cout << "system momentum: " << sysMomentum << endl;
const SimTK::Vector &appliedMobilityForces = _modelCopy.getMultibodySystem().getMobilityForces(s_solver, SimTK::Stage::Dynamics);
appliedMobilityForces.dump("All Applied Mobility Forces");
// Get all applied body forces like those from contact
const SimTK::Vector_<SimTK::SpatialVec>& appliedBodyForces = _modelCopy.getMultibodySystem().getRigidBodyForces(s_solver, SimTK::Stage::Dynamics);
appliedBodyForces.dump("All Applied Body Forces");
SimTK::Vector ucUdot;
SimTK::Vector_<SimTK::SpatialVec> ucA_GB;
_modelCopy.getMultibodySystem().getMatterSubsystem().calcAccelerationIgnoringConstraints(s_solver, 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 = _modelCopy.getMultibodySystem().getMatterSubsystem().getNumConstraints();
for (SimTK::ConstraintIndex cx(0); cx < nc; ++cx) {
if (!_modelCopy.getMultibodySystem().getMatterSubsystem().isConstraintDisabled(s_solver, cx)){
cout << "Constraint " << cx << " enabled!" << endl;
}
}
//int nMults = _modelCopy.getMultibodySystem().getMatterSubsystem().getTotalMultAlloc();
for(int i=0; i<constraintOn.getSize(); i++) {
if(constraintOn[i])
_constraintSet[i].calcConstraintForces(s_solver, constraintBodyForces, constraintMobilityForces);
}
constraintBodyForces.dump("Constraint Body Forces");
constraintMobilityForces.dump("Constraint Mobility Forces");
// ******************************* end ERROR CHECKING *******************************/
for(int i=0; i<constraintOn.getSize(); i++) {
_replacementConstraints[i].setDisabled(s_solver, !constraintOn[i]);
// Make sure we stay at Dynamics so each constraint can evaluate its conditions
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Acceleration);
}
// This should also push changes to defaults for unilateral conditions
_modelCopy.setPropertiesFromState(s_solver);
}
else if(forceName == "gravity"){
// Set gravity ON
_modelCopy.updForceSubsystem().setForceIsDisabled(s_solver, _modelCopy.getGravityForce().getForceIndex(), false);
// zero velocity
s_solver.setU(SimTK::Vector(nu,0.0));
// disable other forces
for(int f=0; f<_modelCopy.getForceSet().getSize(); f++){
_modelCopy.updForceSet()[f].setDisabled(s_solver, true);
}
}
else if(forceName == "velocity"){
// Set gravity off
_modelCopy.updForceSubsystem().setForceIsDisabled(s_solver, _modelCopy.getGravityForce().getForceIndex(), true);
// non-zero velocity
s_solver.updU() = s.getU();
// zero actuator forces
for(int f=0; f<_modelCopy.getActuators().getSize(); f++){
_modelCopy.updActuators().get(f).setDisabled(s_solver, true);
}
// Set the configuration (gen. coords and speeds) of the model.
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Velocity);
}
else{ //The rest are actuators
// Set gravity OFF
_modelCopy.updForceSubsystem().setForceIsDisabled(s_solver, _modelCopy.getGravityForce().getForceIndex(), true);
// zero actuator forces
for(int f=0; f<_modelCopy.getActuators().getSize(); f++){
_modelCopy.updActuators().get(f).setDisabled(s_solver, true);
}
// zero velocity
SimTK::Vector U(nu,0.0);
s_solver.setU(U);
s_solver.updZ() = s.getZ();
// light up the one Force who's contribution we are looking for
int ai = _modelCopy.getForceSet().getIndex(forceName);
if(ai<0){
cout << "Force '"<< forceName << "' not found in model '" <<
_modelCopy.getName() << "'." << endl;
}
Force &force = _modelCopy.getForceSet().get(ai);
force.setDisabled(s_solver, false);
ScalarActuator *actuator = dynamic_cast<ScalarActuator*>(&force);
if(actuator){
if(computeActuatorPotentialOnly){
actuator->overrideActuation(s_solver, true);
actuator->setOverrideActuation(s_solver, 1.0);
}
}
// Set the configuration (gen. coords and speeds) of the model.
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Model);
_modelCopy.getMultibodySystem().realize(s_solver, 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_solver) ? "off" : "on") << endl;
// cout << "Constraint 1 is of "<< _constraintSet[1].getConcreteClassName() << " and should be " << constraintOn[1] << " and is actually " << (_constraintSet[1].isDisabled(s_solver) ? "off" : "on") << endl;
// After setting the state of the model and applying forces
// Compute the derivative of the multibody system (speeds and accelerations)
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Acceleration);
// Sanity check that constraints hasn't totally changed the configuration of the model
// double error = (s.getQ()-s_solver.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_solver, constraintBodyForces, mobilityForces);
}*/
return s_solver.getUDot();
}
