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