/**
* Set the list of muscles to analyze.
*
* @param aMuscles is the array of names of muscles to analyze.
*/
void MuscleAnalysis::setMuscles(OpenSim::Array<std::string>& aMuscles)
{
int size = aMuscles.getSize();
_muscleListProp.getValueStrArray().setSize(aMuscles.getSize());
for(int i=0; i<size; i++){
_muscleListProp.getValueStrArray().updElt(i) = aMuscles.get(i);
}
}
void CustomJoint::constructCoordinates()
{
CoordinateSet& coordinateSet = upd_CoordinateSet();
const SpatialTransform& spatialTransform = getSpatialTransform();
OpenSim::Array<std::string> coordinateNames
= spatialTransform.getCoordinateNames();
// Check how many coordinates are required
int ncoords = coordinateNames.getSize();
for (int i = 0; i < ncoords; i++){
std::string coordName = spatialTransform.getCoordinateNames()[i];
int coordIndex = coordinateSet.getIndex(coordName);
Coordinate* coord = nullptr;
if (coordIndex < 0){
coord = new Coordinate();
coord->setName(coordName);
// Let joint take ownership as it should
coordinateSet.adoptAndAppend(coord);
}
else {
//if already in the set it was already specified
continue;
}
// Determine if the MotionType of the Coordinate based
// on which TransformAxis it is relate to 0-2 are Rotational
// and 3-5 are Translational. Coordinates appearing
// in both categories are Coupled
Coordinate::MotionType mt = Coordinate::MotionType::Coupled;
for (int j = 0; j < 3; ++j){
if (spatialTransform[j]
.getCoordinateNamesInArray().findIndex(coordName) >= 0){
mt = Coordinate::MotionType::Rotational;
break;
}
}
for (int j = 0; j < 3; ++j){
if (spatialTransform[j + 3]
.getCoordinateNamesInArray().findIndex(coordName) >= 0){
if (mt == Coordinate::MotionType::Rotational){
// already had a Rotational contribution so must be coupled
mt = Coordinate::MotionType::Coupled;
}
// otherwise just Translational
else mt = Coordinate::MotionType::Translational;
break;
}
}
coord->setMotionType(mt);
}
}
// Spatial Transform specific methods
OpenSim::Array<string> SpatialTransform::getCoordinateNames() const
{
OpenSim::Array<string> coordinateNames;
for(int i=0; i < NumTransformAxes; i++){
const TransformAxis& transform = getTransformAxis(i);
for(int j = 0; j < transform.getCoordinateNames().size(); j++){
string name = transform.getCoordinateNames()[j];
if(coordinateNames.findIndex(name) < 0)
coordinateNames.append(name);
}
}
return coordinateNames;
}
/**
* Obtain actuator equilibrium. A series of long (e.g., 200 msec) integrations
* are performed to allow time-dependent actuators forces to reach
* equilibrium values.
*
* @param tiReal Initial time expressed in real time units.
* @param dtReal Duration of the time interval.
* @param x Array of control values.
* @param y Array of states.
* @param hold Flag indicating whether or not to hold model coordinates
* constant (true) or let them change according to the desired trajectories
* (false).
*/
void CMC::
obtainActuatorEquilibrium(SimTK::State& s, double tiReal,double dtReal,
const OpenSim::Array<double> &x,bool hold)
{
// HOLD COORDINATES
if(hold) {
_predictor->getCMCActSubsys()->holdCoordinatesConstant(tiReal);
} else {
_predictor->getCMCActSubsys()->releaseCoordinates();
}
// INITIALIZE
_predictor->setInitialTime(tiReal);
_predictor->setFinalTime(tiReal+dtReal);
_predictor->getCMCActSubsys()->setCompleteState( s );
// INTEGRATE FORWARD
Array<double> f(0.0,x.getSize());
_predictor->evaluate(s, &x[0], &f[0]);
// update the muscle states
//_model->getForceSubsystem().updZ(s) =
//_model->getForceSubsystem().getZ(_predictor->getCMCActSubsys()->getCompleteState());
_model->updMultibodySystem().updDefaultSubsystem().updZ(s) =
_model->getMultibodySystem().getDefaultSubsystem().getZ(_predictor->getCMCActSubsys()->getCompleteState());
// RELEASE COORDINATES
_predictor->getCMCActSubsys()->releaseCoordinates();
}
void PathSpring::computeForce(const SimTK::State& s,
SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
SimTK::Vector& generalizedForces) const
{
const GeometryPath& path = getGeometryPath();
const double& tension = getTension(s);
OpenSim::Array<PointForceDirection*> PFDs;
path.getPointForceDirections(s, &PFDs);
for (int i=0; i < PFDs.getSize(); i++) {
applyForceToPoint(s, PFDs[i]->frame(), PFDs[i]->point(),
tension*PFDs[i]->direction(), bodyForces);
}
for(int i=0; i < PFDs.getSize(); i++)
delete PFDs[i];
}
/**
* Set the list of coordinates.
*
* @param aCoordinates Array of coordinates about which to compute moment arms.
*/
void MuscleAnalysis::
setCoordinates(OpenSim::Array<std::string>& aCoordinates)
{
int size = aCoordinates.getSize();
_coordinateListProp.getValueStrArray().setSize(size);
for(int i=0; i<size; i++){
_coordinateListProp.getValueStrArray().updElt(i) = aCoordinates[i];
}
}
/**
* Filter the controls. This method was introduced as a means of attempting
* to reduce the sizes of residuals. Unfortunately, the approach was
* generally unsuccessful because the desired accelerations were not
* achieved.
*
* @param aControlSet Set of controls computed by CMC.
* @param aDT Current time window.
* @param rControls Array of filtered controls.
*/
void CMC::
FilterControls(const SimTK::State& s, const ControlSet &aControlSet,double aDT,
OpenSim::Array<double> &rControls,bool aVerbosePrinting)
{
if(aDT <= SimTK::Zero) {
if(aVerbosePrinting) cout<<"\nCMC.filterControls: aDT is practically 0.0, skipping!\n\n";
return;
}
if(aVerbosePrinting) cout<<"\n\nFiltering controls to limit curvature...\n";
int i;
int size = rControls.getSize();
Array<double> x0(0.0,size),x1(0.0,size),x2(0.0,size);
// SET TIMES
double t0,t1/*,t2*/;
// t2 = s.getTime();
t1 = s.getTime() - aDT;
t0 = t1 - aDT;
// SET CONTROL VALUES
x2 = rControls;
aControlSet.getControlValues(t1,x1);
aControlSet.getControlValues(t0,x0);
// LOOP OVER CONTROLS
double m1,m2;
double curvature;
double thresholdCurvature = 2.0 * 0.05 / (aDT * aDT);
//double thresholdSlopeDiff = 0.2 / aDT;
for(i=0;i<size;i++) {
m2 = (x2[i]-x1[i]) / aDT;
m1 = (x1[i]-x0[i]) / aDT;
curvature = (m2 - m1) / aDT;
curvature = fabs(curvature);
if(curvature<=thresholdCurvature) continue;
// diff = fabs(m2) - fabs(m1);
// cout<<"thresholdSlopeDiff="<<thresholdSlopeDiff<<" slopeDiff="<<diff<<endl;
// if(diff>thresholdSlopeDiff) continue;
// ALTER CONTROL VALUE
rControls[i] = (3.0*x2[i] + 2.0*x1[i] + x0[i]) / 6.0;
// PRINT
if(aVerbosePrinting) cout<<aControlSet[i].getName()<<": old="<<x2[i]<<" new="<<rControls[i]<<endl;
}
if(aVerbosePrinting) cout<<endl<<endl;
}
/**
* 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());
}
}
}
void GeneralisedCoordinatesFromStorageFile::updateKinematicsSplines(double fc) {
//reinitialise because I modify the storage with padding and filtering..
OpenSim::Storage kinematics(kinematicsStorageFilename_);
if (fc > 0) {
kinematics.pad(kinematics.getSize() / 2.);
kinematics.lowpassIIR(fc);
}
if (kinematics.isInDegrees()){
model_.getSimbodyEngine().convertDegreesToRadians(kinematics);
}
// Create differentiable splines of the coordinate data
OpenSim::GCVSplineSet kinematicsSplines(5, &kinematics);
//Functions must correspond to model coordinates and their order for the solver
OpenSim::Array<string> coordinateNamesOS;
model_.getCoordinateSet().getNames(coordinateNamesOS);
for (size_t i(0); i < coordinateNamesOS.getSize(); ++i){
if (kinematicsSplines.contains(coordinateNamesOS.get(i))){
kinematicsSplines.insert(i, kinematicsSplines.get(coordinateNamesOS.get(i)));
}
else{
kinematicsSplines.insert(i, OpenSim::Constant(model_.getCoordinateSet().get(i).getDefaultValue()));
}
}
if (kinematicsSplines.getSize() > coordinateNamesOS.getSize()){
kinematicsSplines.setSize(coordinateNamesOS.getSize());
}
splines_ = kinematicsSplines;
}
void PrescribedForce::setForceFunctionNames
(const OpenSim::Array<std::string>& aFunctionNames,
const Storage& kineticsStore)
{
FunctionSet& forceFunctions = updForceFunctions();
int forceSize = kineticsStore.getSize();
if(forceSize<=0) return;
double *t=0;
// Expected column labels for the file
kineticsStore.getTimeColumn(t);
double *column=0;
SimmSpline** tSpline = new SimmSpline*[3];
for(int i=0;i<aFunctionNames.getSize();i++)
{
kineticsStore.getDataColumn(aFunctionNames[i], column);
tSpline[i]= new SimmSpline((forceSize>10?10:forceSize), t, column, aFunctionNames[i]);
}
setForceFunctions(tSpline[0], tSpline[1], tSpline[2]);
for (int i=0; i<aFunctionNames.getSize();i++)
forceFunctions[i].setName(aFunctionNames.get(i));
}
/**
* Helper function to recover Identifier based on the conventions used in earlier versions before identifiers were introduced
*/
std::string ExternalLoads::createIdentifier(OpenSim::Array<std::string>&oldFunctionNames, const Array<std::string>& labels)
{
if (oldFunctionNames.getSize()==0) return "";
// There was at least one function that could be specified by name or #
std::string firstFuncName = oldFunctionNames[0];
string fullIdentifier=firstFuncName;
if (firstFuncName.c_str()[0]=='#'){ // it's really a number, may imply the storage has duplicate labels
int columnOrder=-1;
sscanf(firstFuncName.c_str(), "#%d", &columnOrder);
fullIdentifier = labels[columnOrder];
}
return fullIdentifier.substr(0, fullIdentifier.length()-1);
}
//-----------------------------------------------------------------------------
// UPDATE FROM OLDER VERSION
//-----------------------------------------------------------------------------
//_____________________________________________________________________________
void ExternalLoads::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)
{
int documentVersion = versionNumber;
if ( documentVersion < 20301){
if (Object::getDebugLevel()>=1)
cout << "Updating ExternalLoad object to latest format..." << endl;
_dataFileName="";
SimTK::Xml::element_iterator dataFileElementIter =aNode.element_begin("datafile");
if(dataFileElementIter!=aNode.element_end()) {
// Could still be empty or whiteSpace
SimTK::String transcoded = dataFileElementIter->getValueAs<SimTK::String>();
if (transcoded.length()>0)
_dataFileName =transcoded;
}
SimTK::Xml::element_iterator kinFileNode = aNode.element_begin("external_loads_model_kinematics_file");
if (kinFileNode != aNode.element_end()){
SimTK::String transcoded = kinFileNode->getValueAs<SimTK::String>();
if (transcoded.length()>0)
_externalLoadsModelKinematicsFileName =transcoded;
}
SimTK::Xml::element_iterator kinFilterNode = aNode.element_begin("lowpass_cutoff_frequency_for_load_kinematics");
if (kinFilterNode != aNode.element_end()){
_lowpassCutoffFrequencyForLoadKinematics = kinFilterNode->getValueAs<double>();
}
bool changeWorkingDir = false;
std::string savedCwd;
// Change to directory of Document
if(!ifstream(_dataFileName.c_str(), ios_base::in).good()) {
string msg =
"Object: ERR- Could not open file " + _dataFileName+ "IO. It may not exist or you don't have permission to read it.";
cout << msg;
// Try switching to directory of setup file before aborting
if(getDocument()) {
savedCwd = IO::getCwd();
IO::chDir(IO::getParentDirectory(getDocument()->getFileName()));
changeWorkingDir=true;
}
if(!ifstream(_dataFileName.c_str(), ios_base::in).good()) {
if(changeWorkingDir) IO::chDir(savedCwd);
throw Exception(msg,__FILE__,__LINE__);
}
}
Storage* dataSource = new Storage(_dataFileName, true);
if (!dataSource->makeStorageLabelsUnique()){
cout << "Making labels unique in storage file "<< _dataFileName << endl;
dataSource = new Storage(_dataFileName);
dataSource->makeStorageLabelsUnique();
dataSource->print(_dataFileName);
}
if(changeWorkingDir) IO::chDir(savedCwd);
const Array<string> &labels = dataSource->getColumnLabels();
// Populate data file and other things that haven't changed
// Create a ForceSet out of this XML node, this will create a set of PrescribedForces then we can reassign at a higher level to ExternalForces
ModelComponentSet<PrescribedForce> oldForces(updModel(), getDocument()->getFileName(), true);
for(int i=0; i< oldForces.getSize(); i++){
PrescribedForce& oldPrescribedForce = oldForces.get(i);
ExternalForce* newExternalForce = new ExternalForce();
newExternalForce->setName(oldPrescribedForce.getName());
newExternalForce->setAppliedToBodyName(oldPrescribedForce.getBodyName());
newExternalForce->setPointExpressedInBodyName("ground");
newExternalForce->setForceExpressedInBodyName("ground");
// Reconstruct function names and use these to extract the identifier(s)
OpenSim::Array<std::string> aFunctionNames;
oldPrescribedForce.getForceFunctionNames(aFunctionNames);
// Get names from force functions
newExternalForce->setForceIdentifier(createIdentifier(aFunctionNames, labels));
aFunctionNames.setSize(0);
oldPrescribedForce.getPointFunctionNames(aFunctionNames);
newExternalForce->setPointIdentifier(createIdentifier(aFunctionNames, labels));
aFunctionNames.setSize(0);
// Now the torques
oldPrescribedForce.getTorqueFunctionNames(aFunctionNames);
newExternalForce->setTorqueIdentifier(createIdentifier(aFunctionNames, labels));
//newExternalForce->setDataSourceName("");
adoptAndAppend(newExternalForce);
}
delete dataSource;
}
else
// Call base class now assuming _node has been corrected for current version
ModelComponentSet<ExternalForce>::updateFromXMLNode(aNode, versionNumber);
}
Array<std::string> MuscleOptimizer::getJointSpannedByMuscle(Model& model, const string& muscleName)
{
OpenSim::Array<std::string> jointNames;
OpenSim::BodySet bodySet(model.getBodySet());
OpenSim::Set<OpenSim::Muscle> muscles = model.getMuscles();
OpenSim::Muscle& muscle = muscles.get(muscleName);
OpenSim::GeometryPath musclePath(muscle.getGeometryPath());
OpenSim::PathPointSet musclePathPointSet(musclePath.getPathPointSet());
// check if there is a simple kinematic chain between geometry path extremes
int firstBodyIndex = 0;
const OpenSim::JointSet& jointSet = model.getJointSet();
// try to use consider first path point as on the proximal body, and last path point on the distal body
OpenSim::Body& lastBody = musclePathPointSet.get(musclePathPointSet.getSize() - 1).getBody();
OpenSim::Body& firstBody = musclePathPointSet.get(firstBodyIndex).getBody();
std::string currentBodyName(lastBody.getName());
std::string proximalBodyName(firstBody.getName());
while (currentBodyName != proximalBodyName && currentBodyName !="ground") {
for (int i = 0; i < jointSet.getSize(); i++)
{
if (jointSet.get(i).getBody().getName() == currentBodyName)
{
currentBodyName = jointSet.get(i).getParentBody().getName();
continue;
}
}
}
if (currentBodyName == "ground") {
// reverse - consider first path point as on distal body
currentBodyName = firstBody.getName();
proximalBodyName = lastBody.getName();
while (currentBodyName != proximalBodyName && currentBodyName != "ground") {
for (int i = 0; i < jointSet.getSize(); i++)
{
if (jointSet.get(i).getBody().getName() == currentBodyName)
{
currentBodyName = jointSet.get(i).getParentBody().getName();
continue;
}
}
}
if (currentBodyName == "ground") {
std::cout << "WARNING: Geometry Path for muscle " << muscleName << " does not follow a simple open kinematic chain" << std::endl;
return jointNames;
}
else {
currentBodyName = firstBody.getName();
}
}
else {
currentBodyName = lastBody.getName();
}
while (currentBodyName != proximalBodyName) {
for (int i = 0; i < jointSet.getSize(); i++)
{
if (jointSet.get(i).getBody().getName() == currentBodyName)
{
if (jointSet.get(i).getCoordinateSet().getSize() != 0)
jointNames.append(jointSet.get(i).getName());
currentBodyName = jointSet.get(i).getParentBody().getName();
continue;
}
}
}
return jointNames;
}
void AnalysisSet::
setOn(const OpenSim::Array<bool> &aOn)
{
if(aOn.getSize()!=getSize()) throw Exception("AnalysisSet.setOn: ERROR- incompatible array sizes",__FILE__,__LINE__);
for(int i=0; i<getSize(); i++) get(i).setOn(aOn[i]);
}
