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