/**
* 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);
}
/// Add the functions called by a function to the function set
/// \param func
/// The function for which all called functions will be added
/// \param funcSet
/// The set of currently called functions
/// \param funcList
/// The list of all functions
/// \return
/// True if all functions are found in the funcList, false otherwise.
bool HlslLinker::addCalledFunctions( GlslFunction *func, FunctionSet& funcSet, std::vector<GlslFunction*> &funcList )
{
const std::set<std::string> &cf = func->getCalledFunctions();
for (std::set<std::string>::const_iterator cit=cf.begin(); cit != cf.end(); cit++)
{
std::vector<GlslFunction*>::iterator it = funcList.begin();
//This might be better as a more efficient search
while (it != funcList.end())
{
if ( *cit == (*it)->getMangledName())
break;
it++;
}
//check to see if it really exists
if ( it == funcList.end())
{
infoSink.info << "Failed to find function '" << *cit <<"'\n";
return false;
}
//add the function (if it's not there already) and recurse
if (std::find (funcSet.begin(), funcSet.end(), *it) == funcSet.end())
funcSet.push_back (*it);
addCalledFunctions( *it, funcSet, funcList);
}
return true;
}
bool
query_function_entry(void *addr)
{
FunctionSet::iterator it = function_entries.find(addr);
if (it == function_entries.end()) return false;
else return true;
}
void FunctionManager::destroy_functions_in_set(FunctionSet &function_set)
{
// We iterate over the set like this because destroy_function() will erase
// the function from function_set, thereby invalidating any iterator we are
// holding on to.
while (!function_set.empty())
{
destroy_function(*function_set.begin());
}
}
/**
* Set the velocity functions for the tasks. Functions are set based on the
* correspondence of the function and the task. For example,
* a task with the name "x" will search for a function or functions
* with the name "x". For tasks that require 3 functions, such
* as CMC_Point tasks, the assumption is that there will be three
* consecutive functions named "x" in the function set. If the correct
* number of functions is not found, the task is disabled.
*
* @param aFuncSet Function set.
* @return Pointer to the previous function set.
*/
void CMC_TaskSet::
setFunctionsForVelocity(FunctionSet &aFuncSet)
{
// LOOP THROUGH TRACK OBJECTS
int i,j,iFunc=0;
int nTrk;
string name;
Function *f[3];
const CoordinateSet& coords = getModel()->getCoordinateSet();
for(i=0;i<getSize();i++) {
// OBJECT
TrackingTask& ttask = get(i);
// If CMC_Task process same way as pre 2.0.2
if (dynamic_cast<CMC_Task*>(&ttask)==NULL)
continue;
CMC_Task& task = dynamic_cast<CMC_Task&>(ttask);
// NAME
name = task.getName();
if(name.empty()) continue;
const Coordinate& coord = coords.get(name);
// FIND FUNCTION(S)
f[0] = f[1] = f[2] = NULL;
nTrk = task.getNumTaskFunctions();
iFunc = aFuncSet.getIndex(coord.getSpeedName(),iFunc);
if (iFunc < 0){
name = coord.getJoint().getName() + "/" + coord.getSpeedName();
iFunc = aFuncSet.getIndex(name, iFunc);
if (iFunc < 0){
string msg = "CMC_TaskSet::setFunctionsForVelocity: function for task '";
msg += name + " not found.";
throw Exception(msg);
}
}
for(j=0;j<nTrk;j++) {
try {
f[j] = &aFuncSet.get(iFunc);
} catch(const Exception& x) {
x.print(cout);
}
if(f[j]==NULL) break;
}
task.setTaskFunctionsForVelocity(f[0],f[1],f[2]);
}
}
// Free the function_entries map, the Function objects in the map, the
// excluded_function_entries set and cbranges intervals.
//
void
function_entries_reinit(void)
{
FunctionSet::iterator it;
for (it = function_entries.begin(); it != function_entries.end(); it++) {
Function *f = it->second;
delete f->comment;
delete f;
}
function_entries.clear();
excluded_function_entries.clear();
cbranges.clear();
num_entries_total = 0;
}
static void EmitCalledFunctions (std::stringstream& shader, const FunctionSet& functions)
{
if (functions.empty())
return;
// Functions must be emitted in reverse order as they are sorted topologically in top to bottom.
for (FunctionSet::const_reverse_iterator fit = functions.rbegin(); fit != functions.rend(); fit++)
{
shader << "\n";
OutputLineDirective(shader, (*fit)->getLine());
shader << (*fit)->getPrototype() << " {\n";
shader << (*fit)->getCode() << "\n"; //has embedded }
shader << "\n";
}
}
void FPInferredTargetsAnalysis::findAllFunctionPointersInValue(Value* V, ValueContextPairList& worklist, ValueSet& visited) {
if (!visited.count(V)) {
visited.insert(V);
if (GlobalVariable* G = dyn_cast<GlobalVariable>(V)) {
SDEBUG("soaap.analysis.infoflow.fp.infer", 3, dbgs() << INDENT_1 << "Global var: " << G->getName() << "\n");
// don't look in the llvm.global.annotations array
if (G->getName() != "llvm.global.annotations" && G->hasInitializer()) {
findAllFunctionPointersInValue(G->getInitializer(), worklist, visited);
}
}
else if (ConstantArray* CA = dyn_cast<ConstantArray>(V)) {
SDEBUG("soaap.analysis.infoflow.fp.infer", 3, dbgs() << INDENT_1 << "Constant array, num of operands: " << CA->getNumOperands() << "\n");
for (int i=0; i<CA->getNumOperands(); i++) {
Value* V2 = CA->getOperand(i)->stripInBoundsOffsets();
findAllFunctionPointersInValue(V2, worklist, visited);
}
}
else if (Function* F = dyn_cast<Function>(V)) {
fpTargetsUniv.insert(F);
SDEBUG("soaap.analysis.infoflow.fp.infer", 3, dbgs() << INDENT_1 << "Func: " << F->getName() << "\n");
setBitVector(state[ContextUtils::NO_CONTEXT][V], F);
addToWorklist(V, ContextUtils::NO_CONTEXT, worklist);
}
else if (ConstantStruct* S = dyn_cast<ConstantStruct>(V)) {
SDEBUG("soaap.analysis.infoflow.fp.infer", 3, dbgs() << INDENT_1 << "Struct, num of fields: " << S->getNumOperands() << "\n");
for (int j=0; j<S->getNumOperands(); j++) {
Value* V2 = S->getOperand(j)->stripInBoundsOffsets();
findAllFunctionPointersInValue(V2, worklist, visited);
}
}
}
}
void
entries_in_range(void *start, void *end, vector<void *> &result)
{
#ifdef DEBUG_ENTRIES_IN_RANGE
printf("function entries for range [%p, %p]\n", start, end);
#endif
FunctionSet::iterator it = function_entries.find(start);
for (; it != function_entries.end(); it++) {
void *addr = (*it).first;
if (addr > end) return;
#ifdef DEBUG_ENTRIES_IN_RANGE
printf(" %p\n", addr);
#endif
result.push_back(addr);
}
}
static void EmitCalledFunctions (std::stringstream& shader, const FunctionSet& functions)
{
if (functions.empty())
return;
for (FunctionSet::const_reverse_iterator fit = functions.rbegin(); fit != functions.rend(); fit++) // emit backwards, will put least used ones in front
{
shader << (*fit)->getPrototype() << ";\n";
}
for (FunctionSet::const_reverse_iterator fit = functions.rbegin(); fit != functions.rend(); fit++) // emit backwards, will put least used ones in front
{
shader << (*fit)->getPrototype() << " {\n";
shader << (*fit)->getLocalDecls(1) << "\n";
shader << (*fit)->getCode() << "\n"; //has embedded }
shader << "\n";
}
}
void
add_function_entry(void *addr, const string *comment, bool isvisible,
int call_count)
{
FunctionSet::iterator it = function_entries.find(addr);
if (it == function_entries.end()) {
new_function_entry(addr, comment ? new string(*comment) : NULL,
isvisible, call_count);
} else {
Function *f = (*it).second;
if (comment) {
f->AppendComment(comment);
} else if (f->comment) {
f->isvisible = true;
}
f->call_count += call_count;
}
}
void FPInferredTargetsAnalysis::addInferredFunction(
Function *F, ContextVector contexts, Value *V,
ValueContextPairList& worklist) {
fpTargetsUniv.insert(F);
for (Context* Ctx : contexts) {
setBitVector(state[Ctx][V], F);
addToWorklist(V, Ctx, worklist);
}
}
void
dump_reachable_functions()
{
char buffer[1024];
FunctionSet::iterator i = function_entries.begin();
for (; i != function_entries.end();) {
Function *f = (*i).second;
++i;
const char *name;
if (!f->isvisible && !(f->call_count > 1) && !is_possible_fn(f->address)) continue;
if (f->comment) {
name = f->comment->c_str();
}
else {
// inferred functions must be at least 16 bytes long
if (i != function_entries.end()) {
Function *nextf = (*i).second;
if (f->call_count == 0 &&
(((unsigned long) nextf->address) -
((unsigned long) f->address)) < 16) {
long offset = offset_for_fn(f->address);
if (offset == 0) {
// if f->address lies within a valid code range, its
// offset will be non-zero. if offset is zero, the
// address cannot be a valid function start.
continue;
}
if (!range_contains_control_flow((char *) f->address + offset,
((char *) nextf->address +
offset)))
continue;
}
}
sprintf(buffer,"stripped_%p", f->address);
name = buffer;
}
dump_function_entry(f->address, name);
}
}
void HlslLinker::buildUniformsAndLibFunctions(const FunctionSet& calledFunctions, std::vector<GlslSymbol*>& constants, std::set<TOperator>& libFunctions)
{
for (FunctionSet::const_iterator it = calledFunctions.begin(); it != calledFunctions.end(); ++it) {
const std::vector<GlslSymbol*> &symbols = (*it)->getSymbols();
unsigned n_symbols = symbols.size();
for (unsigned i = 0; i != n_symbols; ++i) {
GlslSymbol* s = symbols[i];
if (s->getQualifier() == EqtUniform || s->getQualifier() == EqtMutableUniform)
constants.push_back(s);
}
//take each referenced library function, and add it to the set
const std::set<TOperator> &referencedFunctions = (*it)->getLibFunctions();
libFunctions.insert( referencedFunctions.begin(), referencedFunctions.end());
}
// std::unique only removes contiguous duplicates, so vector must be sorted to remove them all
std::sort(constants.begin(), constants.end(), GlslSymbolSorter());
// Remove duplicates
constants.resize(std::unique(constants.begin(), constants.end()) - constants.begin());
}
bool TraceMem::TraceMemPass::runOnModule(Module &M) {
FunctionSet fAdded;
trace_read = dyn_cast<Function>(M.getOrInsertFunction("trace_readi32",
Type::getInt32Ty(M.getContext()),
Type::getInt32Ty(M.getContext()),
Type::getInt32PtrTy(M.getContext()),
NULL));
trace_write = dyn_cast<Function>(M.getOrInsertFunction("trace_writei32",
Type::getVoidTy(M.getContext()),
Type::getInt32Ty(M.getContext()),
Type::getInt32PtrTy(M.getContext()),
Type::getInt32Ty(M.getContext()),
NULL));
seq = 0;
// Process each function
for (auto i = M.begin(), ie = M.end(); i != ie; ++i) {
auto func = i;
auto it = fAdded.find(func);
if (it == fAdded.end()) {
fAdded.insert(func);
if (func->size()) {
analyzeFunction(func);
++num_functions_analyzed;
} else {
++num_functions_skipped;
}
}
}
num_instructions_analyzed = fAnalyzedInstructions.size();
return true;
}
//
// Write one function entry in one format: server, C or text.
//
static void
dump_function_entry(void *addr, const char *comment)
{
num_entries_total++;
if (server_mode()) {
syserv_add_addr(addr, function_entries.size());
return;
}
if (c_mode()) {
if (num_entries_total > 1) {
printf(",\n");
}
printf(" 0x%" PRIxPTR " /* %s */", (uintptr_t) addr, comment);
return;
}
// default is text mode
printf("0x%" PRIxPTR " %s\n", (uintptr_t) addr, comment);
}
/**
* This method creates a SimmMotionTrial instance with the markerFile and
* timeRange parameters. It also creates a Storage instance with the
* coordinateFile parameter. Then it updates the coordinates and markers in
* the model, if specified. Then it does IK to fit the model to the static
* pose. Then it uses the current model pose to relocate all non-fixed markers
* according to their locations in the SimmMotionTrial. Then it writes the
* output files selected by the user.
*
* @param aModel the model to use for the marker placing process.
* @return Whether the marker placing process was successful or not.
*/
bool MarkerPlacer::processModel(Model* aModel,
const string& aPathToSubject) const {
if(!getApply()) return false;
cout << endl << "Step 3: Placing markers on model" << endl;
if (_timeRange.getSize()<2)
throw Exception("MarkerPlacer::processModel, time_range is unspecified.");
/* Load the static pose marker file, and average all the
* frames in the user-specified time range.
*/
TimeSeriesTableVec3 staticPoseTable{aPathToSubject + _markerFileName};
const auto& timeCol = staticPoseTable.getIndependentColumn();
// Users often set a time range that purposely exceeds the range of
// their data with the mindset that all their data will be used.
// To allow for that, we have to narrow the provided range to data
// range, since the TimeSeriesTable will correctly throw that the
// desired time exceeds the data range.
if (_timeRange[0] < timeCol.front())
_timeRange[0] = timeCol.front();
if (_timeRange[1] > timeCol.back())
_timeRange[1] = timeCol.back();
const auto avgRow = staticPoseTable.averageRow(_timeRange[0],
_timeRange[1]);
for(size_t r = staticPoseTable.getNumRows(); r-- > 0; )
staticPoseTable.removeRowAtIndex(r);
staticPoseTable.appendRow(_timeRange[0], avgRow);
OPENSIM_THROW_IF(!staticPoseTable.hasTableMetaDataKey("Units"),
Exception,
"MarkerPlacer::processModel -- Marker file does not have "
"'Units'.");
Units
staticPoseUnits{staticPoseTable.getTableMetaData<std::string>("Units")};
double scaleFactor = staticPoseUnits.convertTo(aModel->getLengthUnits());
OPENSIM_THROW_IF(SimTK::isNaN(scaleFactor),
Exception,
"Model has unspecified units.");
if(std::fabs(scaleFactor - 1) >= SimTK::Eps) {
for(unsigned r = 0; r < staticPoseTable.getNumRows(); ++r)
staticPoseTable.updRowAtIndex(r) *= scaleFactor;
staticPoseUnits = aModel->getLengthUnits();
staticPoseTable.removeTableMetaDataKey("Units");
staticPoseTable.addTableMetaData("Units",
staticPoseUnits.getAbbreviation());
}
MarkerData* staticPose = new MarkerData(aPathToSubject + _markerFileName);
staticPose->averageFrames(_maxMarkerMovement, _timeRange[0], _timeRange[1]);
staticPose->convertToUnits(aModel->getLengthUnits());
/* Delete any markers from the model that are not in the static
* pose marker file.
*/
aModel->deleteUnusedMarkers(staticPose->getMarkerNames());
// Construct the system and get the working state when done changing the model
SimTK::State& s = aModel->initSystem();
s.updTime() = _timeRange[0];
// Create references and WeightSets needed to initialize InverseKinemaicsSolver
Set<MarkerWeight> markerWeightSet;
_ikTaskSet.createMarkerWeightSet(markerWeightSet); // order in tasks file
// MarkersReference takes ownership of marker data (staticPose)
MarkersReference markersReference(staticPoseTable, &markerWeightSet);
SimTK::Array_<CoordinateReference> coordinateReferences;
// Load the coordinate data
// create CoordinateReferences for Coordinate Tasks
FunctionSet *coordFunctions = NULL;
// bool haveCoordinateFile = false;
if(_coordinateFileName != "" && _coordinateFileName != "Unassigned"){
Storage coordinateValues(aPathToSubject + _coordinateFileName);
aModel->getSimbodyEngine().convertDegreesToRadians(coordinateValues);
// haveCoordinateFile = true;
coordFunctions = new GCVSplineSet(5,&coordinateValues);
}
int index = 0;
for(int i=0; i< _ikTaskSet.getSize(); i++){
IKCoordinateTask *coordTask = dynamic_cast<IKCoordinateTask *>(&_ikTaskSet[i]);
if (coordTask && coordTask->getApply()){
std::unique_ptr<CoordinateReference> coordRef{};
if(coordTask->getValueType() == IKCoordinateTask::FromFile){
index = coordFunctions->getIndex(coordTask->getName(), index);
if(index >= 0){
coordRef.reset(new CoordinateReference(coordTask->getName(),coordFunctions->get(index)));
}
}
else if((coordTask->getValueType() == IKCoordinateTask::ManualValue)){
Constant reference(Constant(coordTask->getValue()));
coordRef.reset(new CoordinateReference(coordTask->getName(), reference));
}
else{ // assume it should be held at its current/default value
double value = aModel->getCoordinateSet().get(coordTask->getName()).getValue(s);
Constant reference = Constant(value);
coordRef.reset(new CoordinateReference(coordTask->getName(), reference));
}
if(coordRef == NULL)
throw Exception("MarkerPlacer: value for coordinate "+coordTask->getName()+" not found.");
// We have a valid coordinate reference so now set its weight according to the task
coordRef->setWeight(coordTask->getWeight());
coordinateReferences.push_back(*coordRef);
}
}
double constraintWeight = std::numeric_limits<SimTK::Real>::infinity();
InverseKinematicsSolver ikSol(*aModel, markersReference,
coordinateReferences, constraintWeight);
ikSol.assemble(s);
// Call realize Position so that the transforms are updated and markers can be moved correctly
aModel->getMultibodySystem().realize(s, SimTK::Stage::Position);
// Report marker errors to assess the quality
int nm = markerWeightSet.getSize();
SimTK::Array_<double> squaredMarkerErrors(nm, 0.0);
SimTK::Array_<Vec3> markerLocations(nm, Vec3(0));
double totalSquaredMarkerError = 0.0;
double maxSquaredMarkerError = 0.0;
int worst = -1;
// Report in the same order as the marker tasks/weights
ikSol.computeCurrentSquaredMarkerErrors(squaredMarkerErrors);
for(int j=0; j<nm; ++j){
totalSquaredMarkerError += squaredMarkerErrors[j];
if(squaredMarkerErrors[j] > maxSquaredMarkerError){
maxSquaredMarkerError = squaredMarkerErrors[j];
worst = j;
}
}
cout << "Frame at (t=" << s.getTime() << "):\t";
cout << "total squared error = " << totalSquaredMarkerError;
cout << ", marker error: RMS=" << sqrt(totalSquaredMarkerError/nm);
cout << ", max=" << sqrt(maxSquaredMarkerError) << " (" << ikSol.getMarkerNameForIndex(worst) << ")" << endl;
/* Now move the non-fixed markers on the model so that they are coincident
* with the measured markers in the static pose. The model is already in
* the proper configuration so the coordinates do not need to be changed.
*/
if(_moveModelMarkers) moveModelMarkersToPose(s, *aModel, *staticPose);
_outputStorage.reset();
// Make a storage file containing the solved states and markers for display in GUI.
Storage motionData;
StatesReporter statesReporter(aModel);
statesReporter.begin(s);
_outputStorage.reset(new Storage(statesReporter.updStatesStorage()));
_outputStorage->setName("static pose");
_outputStorage->getStateVector(0)->setTime(s.getTime());
if(_printResultFiles) {
std::string savedCwd = IO::getCwd();
IO::chDir(aPathToSubject);
try { // writing can throw an exception
if (_outputModelFileNameProp.isValidFileName()) {
aModel->print(aPathToSubject + _outputModelFileName);
cout << "Wrote model file " << _outputModelFileName <<
" from model " << aModel->getName() << endl;
}
if (_outputMarkerFileNameProp.isValidFileName()) {
aModel->writeMarkerFile(aPathToSubject + _outputMarkerFileName);
cout << "Wrote marker file " << _outputMarkerFileName <<
" from model " << aModel->getName() << endl;
}
if (_outputMotionFileNameProp.isValidFileName()) {
_outputStorage->print(aPathToSubject + _outputMotionFileName,
"w", "File generated from solving marker data for model "
+ aModel->getName());
}
} // catch the exception so we can reset the working directory
catch (std::exception& ex) {
IO::chDir(savedCwd);
OPENSIM_THROW_FRMOBJ(Exception, ex.what());
}
IO::chDir(savedCwd);
}
return true;
}
/**
* This method creates a SimmMotionTrial instance with the markerFile and
* timeRange parameters. It also creates a Storage instance with the
* coordinateFile parameter. Then it updates the coordinates and markers in
* the model, if specified. Then it does IK to fit the model to the static
* pose. Then it uses the current model pose to relocate all non-fixed markers
* according to their locations in the SimmMotionTrial. Then it writes the
* output files selected by the user.
*
* @param aModel the model to use for the marker placing process.
* @return Whether the marker placing process was successful or not.
*/
bool MarkerPlacer::processModel(Model* aModel, const string& aPathToSubject)
{
if(!getApply()) return false;
cout << endl << "Step 3: Placing markers on model" << endl;
/* Load the static pose marker file, and average all the
* frames in the user-specified time range.
*/
MarkerData staticPose(aPathToSubject + _markerFileName);
if (_timeRange.getSize()<2)
throw Exception("MarkerPlacer::processModel, time_range is unspecified.");
staticPose.averageFrames(_maxMarkerMovement, _timeRange[0], _timeRange[1]);
staticPose.convertToUnits(aModel->getLengthUnits());
/* Delete any markers from the model that are not in the static
* pose marker file.
*/
aModel->deleteUnusedMarkers(staticPose.getMarkerNames());
// Construct the system and get the working state when done changing the model
SimTK::State& s = aModel->initSystem();
// Create references and WeightSets needed to initialize InverseKinemaicsSolver
Set<MarkerWeight> markerWeightSet;
_ikTaskSet.createMarkerWeightSet(markerWeightSet); // order in tasks file
MarkersReference markersReference(staticPose, &markerWeightSet);
SimTK::Array_<CoordinateReference> coordinateReferences;
// Load the coordinate data
// create CoordinateReferences for Coordinate Tasks
FunctionSet *coordFunctions = NULL;
bool haveCoordinateFile = false;
if(_coordinateFileName != "" && _coordinateFileName != "Unassigned"){
Storage coordinateValues(aPathToSubject + _coordinateFileName);
aModel->getSimbodyEngine().convertDegreesToRadians(coordinateValues);
haveCoordinateFile = true;
coordFunctions = new GCVSplineSet(5,&coordinateValues);
}
int index = 0;
for(int i=0; i< _ikTaskSet.getSize(); i++){
IKCoordinateTask *coordTask = dynamic_cast<IKCoordinateTask *>(&_ikTaskSet[i]);
if (coordTask && coordTask->getApply()){
CoordinateReference *coordRef = NULL;
if(coordTask->getValueType() == IKCoordinateTask::FromFile){
index = coordFunctions->getIndex(coordTask->getName(), index);
if(index >= 0){
coordRef = new CoordinateReference(coordTask->getName(),coordFunctions->get(index));
}
}
else if((coordTask->getValueType() == IKCoordinateTask::ManualValue)){
Constant reference(Constant(coordTask->getValue()));
coordRef = new CoordinateReference(coordTask->getName(), reference);
}
else{ // assume it should be held at its current/default value
double value = aModel->getCoordinateSet().get(coordTask->getName()).getValue(s);
Constant reference = Constant(value);
coordRef = new CoordinateReference(coordTask->getName(), reference);
}
if(coordRef == NULL)
throw Exception("MarkerPlacer: value for coordinate "+coordTask->getName()+" not found.");
// We have a valid coordinate reference so now set its weight according to the task
coordRef->setWeight(coordTask->getWeight());
coordinateReferences.push_back(*coordRef);
}
}
double constraintWeight = std::numeric_limits<SimTK::Real>::infinity();
InverseKinematicsSolver ikSol(*aModel, markersReference,
coordinateReferences, constraintWeight);
ikSol.assemble(s);
// Call realize Position so that the transforms are updated and markers can be moved correctly
aModel->getMultibodySystem().realize(s, SimTK::Stage::Position);
// Report marker errors to assess the quality
int nm = markerWeightSet.getSize();
SimTK::Array_<double> squaredMarkerErrors(nm, 0.0);
SimTK::Array_<Vec3> markerLocations(nm, Vec3(0));
double totalSquaredMarkerError = 0.0;
double maxSquaredMarkerError = 0.0;
int worst = -1;
// Report in the same order as the marker tasks/weights
ikSol.computeCurrentSquaredMarkerErrors(squaredMarkerErrors);
for(int j=0; j<nm; ++j){
totalSquaredMarkerError += squaredMarkerErrors[j];
if(squaredMarkerErrors[j] > maxSquaredMarkerError){
maxSquaredMarkerError = squaredMarkerErrors[j];
worst = j;
}
}
cout << "Frame at (t=" << s.getTime() << "):\t";
cout << "total squared error = " << totalSquaredMarkerError;
cout << ", marker error: RMS=" << sqrt(totalSquaredMarkerError/nm);
cout << ", max=" << sqrt(maxSquaredMarkerError) << " (" << ikSol.getMarkerNameForIndex(worst) << ")" << endl;
/* Now move the non-fixed markers on the model so that they are coincident
* with the measured markers in the static pose. The model is already in
* the proper configuration so the coordinates do not need to be changed.
*/
if(_moveModelMarkers) moveModelMarkersToPose(s, *aModel, staticPose);
if (_outputStorage!= NULL){
delete _outputStorage;
}
// Make a storage file containing the solved states and markers for display in GUI.
Storage motionData;
StatesReporter statesReporter(aModel);
statesReporter.begin(s);
_outputStorage = new Storage(statesReporter.updStatesStorage());
_outputStorage->setName("static pose");
//_outputStorage->print("statesReporterOutput.sto");
Storage markerStorage;
staticPose.makeRdStorage(*_outputStorage);
_outputStorage->getStateVector(0)->setTime(s.getTime());
statesReporter.updStatesStorage().addToRdStorage(*_outputStorage, s.getTime(), s.getTime());
//_outputStorage->print("statesReporterOutputWithMarkers.sto");
if(_printResultFiles) {
if (!_outputModelFileNameProp.getValueIsDefault())
{
aModel->print(aPathToSubject + _outputModelFileName);
cout << "Wrote model file " << _outputModelFileName << " from model " << aModel->getName() << endl;
}
if (!_outputMarkerFileNameProp.getValueIsDefault())
{
aModel->writeMarkerFile(aPathToSubject + _outputMarkerFileName);
cout << "Wrote marker file " << _outputMarkerFileName << " from model " << aModel->getName() << endl;
}
if (!_outputMotionFileNameProp.getValueIsDefault())
{
_outputStorage->print(aPathToSubject + _outputMotionFileName,
"w", "File generated from solving marker data for model "+aModel->getName());
}
}
return true;
}
bool HlslLinker::link(HlslCrossCompiler* compiler, const char* entryFunc, bool usePrecision)
{
std::vector<GlslFunction*> globalList;
std::vector<GlslFunction*> functionList;
std::string entryPoint;
GlslFunction* funcMain = NULL;
FunctionSet calledFunctions;
std::set<TOperator> libFunctions;
std::map<std::string,GlslSymbol*> globalSymMap;
std::map<std::string,GlslStruct*> structMap;
if (!compiler)
{
infoSink.info << "No shader compiler provided\n";
return false;
}
EShLanguage lang = compiler->getLanguage();
if (!entryFunc)
{
infoSink.info << "No shader entry function provided\n";
return false;
}
entryPoint = GetEntryName (entryFunc);
//build the list of functions
HlslCrossCompiler *comp = static_cast<HlslCrossCompiler*>(compiler);
std::vector<GlslFunction*> &fl = comp->functionList;
for ( std::vector<GlslFunction*>::iterator fit = fl.begin(); fit < fl.end(); fit++)
{
if ( (*fit)->getName() == "__global__")
globalList.push_back( *fit);
else
functionList.push_back( *fit);
if ((*fit)->getName() == entryPoint)
{
if (funcMain)
{
infoSink.info << kShaderTypeNames[lang] << " entry function cannot be overloaded\n";
return false;
}
funcMain = *fit;
}
}
// check to ensure that we found the entry function
if (!funcMain)
{
infoSink.info << "Failed to find entry function: '" << entryPoint <<"'\n";
return false;
}
//add all the called functions to the list
calledFunctions.push_back (funcMain);
if (!addCalledFunctions (funcMain, calledFunctions, functionList))
{
infoSink.info << "Failed to resolve all called functions in the " << kShaderTypeNames[lang] << " shader\n";
}
//iterate over the functions, building a global list of structure declaractions and symbols
// assume a single compilation unit for expediency (eliminates name clashes, as type checking
// withing a single compilation unit has been performed)
for (FunctionSet::iterator it=calledFunctions.begin(); it != calledFunctions.end(); it++)
{
//get each symbol and each structure, and add them to the map
// checking that any previous entries are equivalent
const std::vector<GlslSymbol*> &symList = (*it)->getSymbols();
for (std::vector<GlslSymbol*>::const_iterator cit = symList.begin(); cit < symList.end(); cit++)
{
if ( (*cit)->getIsGlobal())
{
//should check for already added ones here
globalSymMap[(*cit)->getName()] = *cit;
}
}
//take each referenced library function, and add it to the set
const std::set<TOperator> &libSet = (*it)->getLibFunctions();
libFunctions.insert( libSet.begin(), libSet.end());
}
// The following code is what is used to generate the actual shader and "main"
// function. The process is to take all the components collected above, and
// write them to the appropriate code stream. Finally, a main function is
// generated that calls the specified entrypoint. That main function uses
// semantics on the arguments and return values to connect items appropriately.
//
// Write Library Functions & required extensions
std::string shaderExtensions, shaderLibFunctions;
if (!libFunctions.empty())
{
for (std::set<TOperator>::iterator it = libFunctions.begin(); it != libFunctions.end(); it++)
{
const std::string &func = getHLSLSupportCode(*it, shaderExtensions, lang==EShLangVertex);
if (!func.empty())
{
shaderLibFunctions += func;
shaderLibFunctions += '\n';
}
}
}
shader << shaderExtensions;
shader << shaderLibFunctions;
//
//Structure addition hack
// Presently, structures are not tracked per function, just dump them all
// This could be improved by building a complete list of structures for the
// shaders based on the variables in each function
//
{
HlslCrossCompiler *comp = static_cast<HlslCrossCompiler*>(compiler);
std::vector<GlslStruct*> &sList = comp->structList;
if (!sList.empty())
{
for (std::vector<GlslStruct*>::iterator it = sList.begin(); it < sList.end(); it++)
{
shader << (*it)->getDecl() << "\n";
}
}
}
//
// Write global variables
//
if (!globalSymMap.empty())
{
for (std::map<std::string,GlslSymbol*>::iterator sit = globalSymMap.begin(); sit != globalSymMap.end(); sit++)
{
sit->second->writeDecl(shader,false,false);
shader << ";\n";
if ( sit->second->getIsMutable() )
{
sit->second->writeDecl(shader, true, false);
shader << ";\n";
}
}
}
//
// Write function declarations and definitions
//
EmitCalledFunctions (shader, calledFunctions);
//
// Gather the uniforms into the uniform list
//
for (std::map<std::string, GlslSymbol*>::iterator it = globalSymMap.begin(); it != globalSymMap.end(); it++)
{
if (it->second->getQualifier() != EqtUniform)
continue;
ShUniformInfo infoStruct;
infoStruct.name = new char[it->first.size()+1];
strcpy( infoStruct.name, it->first.c_str());
if (it->second->getSemantic() != "")
{
infoStruct.semantic = new char[it->second->getSemantic().size()+1];
strcpy( infoStruct.semantic, it->second->getSemantic().c_str());
}
else
infoStruct.semantic = 0;
//gigantic hack, the enumerations are kept in alignment
infoStruct.type = (EShType)it->second->getType();
infoStruct.arraySize = it->second->getArraySize();
if ( it->second->hasInitializer() )
{
int initSize = it->second->initializerSize();
infoStruct.init = new float[initSize];
memcpy( infoStruct.init, it->second->getInitializer(), sizeof(float) * initSize);
}
else
infoStruct.init = 0;
//TODO: need to add annotation
uniforms.push_back( infoStruct);
}
//
// Generate the main function
//
std::stringstream attrib;
std::stringstream uniform;
std::stringstream preamble;
std::stringstream postamble;
std::stringstream varying;
std::stringstream call;
const int pCount = funcMain->getParameterCount();
preamble << "void main() {\n";
const EGlslSymbolType retType = funcMain->getReturnType();
GlslStruct *retStruct = funcMain->getStruct();
if ( retType == EgstStruct)
{
assert(retStruct);
preamble << " " << retStruct->getName() << " xl_retval;\n";
}
else
{
if ( retType != EgstVoid)
{
preamble << " ";
writeType (preamble, retType, NULL, usePrecision?funcMain->getPrecision():EbpUndefined);
preamble << " xl_retval;\n";
}
}
// Write all mutable initializations
if ( calledFunctions.size() > 0 )
{
for (FunctionSet::iterator fit = calledFunctions.begin(); fit != calledFunctions.end(); fit++)
{
std::string mutableDecls = (*fit)->getMutableDecls(1, calledFunctions.begin(), fit);
if ( mutableDecls.size() > 0 )
{
preamble << mutableDecls;
}
}
}
call << " ";
if (retType != EgstVoid)
call << "xl_retval = " << funcMain->getName() << "( ";
else
call << funcMain->getName() << "( ";
// pass main function parameters
for (int ii=0; ii<pCount; ii++)
{
GlslSymbol *sym = funcMain->getParameter(ii);
EAttribSemantic attrSem = parseAttributeSemantic( sym->getSemantic());
switch (sym->getQualifier())
{
// -------- IN & OUT parameters
case EqtIn:
case EqtInOut:
if ( sym->getType() != EgstStruct)
{
std::string name, ctor;
int pad;
if ( getArgumentData( sym, lang==EShLangVertex ? EClassAttrib : EClassVarIn, name, ctor, pad) )
{
// In fragment shader, pass zero for POSITION inputs
bool ignoredPositionInFragment = false;
if (lang == EShLangFragment && attrSem == EAttrSemPosition)
{
call << ctor << "(0.0)";
ignoredPositionInFragment = true;
}
// For "in" parameters, just call directly to the main
else if ( sym->getQualifier() != EqtInOut )
{
call << ctor << "(" << name;
for (int ii = 0; ii<pad; ii++)
call << ", 0.0";
call << ")";
}
// For "inout" parameters, declare a temp and initialize the temp
else
{
preamble << " ";
writeType (preamble, sym->getType(), NULL, usePrecision?sym->getPrecision():EbpUndefined);
preamble << " xlt_" << sym->getName() << " = ";
preamble << ctor << "(" << name;
for (int ii = 0; ii<pad; ii++)
preamble << ", 0.0";
preamble << ");\n";
}
if (lang == EShLangVertex) // vertex shader: deal with gl_ attributes
{
if ( strncmp( name.c_str(), "gl_", 3))
{
int typeOffset = 0;
// If the type is integer or bool based, we must convert to a float based
// type. This is because GLSL does not allow int or bool based vertex attributes.
if ( sym->getType() >= EgstInt && sym->getType() <= EgstInt4)
{
typeOffset += 4;
}
if ( sym->getType() >= EgstBool && sym->getType() <= EgstBool4)
{
typeOffset += 8;
}
// This is an undefined attribute
attrib << "attribute " << getTypeString((EGlslSymbolType)(sym->getType() + typeOffset)) << " " << name << ";\n";
}
}
if (lang == EShLangFragment) // deal with varyings
{
if (!ignoredPositionInFragment)
AddToVaryings (varying, sym->getPrecision(), ctor, name);
}
}
else
{
//should deal with fall through cases here
assert(0);
infoSink.info << "Unsupported type for shader entry parameter (";
infoSink.info << getTypeString(sym->getType()) << ")\n";
}
}
else
{
//structs must pass the struct, then process per element
GlslStruct *Struct = sym->getStruct();
assert(Struct);
//first create the temp
std::string tempVar = "xlt_" + sym->getName();
preamble << " " << Struct->getName() << " ";
preamble << tempVar <<";\n";
call << tempVar;
const int elem = Struct->memberCount();
for (int jj=0; jj<elem; jj++)
{
const GlslStruct::member ¤t = Struct->getMember(jj);
EAttribSemantic memberSem = parseAttributeSemantic (current.semantic);
std::string name, ctor;
int pad;
int numArrayElements = 1;
bool bIsArray = false;
// If it is an array, loop over each member
if ( current.arraySize > 0 )
{
numArrayElements = current.arraySize;
bIsArray = true;
}
for ( int arrayIndex = 0; arrayIndex < numArrayElements; arrayIndex++ )
{
if ( getArgumentData2( current.name, current.semantic, current.type,
lang==EShLangVertex ? EClassAttrib : EClassVarIn, name, ctor, pad, arrayIndex ) )
{
preamble << " ";
preamble << tempVar << "." << current.name;
if ( bIsArray )
preamble << "[" << arrayIndex << "]";
// In fragment shader, pass zero for POSITION inputs
bool ignoredPositionInFragment = false;
if (lang == EShLangFragment && memberSem == EAttrSemPosition)
{
preamble << " = " << ctor << "(0.0);\n";
ignoredPositionInFragment = true;
}
else
{
preamble << " = " << ctor << "( " << name;
for (int ii = 0; ii<pad; ii++)
preamble << ", 0.0";
preamble << ");\n";
}
if (lang == EShLangVertex) // vertex shader: gl_ attributes
{
if ( strncmp( name.c_str(), "gl_", 3))
{
int typeOffset = 0;
// If the type is integer or bool based, we must convert to a float based
// type. This is because GLSL does not allow int or bool based vertex attributes.
if ( current.type >= EgstInt && current.type <= EgstInt4)
{
typeOffset += 4;
}
if ( current.type >= EgstBool && current.type <= EgstBool4)
{
typeOffset += 8;
}
// This is an undefined attribute
attrib << "attribute " << getTypeString((EGlslSymbolType)(current.type + typeOffset)) << " " << name << ";\n";
}
}
if (lang == EShLangFragment) // deal with varyings
{
if (!ignoredPositionInFragment)
AddToVaryings (varying, current.precision, ctor, name);
}
}
else
{
//should deal with fall through cases here
assert(0);
infoSink.info << "Unsupported type for struct element in shader entry parameter (";
infoSink.info << getTypeString(current.type) << ")\n";
}
}
}
}
//
// NOTE: This check only breaks out of the case if we have an "in" parameter, for
// "inout" it will fallthrough to the next case
//
if ( sym->getQualifier() != EqtInOut )
{
break;
}
// -------- OUT parameters; also fall-through for INOUT (see the check above)
case EqtOut:
if ( sym->getType() != EgstStruct)
{
std::string name, ctor;
int pad;
if ( getArgumentData( sym, lang==EShLangVertex ? EClassVarOut : EClassRes, name, ctor, pad) )
{
// For "inout" parameters, the preamble was already written so no need to do it here.
if ( sym->getQualifier() != EqtInOut )
{
preamble << " ";
writeType (preamble, sym->getType(), NULL, usePrecision?sym->getPrecision():EbpUndefined);
preamble << " xlt_" << sym->getName() << ";\n";
}
if (lang == EShLangVertex) // deal with varyings
{
AddToVaryings (varying, sym->getPrecision(), ctor, name);
}
call << "xlt_" << sym->getName();
postamble << " ";
postamble << name << " = " << ctor << "( xlt_" <<sym->getName();
for (int ii = 0; ii<pad; ii++)
postamble << ", 0.0";
postamble << ");\n";
}
else
{
//should deal with fall through cases here
assert(0);
infoSink.info << "Unsupported type for shader entry parameter (";
infoSink.info << getTypeString(sym->getType()) << ")\n";
}
}
else
{
//structs must pass the struct, then process per element
GlslStruct *Struct = sym->getStruct();
assert(Struct);
//first create the temp
std::string tempVar = "xlt_" + sym->getName();
// For "inout" parmaeters the preamble and call were already written, no need to do it here
if ( sym->getQualifier() != EqtInOut )
{
preamble << " " << Struct->getName() << " ";
preamble << tempVar <<";\n";
call << tempVar;
}
const int elem = Struct->memberCount();
for (int ii=0; ii<elem; ii++)
{
const GlslStruct::member ¤t = Struct->getMember(ii);
std::string name, ctor;
int pad;
if ( getArgumentData2( current.name, current.semantic, current.type, lang==EShLangVertex ? EClassVarOut : EClassRes, name, ctor, pad, 0) )
{
postamble << " ";
postamble << name << " = " << ctor;
postamble << "( " << tempVar << "." << current.name;
for (int ii = 0; ii<pad; ii++)
postamble << ", 0.0";
postamble << ");\n";
if (lang == EShLangVertex) // deal with varyings
{
AddToVaryings (varying, current.precision, ctor, name);
}
}
else
{
//should deal with fall through cases here
assert(0);
infoSink.info << "Unsupported type in struct element for shader entry parameter (";
infoSink.info << getTypeString(current.type) << ")\n";
}
}
}
break;
case EqtUniform:
uniform << "uniform ";
writeType (uniform, sym->getType(), NULL, usePrecision?sym->getPrecision():EbpUndefined);
uniform << " xlu_" << sym->getName() << ";\n";
call << "xlu_" << sym->getName();
break;
default:
assert(0);
};
if (ii != pCount -1)
call << ", ";
}
call << ");\n";
// -------- return value of main entry point
if (retType != EgstVoid)
{
if (retType != EgstStruct)
{
std::string name, ctor;
int pad;
if ( getArgumentData2( "", funcMain->getSemantic(), retType, lang==EShLangVertex ? EClassVarOut : EClassRes,
name, ctor, pad, 0) )
{
postamble << " ";
postamble << name << " = " << ctor << "( xl_retval";
for (int ii = 0; ii<pad; ii++)
postamble << ", 0.0";
postamble << ");\n";
if (lang == EShLangVertex) // deal with varyings
{
AddToVaryings (varying, funcMain->getPrecision(), ctor, name);
}
}
else
{
//should deal with fall through cases here
assert(0);
infoSink.info << (lang==EShLangVertex ? "Unsupported type for shader return value (" : "Unsupported return type for shader entry function (");
infoSink.info << getTypeString(retType) << ")\n";
}
}
else
{
const int elem = retStruct->memberCount();
for (int ii=0; ii<elem; ii++)
{
const GlslStruct::member ¤t = retStruct->getMember(ii);
std::string name, ctor;
int pad;
int numArrayElements = 1;
bool bIsArray = false;
if (lang == EShLangVertex) // vertex shader
{
// If it is an array, loop over each member
if ( current.arraySize > 0 )
{
numArrayElements = current.arraySize;
bIsArray = true;
}
}
for ( int arrayIndex = 0; arrayIndex < numArrayElements; arrayIndex++ )
{
if ( getArgumentData2( current.name, current.semantic, current.type, lang==EShLangVertex ? EClassVarOut : EClassRes, name, ctor, pad, arrayIndex) )
{
postamble << " ";
postamble << name;
postamble << " = " << ctor;
postamble << "( xl_retval." << current.name;
if ( bIsArray )
{
postamble << "[" << arrayIndex << "]";
}
for (int ii = 0; ii<pad; ii++)
postamble << ", 0.0";
postamble << ");\n";
if (lang == EShLangVertex) // deal with varyings
{
AddToVaryings (varying, current.precision, ctor, name);
}
}
else
{
//should deal with fall through cases here
//assert(0);
infoSink.info << (lang==EShLangVertex ? "Unsupported element type in struct for shader return value (" : "Unsupported struct element type in return type for shader entry function (");
infoSink.info << getTypeString(current.type) << ")\n";
return false;
}
}
}
}
}
else
{
if (lang == EShLangFragment) // fragment shader
{
// If no return type, close off the output
postamble << ";\n";
}
}
postamble << "}\n\n";
EmitIfNotEmpty (shader, uniform);
EmitIfNotEmpty (shader, attrib);
EmitIfNotEmpty (shader, varying);
shader << preamble.str() << "\n";
shader << call.str() << "\n";
shader << postamble.str() << "\n";
return true;
}
bool contains_function_entry(void *address)
{
FunctionSet::iterator it = function_entries.find(address);
if (it != function_entries.end()) return true;
else return false;
}
/**
* Set the functions for the tasks. Functions are set based on the
* correspondence of the function and the task. For example,
* a task with the name "x" will search for a function or functions
* with the name "x". For tasks that require 3 functions, such
* as CMC_Point tasks, the assumption is that there will be three
* consecutive functions named "x" in the function set. If the correct
* number of functions is not found, the task is disabled.
*
* @param aFuncSet Function set.
* @return Pointer to the previous function set.
*/
void CMC_TaskSet::
setFunctions(FunctionSet &aFuncSet)
{
// LOOP THROUGH TRACK OBJECTS
int i,j,iFunc=0;
int nTrk;
string name;
Function *f[3];
for(i=0;i<getSize();i++) {
// OBJECT
TrackingTask& ttask = get(i);
if (dynamic_cast<StateTrackingTask*>(&ttask)!=NULL) {
StateTrackingTask& sTask = dynamic_cast<StateTrackingTask&>(ttask);
if (aFuncSet.contains(sTask.getName())){
sTask.setTaskFunctions(&aFuncSet.get(sTask.getName()));
}
else{
cout << "State tracking task " << sTask.getName()
<< "has no data to track and will be ignored" << std::endl;
}
continue;
}
// If CMC_Task process same way as pre 2.0.2
if (dynamic_cast<CMC_Task*>(&ttask)==NULL)
continue;
CMC_Task& task = dynamic_cast<CMC_Task&>(ttask);
// NAME
name = task.getName();
if(name.empty()) continue;
// FIND FUNCTION(S)
f[0] = f[1] = f[2] = NULL;
nTrk = task.getNumTaskFunctions();
iFunc = aFuncSet.getIndex(name,iFunc);
if (iFunc < 0){
const Coordinate& coord = _model->getCoordinateSet().get(name);
name = coord.getJoint().getName() + "/" + name + "/value";
iFunc = aFuncSet.getIndex(name, iFunc);
if (iFunc < 0){
string msg = "CMC_TaskSet::setFunctionsForVelocity: function for task '";
msg += name + " not found.";
throw Exception(msg);
}
}
for(j=0;j<nTrk;j++) {
try {
f[j] = &aFuncSet.get(iFunc);
} catch(const Exception& x) {
x.print(cout);
}
if(f[j]==NULL) break;
if(name == f[j]->getName()) {
iFunc++;
} else {
f[j] = NULL;
break;
}
}
task.setTaskFunctions(f[0],f[1],f[2]);
}
}
void FPInferredTargetsAnalysis::initialise(ValueContextPairList& worklist, Module& M, SandboxVector& sandboxes) {
FPTargetsAnalysis::initialise(worklist, M, sandboxes);
SDEBUG("soaap.analysis.infoflow.fp.infer", 3, dbgs() << "Running FP inferred targets analysis\n");
bool debug = false;
SDEBUG("soaap.analysis.infoflow.fp.infer", 3, debug = true);
if (debug) {
dbgs() << "Program statistics:\n";
// find all assignments of functions and propagate them!
long numFuncs = 0;
long numFPFuncs = 0;
long numFPcalls = 0;
long numInsts = 0;
long numAddFuncs = 0;
long loadInsts = 0;
long storeInsts = 0;
long intrinsInsts = 0;
for (Function& F : M.functions()) {
if (F.hasAddressTaken()) numAddFuncs++;
bool hasFPcall = false;
for (Instruction& I : instructions(&F)) {
numInsts++;
if (IntrinsicInst* II = dyn_cast<IntrinsicInst>(&I)) {
intrinsInsts++;
}
else if (CallInst* C = dyn_cast<CallInst>(&I)) {
if (CallGraphUtils::isIndirectCall(C)) {
hasFPcall = true;
numFPcalls++;
}
}
else if (LoadInst* L = dyn_cast<LoadInst>(&I)) {
loadInsts++;
}
else if (StoreInst* S = dyn_cast<StoreInst>(&I)) {
storeInsts++;
}
}
if (hasFPcall) {
numFPFuncs++;
}
numFuncs++;
}
dbgs() << "Num of funcs (total): " << numFuncs << "\n";
dbgs() << "Num of funcs (w/ fp calls): " << numFPFuncs << "\n";
dbgs() << "Num of funcs (addr. taken): " << numAddFuncs << "\n";
dbgs() << "Num of fp calls: " << numFPcalls << "\n";
dbgs() << "Num of instructions: " << numInsts << "\n";
dbgs() << INDENT_1 << "loads: " << loadInsts << "\n";
dbgs() << INDENT_1 << "stores: " << storeInsts << "\n";
dbgs() << INDENT_1 << "intrinsics: " << intrinsInsts << "\n";
}
for (Function& F : M.functions()) {
if (F.isDeclaration()) continue;
SDEBUG("soaap.analysis.infoflow.fp.infer", 3, dbgs() << F.getName() << "\n");
for (Instruction& I : instructions(&F)) {
ContextVector contexts = ContextUtils::getContextsForInstruction(&I, contextInsensitive, sandboxes, M);
if (StoreInst* S = dyn_cast<StoreInst>(&I)) { // assignments
//SDEBUG("soaap.analysis.infoflow.fp.infer", 3, dbgs() << F->getName() << ": " << *S);
Value* Rval = S->getValueOperand()->stripInBoundsConstantOffsets();
if (Function* T = dyn_cast<Function>(Rval)) {
fpTargetsUniv.insert(T);
// we are assigning a function
Value* Lvar = S->getPointerOperand()->stripInBoundsConstantOffsets();
for (Context* C : contexts) {
setBitVector(state[C][Lvar], T);
SDEBUG("soaap.analysis.infoflow.fp.infer", 3, dbgs() << "Adding " << Lvar->getName() << " to worklist\n");
addToWorklist(Lvar, C, worklist);
if (isa<GetElementPtrInst>(Lvar)) {
SDEBUG("soaap.analysis.infoflow.fp.infer", 3, dbgs() << "Rewinding back to alloca\n");
ValueSet visited;
propagateToAggregate(Lvar, C, Lvar, visited, worklist, sandboxes, M);
}
}
}
}
else if (SelectInst* S = dyn_cast<SelectInst>(&I)) {
if (Function* F = dyn_cast<Function>(S->getTrueValue()->stripPointerCasts())) {
addInferredFunction(F, contexts, S, worklist);
}
if (Function* F = dyn_cast<Function>(S->getFalseValue()->stripPointerCasts())) {
addInferredFunction(F, contexts, S, worklist);
}
}
else if (CallInst* C = dyn_cast<CallInst>(&I)) { // passing functions as params
if (Function* callee = CallGraphUtils::getDirectCallee(C)) {
int argIdx = 0;
for (Argument& AI : callee->args()) {
Value* Arg = C->getArgOperand(argIdx)->stripPointerCasts();
if (Function* T = dyn_cast<Function>(Arg)) {
addInferredFunction(T, contexts, &AI, worklist);
}
argIdx++;
}
}
}
}
}
SDEBUG("soaap.analysis.infoflow.fp.infer", 3, dbgs() << "Globals:\n");
// In some applications, functions are stored within globals aggregates like arrays
// We search for such arrays conflating any structure contained within
ValueSet visited;
for (GlobalVariable& G : M.globals()) {
findAllFunctionPointersInValue(&G, worklist, visited);
}
if (debug) {
dbgs() << "num of fp targets: " << fpTargetsUniv.size() << "\n";
}
}
static void
new_function_entry(void *addr, string *comment, bool isvisible, int call_count)
{
Function *f = new Function(addr, comment, isvisible, call_count);
function_entries.insert(pair<void*, Function*>(addr, f));
}
/**
* 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;
}
