/**
* 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);
}
/** Solve the inverse dynamics system of equations for generalized coordinate forces, Tau.
Now the state is not given, but is constructed from known coordinates, q as functions of time.
Coordinate functions must be twice differentiable.
NOTE: state dependent forces and other applied loads are NOT computed since these may depend on
state variables (like muscle fiber lengths) that are not known */
Vector InverseDynamicsSolver::solve(State &s, const FunctionSet &Qs, double time)
{
int nq = getModel().getNumCoordinates();
if(Qs.getSize() != nq){
throw Exception("InverseDynamicsSolver::solve invalid number of q functions.");
}
if( nq != getModel().getNumSpeeds()){
throw Exception("InverseDynamicsSolver::solve using FunctionSet, nq != nu not supported.");
}
// update the State so we get the correct gravity and coriolis effects
// direct references into the state so no allocation required
s.updTime() = time;
Vector &q = s.updQ();
Vector &u = s.updU();
Vector &udot = s.updUDot();
for(int i=0; i<nq; i++){
q[i] = Qs.evaluate(i, 0, time);
u[i] = Qs.evaluate(i, 1, time);
udot[i] = Qs.evaluate(i, 2, time);
}
// Perform general inverse dynamics
return solve(s, udot);
}
/**
* Run the inverse Dynamics tool.
*/
bool InverseDynamicsTool::run()
{
bool success = false;
bool modelFromFile=true;
try{
//Load and create the indicated model
if (!_model)
_model = new Model(_modelFileName);
else
modelFromFile = false;
_model->printBasicInfo(cout);
cout<<"Running tool " << getName() <<".\n"<<endl;
// Do the maneuver to change then restore working directory
// so that the parsing code behaves properly if called from a different directory.
string saveWorkingDirectory = IO::getCwd();
string directoryOfSetupFile = IO::getParentDirectory(getDocumentFileName());
IO::chDir(directoryOfSetupFile);
const CoordinateSet &coords = _model->getCoordinateSet();
int nq = _model->getNumCoordinates();
FunctionSet *coordFunctions = NULL;
//Storage *coordinateValues = NULL;
if(hasCoordinateValues()){
if(_lowpassCutoffFrequency>=0) {
cout<<"\n\nLow-pass filtering coordinates data with a cutoff frequency of "<<_lowpassCutoffFrequency<<"..."<<endl<<endl;
_coordinateValues->pad(_coordinateValues->getSize()/2);
_coordinateValues->lowpassIIR(_lowpassCutoffFrequency);
if (getVerboseLevel()==Debug) _coordinateValues->print("coordinateDataFiltered.sto");
}
// Convert degrees to radian if indicated
if(_coordinateValues->isInDegrees()){
_model->getSimbodyEngine().convertDegreesToRadians(*_coordinateValues);
}
// Create differentiable splines of the coordinate data
coordFunctions = new GCVSplineSet(5, _coordinateValues);
//Functions must correspond to model coordinates and their order for the solver
for(int i=0; i<nq; i++){
if(coordFunctions->contains(coords[i].getName())){
coordFunctions->insert(i,coordFunctions->get(coords[i].getName()));
}
else{
coordFunctions->insert(i,new Constant(coords[i].getDefaultValue()));
std::cout << "InverseDynamicsTool: coordinate file does not contain coordinate " << coords[i].getName() << " assuming default value" << std::endl;
}
}
if(coordFunctions->getSize() > nq){
coordFunctions->setSize(nq);
}
}
else{
IO::chDir(saveWorkingDirectory);
throw Exception("InverseDynamicsTool: no coordinate file found.");
}
bool externalLoads = createExternalLoads(_externalLoadsFileName, *_model, _coordinateValues);
// Initialize the the model's underlying computational system and get its default state.
SimTK::State& s = _model->initSystem();
// Exclude user-specified forces from the dynamics for this analysis
disableModelForces(*_model, s, _excludedForces);
double first_time = _coordinateValues->getFirstTime();
double last_time = _coordinateValues->getLastTime();
// Determine the starting and final time for the Tool by comparing to what data is available
double start_time = ( first_time > _timeRange[0]) ? first_time : _timeRange[0];
double final_time = ( last_time < _timeRange[1]) ? last_time : _timeRange[1];
int start_index = _coordinateValues->findIndex(start_time);
int final_index = _coordinateValues->findIndex(final_time);
// create the solver given the input data
InverseDynamicsSolver ivdSolver(*_model);
const clock_t start = clock();
int nt = final_index-start_index+1;
Array_<double> times(nt, 0.0);
for(int i=0; i<nt; i++){
times[i]=_coordinateValues->getStateVector(start_index+i)->getTime();
}
// Preallocate results
Array_<Vector> genForceTraj(nt, Vector(nq, 0.0));
// solve for the trajectory of generlized forces that correspond to the coordinate
// trajectories provided
ivdSolver.solve(s, *coordFunctions, times, genForceTraj);
success = true;
cout << "InverseDynamicsTool: " << nt << " time frames in " <<(double)(clock()-start)/CLOCKS_PER_SEC << "s\n" <<endl;
JointSet jointsForEquivalentBodyForces;
getJointsByName(*_model, _jointsForReportingBodyForces, jointsForEquivalentBodyForces);
int nj = jointsForEquivalentBodyForces.getSize();
Array<string> labels("time", nq+1);
for(int i=0; i<nq; i++){
labels[i+1] = coords[i].getName();
labels[i+1] += (coords[i].getMotionType() == Coordinate::Rotational) ? "_moment" : "_force";
}
Array<string> body_force_labels("time", 6*nj+1);
string XYZ = "XYZ";
for(int i=0; i<nj; i++){
string joint_body_label = jointsForEquivalentBodyForces[i].getName()+"_";
joint_body_label += jointsForEquivalentBodyForces[i].getBody().getName();
for(int k=0; k<3; ++k){
body_force_labels[6*i+k+1] = joint_body_label+"_F"+XYZ[k]; //first label is time
body_force_labels[6*i+k+3+1] = joint_body_label+"_M"+XYZ[k];
}
}
Storage genForceResults(nt);
Storage bodyForcesResults(nt);
SpatialVec equivalentBodyForceAtJoint;
for(int i=0; i<nt; i++){
StateVector genForceVec(times[i], nq, &((genForceTraj[i])[0]));
genForceResults.append(genForceVec);
// if there are joints requested for equivalent body forces then calculate them
if(nj>0){
Vector forces(6*nj, 0.0);
StateVector bodyForcesVec(times[i], 6*nj, &forces[0]);
s.updTime() = times[i];
Vector &q = s.updQ();
Vector &u = s.updU();
for(int j=0; j<nq; ++j){
q[j] = coordFunctions->evaluate(j, 0, times[i]);
u[j] = coordFunctions->evaluate(j, 1, times[i]);
}
for(int j=0; j<nj; ++j){
equivalentBodyForceAtJoint = jointsForEquivalentBodyForces[j].calcEquivalentSpatialForce(s, genForceTraj[i]);
for(int k=0; k<3; ++k){
// body force components
bodyForcesVec.setDataValue(6*j+k, equivalentBodyForceAtJoint[1][k]);
// body torque components
bodyForcesVec.setDataValue(6*j+k+3, equivalentBodyForceAtJoint[0][k]);
}
}
bodyForcesResults.append(bodyForcesVec);
}
}
genForceResults.setColumnLabels(labels);
genForceResults.setName("Inverse Dynamics Generalized Forces");
IO::makeDir(getResultsDir());
Storage::printResult(&genForceResults, _outputGenForceFileName, getResultsDir(), -1, ".sto");
IO::chDir(saveWorkingDirectory);
// if body forces to be reported for specified joints
if(nj >0){
bodyForcesResults.setColumnLabels(body_force_labels);
bodyForcesResults.setName("Inverse Dynamics Body Forces at Specified Joints");
IO::makeDir(getResultsDir());
Storage::printResult(&bodyForcesResults, _outputBodyForcesAtJointsFileName, getResultsDir(), -1, ".sto");
IO::chDir(saveWorkingDirectory);
}
}
catch (const OpenSim::Exception& ex) {
std::cout << "InverseDynamicsTool Failed: " << ex.what() << std::endl;
throw (Exception("InverseDynamicsTool Failed, please see messages window for details..."));
}
if (modelFromFile) delete _model;
return success;
}
