/**
 * 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);
    }
}
Esempio n. 2
0
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;
}
Esempio n. 4
0
/**
 * 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();
}
Esempio n. 5
0
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];
    }
}
Esempio n. 7
0
/**
 * 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;
}
Esempio n. 8
0
/**
 * 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));
}
Esempio n. 11
0
/**
 * 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);
    
}
Esempio n. 12
0
//-----------------------------------------------------------------------------
// 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]);
}