/**
This function computes a standard central difference dy/dx. If
extrap_endpoints is set to 1, then the derivative at the end points is
estimated by linearly extrapolating the dy/dx values beside the end points
@param x domain vector
@param y range vector
@param extrap_endpoints: (false) Endpoints of the returned vector will be
zero, because a central difference
is undefined at these endpoints
(true) Endpoints are computed by linearly
extrapolating using a first difference from
the neighboring 2 points
@returns dy/dx computed using central differences
*/
SimTK::Vector calcCentralDifference(const SimTK::Vector& x,
const SimTK::Vector& y,
bool extrap_endpoints){
SimTK::Vector dy(x.size());
double dx1,dx2;
double dy1,dy2;
int size = x.size();
for(int i=1; i<x.size()-1; i++){
dx1 = x(i)-x(i-1);
dx2 = x(i+1)-x(i);
dy1 = y(i)-y(i-1);
dy2 = y(i+1)-y(i);
dy(i)= 0.5*dy1/dx1 + 0.5*dy2/dx2;
}
if(extrap_endpoints == true){
dy1 = dy(2)-dy(1);
dx1 = x(2)-x(1);
dy(0) = dy(1) + (dy1/dx1)*(x(0)-x(1));
dy2 = dy(size-2)-dy(size-3);
dx2 = x(size-2)-x(size-3);
dy(size-1) = dy(size-2) + (dy2/dx2)*(x(size-1)-x(size-2));
}
return dy;
}
/** Solve the inverse dynamics system of equations for generalized coordinate forces, Tau.
Applied loads are computed by the model from the state. */
Vector InverseDynamicsSolver::solve(SimTK::State &s, const SimTK::Vector &udot)
{
// Default is a statics inverse dynamics analysis, udot = 0;
Vector knownUdots(getModel().getNumSpeeds(), 0.0);
// If valid accelerations provided then use for inverse dynamics
if(udot.size() == getModel().getNumSpeeds()){
knownUdots = udot;
}
else if (udot.size() != 0){
throw Exception("InverseDynamicsSolver::solve with input 'udot' of invalid size.");
}
// Realize to dynamics stage so that all model forces are computed
getModel().getMultibodySystem().realize(s,SimTK::Stage::Dynamics);
// Get applied mobility (generalized) forces generated by components of the model, like actuators
const Vector &appliedMobilityForces = getModel().getMultibodySystem().getMobilityForces(s, Stage::Dynamics);
// Get all applied body forces like those from conact
const Vector_<SpatialVec>& appliedBodyForces = getModel().getMultibodySystem().getRigidBodyForces(s, Stage::Dynamics);
// Perform general inverse dynamics
return solve(s, knownUdots, appliedMobilityForces, appliedBodyForces);
}
void compareSimulationStates(SimTK::Vector q_sb, SimTK::Vector u_sb,
SimTK::Vector q_osim, SimTK::Vector u_osim,
string errorMessagePrefix = "")
{
using namespace SimTK;
Vector q_err = q_sb;
Vector u_err = u_sb;
int nq = q_osim.size();
if(q_sb.size() > nq){ // we have an unused quaternion slot in Simbody
q_sb.dump("Simbody q's:");
q_osim.dump("OpenSim q's:");
//This is a hack knowing the free and ball joint tests have the
// quaternion q's first and that the q's are packed as qqqq or aaa*
// for a ball and qqqqxyz or aaaxyz* for a free joint
int quat_ind = ((nq > 6) ? 6 : 3);
int j = 0;
for(int i=0; i< q_sb.size(); i++){
if(i != quat_ind){
q_err[j] = q_sb[i] - q_osim[j];
j++;
}
}
}
else{
if(nq > q_sb.size()){ // OpenSim using constrains
q_err[0] = q_sb[0] - q_osim[0];
q_err[1] = q_sb[1] - q_osim[1];
u_err[0] = u_sb[0] - u_osim[0];
u_err[1] = u_sb[1] - u_osim[1];
}
else{
q_err = q_sb - q_osim;
u_err = u_sb - u_osim;
}
}
//cout<<"\nSimbody - OpenSim:"<<endl;
//q_err.dump("Diff q's:");
//u_err.dump("Diff u's:");
stringstream errorMessage1, errorMessage2;
errorMessage1 << "testConstraints compareSimulationStates failed q_err.norm = " << q_err.norm();
errorMessage2 << "testConstraints compareSimulationStates failed u_err.norm = " << u_err.norm();
ASSERT(q_err.norm() <= 10*integ_accuracy, __FILE__, __LINE__, errorMessagePrefix + errorMessage1.str());
ASSERT(u_err.norm() <= 20*integ_accuracy, __FILE__, __LINE__, errorMessagePrefix + errorMessage2.str());
}
/**
* initialize storages and analyses
*
* @param s system state before integration
*/
void Manager::initialize(SimTK::State& s, double dt )
{
// skip initializations for CMC's actuator system
if( _writeToStorage && _performAnalyses ) {
double tReal = s.getTime();
// STORE STARTING CONTROLS
if (_model->isControlled()){
_controllerSet->setModel(*_model);
_controllerSet->storeControls(s, 0);
}
// STORE STARTING STATES
if(hasStateStorage()) {
// ONLY IF NO STATES WERE PREVIOUSLY STORED
if(getStateStorage().getSize()==0) {
SimTK::Vector stateValues = _model->getStateVariableValues(s);
getStateStorage().store(0,tReal,stateValues.size(), &stateValues[0]);
}
}
// ANALYSES
AnalysisSet& analysisSet = _model->updAnalysisSet();
analysisSet.begin(s);
}
return;
}
//==============================================================================
// Common Functions
//==============================================================================
int initTestStates(SimTK::Vector &qi, SimTK::Vector &ui)
{
using namespace SimTK;
Random::Uniform randomAngle(-Pi/4, Pi/4);
Random::Uniform randomSpeed(-1.0, 1.0);
// Provide initial states as random angles and speeds for OpenSim and
// Simbody models
for(int i = 0; i<qi.size(); i++)
qi[i] = randomAngle.getValue();
for(int i = 0; i<ui.size(); i++)
ui[i] = randomSpeed.getValue();
return qi.size();
}
/* Calculate the equivalent spatial force, FB_G, acting on the body connected by this joint at
its location B, expressed in ground. */
SimTK::SpatialVec Joint::calcEquivalentSpatialForce(const SimTK::State &s, const SimTK::Vector &mobilityForces) const
{
// The number of mobilities for the entire system.
int nm = _model->getMatterSubsystem().getNumMobilities();
if(nm != mobilityForces.size()){
throw Exception("Joint::calcEquivalentSpatialForce(): input mobilityForces does not match model's mobilities");
}
const SimTK::MobilizedBodyIndex &mbx = getChildBody().getIndex();
// build a unique list of underlying MobilizedBodies that are involved
// with this Joint in addition to and not including that of the child body
std::set<SimTK::MobilizedBodyIndex> mbds;
const CoordinateSet& coordinateSet = get_CoordinateSet();
for(int i=0; i<coordinateSet.getSize(); ++i){
const MobilizedBodyIndex& coordsMbx = coordinateSet[i].getBodyIndex();
if (coordsMbx != mbx){
mbds.insert(coordsMbx);
}
}
SimTK::SpatialVec FB_G = calcEquivalentSpatialForceForMobilizedBody(s, mbx, mobilityForces);
SimTK::SpatialVec FBx_G;
std::set<SimTK::MobilizedBodyIndex>::const_iterator it = mbds.begin();
const SimTK::MobilizedBody &G = getModel().getMatterSubsystem().getGround();
const SimTK::MobilizedBody &B = getModel().getMatterSubsystem().getMobilizedBody(mbx);
SimTK::Vec3 r_BG =
B.expressVectorInAnotherBodyFrame(s, B.getOutboardFrame(s).p(), G);
while(it != mbds.end()){
FBx_G = calcEquivalentSpatialForceForMobilizedBody(s, *it, mobilityForces);
const SimTK::MobilizedBody &b =
getModel().getMatterSubsystem().getMobilizedBody(*it);
SimTK::Vec3 r_bG =
b.expressVectorInAnotherBodyFrame(s, b.getOutboardFrame(s).p(), G);
// Torques add and include term due to offset in forces
FB_G += FBx_G; // shiftForceFromTo(FBx_G, r_bG, r_BG);
++it;
}
return FB_G;
}
/*
Multiplications Additions Assignments
21 20 13
*/
double SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveVal(double u, const SimTK::Vector& pts)
{
double val = -1;
SimTK_ERRCHK1_ALWAYS( (u>=0 && u <= 1) ,
"SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal",
"Error: double argument u must be between 0.0 and 1.0"
"but %f was entered.",u);
SimTK_ERRCHK_ALWAYS( (pts.size() == 6) ,
"SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal",
"Error: vector argument pts must have a length of 6.");
//Compute the Bezier point
double p0 = pts(0);
double p1 = pts(1);
double p2 = pts(2);
double p3 = pts(3);
double p4 = pts(4);
double p5 = pts(5);
double u5 = 1;
double u4 = u;
double u3 = u4*u;
double u2 = u3*u;
double u1 = u2*u;
double u0 = u1*u;
//See lines 1-6 of MuscleCurveCodeOpt_20120210
double t2 = u1 * 0.5e1;
double t3 = u2 * 0.10e2;
double t4 = u3 * 0.10e2;
double t5 = u4 * 0.5e1;
double t9 = u0 * 0.5e1;
double t10 = u1 * 0.20e2;
double t11 = u2 * 0.30e2;
double t15 = u0 * 0.10e2;
val = p0 * (u0 * (-0.1e1) + t2 - t3 + t4 - t5 + u5 * 0.1e1)
+ p1 * (t9 - t10 + t11 + u3 * (-0.20e2) + t5)
+ p2 * (-t15 + u1 * 0.30e2 - t11 + t4)
+ p3 * (t15 - t10 + t3)
+ p4 * (-t9 + t2) + p5 * u0 * 0.1e1;
return val;
}
/*
Comp Div Mult Additions Assignments
calcIdx 3*3+2=11 1*3=3 3
calcU 15 2 82 42 60
calcQuinticBezierCurveVal
21 20 13
Total 26 2 103 65 76
\endverbatim
Ignoring the costs associated with the integrator itself, and assuming
that the integrator evaluates the function 6 times per integrated point,
the cost of evaluating the integral at each point in vX is:
\verbatim
Comp Div Mult Additions Assignments
RK45 on 1pt 6*(26 2 103 65 76)
Total 156 12 618 390 456
\endverbatim
Typically the integral is evaluated 100 times per section in order to
build an accurate spline-fit of the integrated function. Once again,
ignoring the overhead of the integrator, the function evaluations alone
for the current example would be
\verbatim
RK45 on 100pts per section, over 3 sections
Comp Div Mult Additions Assignments
3*100*(156 12 618 390 456
Total 46,800 3600 185,400 117,000 136,000
*/
SimTK::Matrix SegmentedQuinticBezierToolkit::calcNumIntBezierYfcnX(
const SimTK::Vector& vX,
double ic0, double intAcc,
double uTol, int uMaxIter,
const SimTK::Matrix& mX, const SimTK::Matrix& mY,
const SimTK::Array_<SimTK::Spline>& aSplineUX,
bool flag_intLeftToRight,
const std::string& caller)
{
SimTK::Matrix intXY(vX.size(),2);
BezierData bdata;
bdata._mX = mX;
bdata._mY = mY;
bdata._initalValue = ic0;
bdata._aArraySplineUX = aSplineUX;
bdata._uMaxIter = uMaxIter;
bdata._uTol = uTol;
bdata._flag_intLeftToRight = flag_intLeftToRight;
bdata._name = caller;
//These aren't really times, but I'm perpetuating the SimTK language
//so that I don't make a mistake
double startTime = vX(0);
double endTime = vX(vX.size()-1);
if(flag_intLeftToRight){
bdata._startValue = startTime;
}else{
bdata._startValue = endTime;
}
MySystem sys(bdata);
State initState = sys.realizeTopology();
initState.setTime(startTime);
RungeKuttaMersonIntegrator integ(sys);
integ.setAccuracy(intAcc);
integ.setFinalTime(endTime);
integ.setReturnEveryInternalStep(false);
integ.initialize(initState);
int idx = 0;
double nextTimeInterval = 0;
Integrator::SuccessfulStepStatus status;
while (idx < vX.nelt()) {
if(idx < vX.nelt()){
if(flag_intLeftToRight){
nextTimeInterval = vX(idx);
}else{
nextTimeInterval = endTime-vX(vX.size()-idx-1);
}
}
status=integ.stepTo(nextTimeInterval);
// Use this for variable step output.
//status = integ.stepTo(Infinity);
if (status == Integrator::EndOfSimulation)
break;
const State& state = integ.getState();
if(flag_intLeftToRight){
intXY(idx,0) = nextTimeInterval;
intXY(idx,1) = (double)state.getZ()[0];
}else{
intXY(vX.size()-idx-1,0) = vX(vX.size()-idx-1);
intXY(vX.size()-idx-1,1) = (double)state.getZ()[0];
}
idx++;
}
//intXY.resizeKeep(idx,2);
return intXY;
}
/* Computational Cost Details
Divisions Multiplications Additions Assignments
dx/du 20 19 11
d2x/du2 17 17 9
d3y/du3 14 14 6
*/
double SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU(double u,
const SimTK::Vector& pts,int order)
{
double val = -1;
SimTK_ERRCHK_ALWAYS( (u>=0 && u <= 1) ,
"SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU",
"Error: double argument u must be between 0.0 and 1.0.");
SimTK_ERRCHK_ALWAYS( (pts.size()==6) ,
"SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU",
"Error: vector argument pts \nmust have a length of 6.");
SimTK_ERRCHK_ALWAYS( (order >= 1),
"SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU",
"Error: order must be greater than, or equal to 1");
//Compute the Bezier point
double p0 = pts(0);
double p1 = pts(1);
double p2 = pts(2);
double p3 = pts(3);
double p4 = pts(4);
double p5 = pts(5);
switch(order){
case 1:
{
double t1 = u*u;//u ^ 2;
double t2 = t1*t1;//t1 ^ 2;
double t4 = t1 * u;
double t5 = t4 * 0.20e2;
double t6 = t1 * 0.30e2;
double t7 = u * 0.20e2;
double t10 = t2 * 0.25e2;
double t11 = t4 * 0.80e2;
double t12 = t1 * 0.90e2;
double t16 = t2 * 0.50e2;
val = p0 * (t2 * (-0.5e1) + t5 - t6 + t7 - 0.5e1)
+ p1 * (t10 - t11 + t12 + u * (-0.40e2) + 0.5e1)
+ p2 * (-t16 + t4 * 0.120e3 - t12 + t7)
+ p3 * (t16 - t11 + t6)
+ p4 * (-t10 + t5)
+ p5 * t2 * 0.5e1;
}
break;
case 2:
{
double t1 = u*u;//u ^ 2;
double t2 = t1 * u;
double t4 = t1 * 0.60e2;
double t5 = u * 0.60e2;
double t8 = t2 * 0.100e3;
double t9 = t1 * 0.240e3;
double t10 = u * 0.180e3;
double t13 = t2 * 0.200e3;
val = p0 * (t2 * (-0.20e2) + t4 - t5 + 0.20e2)
+ p1 * (t8 - t9 + t10 - 0.40e2)
+ p2 * (-t13 + t1 * 0.360e3 - t10 + 0.20e2)
+ p3 * (t13 - t9 + t5)
+ p4 * (-t8 + t4)
+ p5 * t2 * 0.20e2;
}
break;
case 3:
{
double t1 = u*u;//u ^ 2;
double t3 = u * 0.120e3;
double t6 = t1 * 0.300e3;
double t7 = u * 0.480e3;
double t10 = t1 * 0.600e3;
val = p0 * (t1 * (-0.60e2) + t3 - 0.60e2)
+ p1 * (t6 - t7 + 0.180e3)
+ p2 * (-t10 + u * 0.720e3 - 0.180e3)
+ p3 * (t10 - t7 + 0.60e2)
+ p4 * (-t6 + t3)
+ p5 * t1 * 0.60e2;
}
break;
case 4:
{
double t4 = u * 0.600e3;
double t7 = u * 0.1200e4;
val = p0 * (u * (-0.120e3) + 0.120e3)
+ p1 * (t4 - 0.480e3)
+ p2 * (-t7 + 0.720e3)
+ p3 * (t7 - 0.480e3)
+ p4 * (-t4 + 0.120e3)
+ p5 * u * 0.120e3;
}
break;
case 5:
{
val = p0 * (-0.120e3)
+ p1 * 0.600e3
+ p2 * (-0.1200e4)
+ p3 * 0.1200e4
+ p4 * (-0.600e3)
+ p5 * 0.120e3;
}
break;
default:
val=0;
}
return val;
}
/*
Detailed Computational Costs
dy/dx Divisions Multiplications Additions Assignments
dy/du 20 19 11
dx/du 20 19 11
dy/dx 1
total 1 40 38 22
d2y/dx2 Divisions Multiplications Additions Assignments
dy/du 20 19 11
dx/du 20 19 11
d2y/du2 17 17 9
d2x/du2 17 17 9
d2y/dx2 2 4 1 3
total 2 78 73 23
d3y/dx3 Divisions Multiplications Additions Assignments
dy/du 20 19 11
dx/du 20 19 11
d2y/du2 17 17 9
d2x/du2 17 17 9
d3y/du3 14 14 6
d3x/du3 14 14 6
d3y/dx3 4 16 5 6
total 4 118 105 58
d4y/dx4 Divisions Multiplications Additions Assignments
dy/du 20 19 11
dx/du 20 19 11
d2y/du2 17 17 9
d2x/du2 17 17 9
d3y/du3 14 14 6
d3x/du3 14 14 6
d4y/du4 11 11 3
d4x/du4 11 11 3
d4y/dx4 5 44 15 13
total 5 168 137 71
d5y/dx5 Divisions Multiplications Additions Assignments
dy/du 20 19 11
dx/du 20 19 11
d2y/du2 17 17 9
d2x/du2 17 17 9
d3y/du3 14 14 6
d3x/du3 14 14 6
d4y/du4 11 11 3
d4x/du4 11 11 3
d5y/du5 6 6 1
d5x/du5 6 6 1
d5y/dx5 7 100 36 28
total 7 236 170 88
d6y/dx6
dy/du 20 19 11
dx/du 20 19 11
d2y/du2 17 17 9
d2x/du2 17 17 9
d3y/du3 14 14 6
d3x/du3 14 14 6
d4y/du4 11 11 3
d4x/du4 11 11 3
d5y/du5 6 6 1
d5x/du5 6 6 1
d6y/dx6 9 198 75 46
total 9 334 209 106
*/
double SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivDYDX(double u,
const SimTK::Vector& xpts, const SimTK::Vector& ypts, int order)
{
double val = SimTK::NaN;
//Bounds checking on the input
SimTK_ERRCHK_ALWAYS( (u>=0 && u <= 1) ,
"SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU",
"Error: double argument u must be between 0.0 and 1.0.");
SimTK_ERRCHK_ALWAYS( (xpts.size()==6) ,
"SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU",
"Error: vector argument xpts \nmust have a length of 6.");
SimTK_ERRCHK_ALWAYS( (ypts.size()==6) ,
"SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU",
"Error: vector argument ypts \nmust have a length of 6.");
SimTK_ERRCHK_ALWAYS( (order >= 1),
"SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU",
"Error: order must be greater than.");
SimTK_ERRCHK_ALWAYS( (order <= 6),
"SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU",
"Error: order must be less than, or equal to 6.");
//std::string localCaller = caller;
//localCaller.append(".calcQuinticBezierCurveDerivDYDX");
//Compute the derivative d^n y/ dx^n
switch(order){
case 1: //Calculate dy/dx
{
double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1);
double dydu =calcQuinticBezierCurveDerivU(u,ypts,1);
double dydx = dydu/dxdu;
val = dydx;
//Question:
//how is a divide by zero treated? Is SimTK::INF returned?
}
break;
case 2: //Calculate d^2y/dx^2
{
double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1);
double dydu =calcQuinticBezierCurveDerivU(u,ypts,1);
double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2);
double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2);
//Optimized code from Maple -
//see MuscleCurveCodeOpt_20120210 for details
double t1 = 0.1e1 / dxdu;
double t3 = dxdu*dxdu;//dxdu ^ 2;
double d2ydx2 = (d2ydu2 * t1 - dydu / t3 * d2xdu2) * t1;
val = d2ydx2;
}
break;
case 3: //Calculate d^3y/dx^3
{
double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1);
double dydu =calcQuinticBezierCurveDerivU(u,ypts,1);
double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2);
double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2);
double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3);
double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3);
double t1 = 1 / dxdu;
double t3 = dxdu*dxdu;//(dxdu ^ 2);
double t4 = 1 / t3;
double t11 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2);
double t14 = (dydu * t4);
double d3ydx3 = ((d3ydu3 * t1 - 2 * d2ydu2 * t4 * d2xdu2
+ 2 * dydu / t3 / dxdu * t11 - t14 * d3xdu3) * t1
- (d2ydu2 * t1 - t14 * d2xdu2) * t4 * d2xdu2) * t1;
val = d3ydx3;
}
break;
case 4: //Calculate d^4y/dx^4
{
double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1);
double dydu =calcQuinticBezierCurveDerivU(u,ypts,1);
double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2);
double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2);
double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3);
double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3);
double d4xdu4=calcQuinticBezierCurveDerivU(u,xpts,4);
double d4ydu4=calcQuinticBezierCurveDerivU(u,ypts,4);
double t1 = 1 / dxdu;
double t3 = dxdu*dxdu;//dxdu ^ 2;
double t4 = 1 / t3;
double t9 = (0.1e1 / t3 / dxdu);
double t11 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2);
double t14 = (d2ydu2 * t4);
double t17 = t3*t3;//(t3 ^ 2);
double t23 = (dydu * t9);
double t27 = (dydu * t4);
double t37 = d3ydu3 * t1 - 2 * t14 * d2xdu2 + 2 * t23 * t11
- t27 * d3xdu3;
double t43 = d2ydu2 * t1 - t27 * d2xdu2;
double t47 = t43 * t4;
double d4ydx4 = (((d4ydu4 * t1 - 3 * d3ydu3 * t4 * d2xdu2
+ 6 * d2ydu2 * t9 * t11 - 3 * t14 * d3xdu3
- 6 * dydu / t17 * t11 * d2xdu2 + 6 * t23 * d2xdu2 * d3xdu3
- t27 * d4xdu4) * t1 - 2 * t37 * t4 * d2xdu2
+ 2 * t43 * t9 * t11 - t47 * d3xdu3) * t1
- (t37 * t1 - t47 * d2xdu2) * t4 * d2xdu2) * t1;
val = d4ydx4;
}
break;
case 5:
{
double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1);
double dydu =calcQuinticBezierCurveDerivU(u,ypts,1);
double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2);
double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2);
double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3);
double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3);
double d4xdu4=calcQuinticBezierCurveDerivU(u,xpts,4);
double d4ydu4=calcQuinticBezierCurveDerivU(u,ypts,4);
double d5xdu5=calcQuinticBezierCurveDerivU(u,xpts,5);
double d5ydu5=calcQuinticBezierCurveDerivU(u,ypts,5);
double t1 = 1 / dxdu;
double t3 = dxdu*dxdu;//dxdu ^ 2;
double t4 = 1 / t3;
double t9 = (0.1e1 / t3 / dxdu);
double t11 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2);
double t14 = (d3ydu3 * t4);
double t17 = t3*t3;//(t3 ^ 2);
double t18 = 1 / t17;
double t20 = (t11 * d2xdu2);
double t23 = (d2ydu2 * t9);
double t24 = (d2xdu2 * d3xdu3);
double t27 = (d2ydu2 * t4);
double t33 = t11*t11;//(t11 ^ 2);
double t36 = (dydu * t18);
double t40 = (dydu * t9);
double t41 = d3xdu3*d3xdu3;//(d3xdu3 ^ 2);
double t47 = (dydu * t4);
double t49 = d5ydu5 * t1 - 4 * d4ydu4 * t4 * d2xdu2 + 12 * d3ydu3 * t9 * t11 - 6 * t14 * d3xdu3 - 24 * d2ydu2 * t18 * t20 + 24 * t23 * t24 - 4 * t27 * d4xdu4 + 24 * dydu / t17 / dxdu * t33 - 36 * t36 * t11 * d3xdu3 + 6 * t40 * t41 + 8 * t40 * d2xdu2 * d4xdu4 - t47 * d5xdu5;
double t63 = d4ydu4 * t1 - 3 * t14 * d2xdu2 + 6 * t23 * t11 - 3 * t27 * d3xdu3 - 6 * t36 * t20 + 6 * t40 * t24 - t47 * d4xdu4;
double t73 = d3ydu3 * t1 - 2 * t27 * d2xdu2 + 2 * t40 * t11 - t47 * d3xdu3;
double t77 = t73 * t4;
double t82 = d2ydu2 * t1 - t47 * d2xdu2;
double t86 = t82 * t9;
double t89 = t82 * t4;
double t99 = t63 * t1 - 2 * t77 * d2xdu2 + 2 * t86 * t11 - t89 * d3xdu3;
double t105 = t73 * t1 - t89 * d2xdu2;
double t109 = t105 * t4;
double d5ydx5 = (((t49 * t1 - 3 * t63 * t4 * d2xdu2
+ 6 * t73 * t9 * t11 - 3 * t77 * d3xdu3 - 6 * t82 * t18 * t20
+ 6 * t86 * t24 - t89 * d4xdu4) * t1 - 2 * t99 * t4 * d2xdu2
+ 2 * t105 * t9 * t11 - t109 * d3xdu3) * t1
- (t99 * t1 - t109 * d2xdu2) * t4 * d2xdu2) * t1;
val = d5ydx5;
}
break;
case 6:
{
double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1);
double dydu =calcQuinticBezierCurveDerivU(u,ypts,1);
double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2);
double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2);
double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3);
double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3);
double d4xdu4=calcQuinticBezierCurveDerivU(u,xpts,4);
double d4ydu4=calcQuinticBezierCurveDerivU(u,ypts,4);
double d5xdu5=calcQuinticBezierCurveDerivU(u,xpts,5);
double d5ydu5=calcQuinticBezierCurveDerivU(u,ypts,5);
double d6xdu6=calcQuinticBezierCurveDerivU(u,xpts,6);
double d6ydu6=calcQuinticBezierCurveDerivU(u,ypts,6);
double t1 = dxdu*dxdu;//(dxdu ^ 2);
double t3 = (0.1e1 / t1 / dxdu);
double t5 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2);
double t8 = t1*t1;//(t1 ^ 2);
double t9 = 1 / t8;
double t11 = (t5 * d2xdu2);
double t14 = (d3ydu3 * t3);
double t15 = (d2xdu2 * d3xdu3);
double t19 = (0.1e1 / t8 / dxdu);
double t21 = t5*t5;//(t5 ^ 2);
double t24 = (d2ydu2 * t9);
double t25 = (t5 * d3xdu3);
double t28 = (d2ydu2 * t3);
double t29 = d3xdu3*d3xdu3;//(d3xdu3 ^ 2);
double t32 = (d2xdu2 * d4xdu4);
double t41 = (dydu * t19);
double t45 = (dydu * t9);
double t49 = (dydu * t3);
double t56 = 1 / dxdu;
double t61 = 1 / t1;
double t62 = (dydu * t61);
double t67 = (d4ydu4 * t61);
double t70 = (d2ydu2 * t61);
double t73 = (d3ydu3 * t61);
double t76 = 20 * d4ydu4 * t3 * t5 - 60 * d3ydu3 * t9 * t11
+ 60 * t14 * t15 + 120 * d2ydu2 * t19 * t21 - 180 * t24 * t25
+ 30 * t28 * t29 + 40 * t28 * t32
- 120 * dydu / t8 / t1 * t21 * d2xdu2 + 240 * t41 *t11*d3xdu3
- 60 * t45 * t5 * d4xdu4 + 20 * t49 * d3xdu3 * d4xdu4
+ 10 * t49 * d2xdu2 * d5xdu5 + d6ydu6 * t56
- 90 * t45 * d2xdu2 * t29 - t62 * d6xdu6
- 5 * d5ydu5 * t61 * d2xdu2 - 10 * t67 * d3xdu3
- 5 * t70 * d5xdu5 - 10 * t73 * d4xdu4;
double t100 = d5ydu5 * t56 - 4 * t67 * d2xdu2 + 12 * t14 * t5
- 6 * t73 * d3xdu3 - 24 * t24 * t11 + 24 * t28 * t15
- 4 * t70 * d4xdu4 + 24 * t41 * t21 - 36 * t45 * t25
+ 6 * t49 * t29 + 8 * t49 * t32 - t62 * d5xdu5;
double t116 = d4ydu4 * t56 - 3 * t73 * d2xdu2 + 6 * t28 * t5
- 3 * t70 * d3xdu3 - 6 * t45 * t11 + 6 * t49 * t15
- t62 * d4xdu4;
double t120 = t116 * t61;
double t129 = d3ydu3 * t56 - 2 * t70 * d2xdu2 + 2 * t49 * t5
- t62 * d3xdu3;
double t133 = t129 * t3;
double t136 = t129 * t61;
double t141 = d2ydu2 * t56 - t62 * d2xdu2;
double t145 = t141 * t9;
double t148 = t141 * t3;
double t153 = t141 * t61;
double t155 = t76 * t56 - 4 * t100 * t61 * d2xdu2
+ 12 * t116 * t3 * t5 - 6 * t120 * d3xdu3
- 24 * t129 * t9 * t11 + 24 * t133 * t15 - 4 * t136 * d4xdu4
+ 24 * t141 * t19 * t21 - 36 * t145 * t25 + 6 * t148 * t29
+ 8 * t148 * t32 - t153 * d5xdu5;
double t169 = t100 * t56 - 3 * t120 * d2xdu2 + 6 * t133 * t5
- 3 * t136 * d3xdu3 - 6 * t145 * t11 + 6 * t148 * t15
- t153 * d4xdu4;
double t179 = t116 * t56 - 2 * t136 * d2xdu2 + 2 * t148 * t5
- t153 * d3xdu3;
double t183 = t179 * t61;
double t188 = t129 * t56 - t153 * d2xdu2;
double t192 = t188 * t3;
double t195 = t188 * t61;
double t205 = t169 * t56 - 2 * t183 * d2xdu2 + 2 * t192 * t5
- t195 * d3xdu3;
double t211 = t179 * t56 - t195 * d2xdu2;
double t215 = t211 * t61;
double d6ydx6 = (((t155 * t56 - 3 * t169 * t61 * d2xdu2
+ 6 * t179 * t3 * t5 - 3 * t183 * d3xdu3 - 6 * t188 * t9 *t11
+ 6 * t192 * t15 - t195 * d4xdu4) * t56
- 2 * t205 * t61 * d2xdu2 + 2 * t211*t3*t5-t215*d3xdu3)*t56
- (t205 * t56 - t215 * d2xdu2) * t61 * d2xdu2) * t56;
val = d6ydx6;
}
break;
default:
val = SimTK::NaN;
}
return val;
}
SimTK::Real calcValue(const SimTK::Vector& x) const {
assert(x.size() == argumentSize);
return value;
}
/**
@param timeV A nx1 time vector to integrate along, which must monotonic
and increasing
@param xM A nxm matrix of row vectors, each corresponding to the row vector
that should be applied to yF at time t
@param yF A function
@param ic The initial condition for the integral
@param intAcc The accuracy of the integral
@returns an nx2 matrix, time in column 0, integral of y in column 1
*/
SimTK::Matrix calcFunctionTimeIntegral(
const SimTK::Vector& timeV,
const SimTK::Matrix& xM,
const MuscleFirstOrderActivationDynamicModel& yF,
double ic,
int dim,
double startTime,
double endTime,
double intAcc)
{
SimTK::Matrix intXY(timeV.size(),2);
//Populate FunctionData
FunctionData fdata(yF);
fdata.m_ic = ic;
fdata.m_intDim = dim;
fdata.m_tmpXV = SimTK::Vector(xM.ncol());
fdata.m_tmpXV = 0;
SimTK::Array_< SimTK::Spline_<double> > splinedInput(xM.ncol());
//Now spline xM over time
for(int i=0; i<xM.ncol(); i++){
splinedInput[i] = SimTK::SplineFitter<Real>::
fitForSmoothingParameter(1,timeV,xM(i),0).getSpline();
}
fdata.m_splinedInput = splinedInput;
//FunctionData is now completely built
//Set up system
//double startTime = timeV(0);
//double endTime = timeV(timeV.nelt()-1);
MySystem sys(fdata);
State initState = sys.realizeTopology();
initState.setTime(startTime);
RungeKuttaMersonIntegrator integ(sys);
integ.setAccuracy(intAcc);
integ.setFinalTime(endTime);
integ.setReturnEveryInternalStep(false);
integ.initialize(initState);
int idx = 0;
double nextTimeInterval = 0;
Integrator::SuccessfulStepStatus status;
while (idx < timeV.nelt()) {
nextTimeInterval = timeV(idx);
status=integ.stepTo(nextTimeInterval);
// Use this for variable step output.
//status = integ.stepTo(Infinity);
if (status == Integrator::EndOfSimulation)
break;
const State& state = integ.getState();
intXY(idx,0) = nextTimeInterval;
intXY(idx,1) = (double)state.getZ()[0];
idx++;
}
//cout << "Integrated Solution"<<endl;
//cout << intXY << endl;
//intXY.resizeKeep(idx,2);
return intXY;
}
/**
* This function tests the accuracy of the natural cubic spline sp. The accuracy of the
* spline is tested in the following manner:
*
* a. Spline must pass through the knots given
* -Error between spline and input data at the knots (should be zero)
* b. The first derivatives are continuous at the knot points
* -Error between the value of the first derivative at the knot point, and
* what a linear extrapolation would predict just to the left and right of
* the knot point. (should be zero, within a tolerance affected by the step size in xD)
* c. The second derivatives are continuous at the knots points
* -Error between the value of the numerically calculated derivative at the knot point, and
* what a linear extrapolation would predict just to the left and right of
* the knot point. (should be zero, within a tolerance affected by the step size in xD)
* d. The second derivative is zero at the end points.
* -Numerically calculated extrapolation of the 2nd derivative should be zero
* at the end points within some tolerance
*
*
*
*
*/
SimTK::Vector testNCSpline(SimTK::Function* sp, SimTK::Vector xK, SimTK::Vector yK, SimTK::Vector xM,SimTK::Vector xD,string name, bool print){
int size = xK.size();
int sizeD= xD.size();
int sizeDK = xD.size()/(xK.size()-1);
double deltaD = (xK(xK.size()-1)-xK(0))/xD.size();
SimTK::Matrix ysp_K(size,2),ysp_M(size-1,2),ysp_D(sizeD,4);
SimTK::Vector errVec(4);
errVec = 1;
ysp_K = 0;
ysp_M = 0;
ysp_D = 0;
vector<int> derOrder(1);
derOrder[0] = 0;
///////////////////////////////////////////
//1. Evaluate the spline at the knots, the mid points and then a dense sample
///////////////////////////////////////////
SimTK::Vector tmpV1(1);
double xVal=0;
for(int i=0;i<size;i++){
xVal = xK(i);
tmpV1(0)=xK(i);
ysp_K(i,0) = sp->calcValue(tmpV1);
ysp_K(i,1) = sp->calcDerivative(derOrder,tmpV1);
}
for(int i=0;i<size-1;i++){
xVal = xM(i);
tmpV1(0) = xM(i);
ysp_M(i,0) = sp->calcValue(tmpV1);
ysp_M(i,1) = sp->calcDerivative(derOrder,tmpV1);
}
for(int i=0;i<sizeD;i++){
xVal = xD(i);
tmpV1(0) = xD(i);
ysp_D(i,0) = sp->calcValue(tmpV1);
ysp_D(i,1) = sp->calcDerivative(derOrder,tmpV1);
}
//////////////////////////////////////
//2. Compute the second derivative of the spline (using central differences), and linearly
// interpolate to get the end points. The end points should go to exactly zero because the
// second derivative is linear in a cubic spline, as is the linear extrapolation
//
// Also compute the 3rd derivative using the same method. The 3rd derivative is required in the
// test to determine if the second derivative is continuous at the knots or not.
//////////////////////////////////////
ysp_D(2) = getCentralDifference(xD, ysp_D(1), true);
ysp_D(3) = getCentralDifference(xD, ysp_D(2), true);
//////////////////////////////////////
//3. Now check to see if the splines meet the conditions of a natural cubic spline:
//////////////////////////////////////
SimTK::Vector tmpK(size,size),tmpM(size-1,size-1);
//* a. Spline passes through all knot points given
tmpK = yK-ysp_K(0);
errVec(0) = tmpK.norm();
// b. The first derivative is continuous at the knot points. Apply a continuity test
// to the data points that define the second derivative
//
// Continuity test: a linear extrapolation of first derivative of the curve in interest
// on either side of the point in interest should equal the point in interest;
double ykL,ykR,y0L,dydxL,y0R,dydxR = 0;
for(int i=1; i<size-1; i++){
y0L = ysp_D(i*sizeDK-1,1);
y0R = ysp_D(i*sizeDK+1,1);
dydxL = ysp_D(i*sizeDK-1,2); //Found using central differences
dydxR = ysp_D(i*sizeDK+1,2); //Found using central differences
ykL = y0L + dydxL*deltaD;
ykR = y0R - dydxR*deltaD;
errVec(1) = (ysp_D(i*sizeDK,1)-ykL)+(ysp_D(i*sizeDK,1)-ykR);
}
// c. The second derivative is continuous at the knot points. Apply a continuity test
// to the data points that define the second derivative. This also tests if the
// first derivative is smooth.
//
// Continuity test: a linear extrapolation of first derivative of the curve in interest
// on either side of the point in interest should equal the point in interest;
for(int i=1; i<size-1; i++){
y0L = ysp_D(i*sizeDK-1,2);
y0R = ysp_D(i*sizeDK+1,2);
dydxL = ysp_D(i*sizeDK-1,3); //Found using central differences
dydxR = ysp_D(i*sizeDK+1,3); //Found using central differences
ykL = y0L + dydxL*deltaD;
ykR = y0R - dydxR*deltaD;
errVec(2) = (ysp_D(i*sizeDK,2)-ykL)+(ysp_D(i*sizeDK,2)-ykR);
}
//////////////////////////////////////
//* d. The second derivative is zero at the end points
//////////////////////////////////////
errVec(3) = abs(ysp_D(0,2)) + abs(ysp_D(sizeD-1,2));
//////////////////////////////////////
//print the data for analysis
//////////////////////////////////////
if(print==true){
string fname = name;
fname.append("_K.csv");
printMatrixToFile(xK, ysp_K, fname);
fname = name;
fname.append("_M.csv");
printMatrixToFile(xM, ysp_M, fname);
fname = name;
fname.append("_D.csv");
printMatrixToFile(xD, ysp_D, fname);
}
return errVec;
}
//=============================================================================
// HELPER
//=============================================================================
void AnalyzeTool::run(SimTK::State& s, Model &aModel, int iInitial, int iFinal, const Storage &aStatesStore, bool aSolveForEquilibrium)
{
AnalysisSet& analysisSet = aModel.updAnalysisSet();
for(int i=0;i<analysisSet.getSize();i++) {
analysisSet.get(i).setStatesStore(aStatesStore);
}
// TODO: some sort of filtering or something to make derivatives smoother?
GCVSplineSet statesSplineSet(5,&aStatesStore);
// PERFORM THE ANALYSES
double tPrev=0.0,t=0.0,dt=0.0;
int ny = s.getNY();
Array<double> dydt(0.0,ny);
Array<double> yFromStorage(0.0,ny);
const Array<string>& labels = aStatesStore.getColumnLabels();
int numOpenSimStates = labels.getSize()-1;
SimTK::Vector stateData;
stateData.resize(numOpenSimStates);
for(int i=iInitial;i<=iFinal;i++) {
tPrev = t;
aStatesStore.getTime(i,s.updTime()); // time
t = s.getTime();
aModel.setAllControllersEnabled(true);
aStatesStore.getData(i,numOpenSimStates,&stateData[0]); // states
// Get data into local Vector and assign to State using common utility
// to handle internal (non-OpenSim) states that may exist
Array<std::string> stateNames = aStatesStore.getColumnLabels();
for (int j=0; j<stateData.size(); ++j){
// storage labels included time at index 0 so +1 to skip
aModel.setStateVariableValue(s, stateNames[j+1], stateData[j]);
}
// Adjust configuration to match constraints and other goals
aModel.assemble(s);
// equilibrateMuscles before realization as it may affect forces
if(aSolveForEquilibrium){
try{// might not be able to equilibrate if model is in
// a non-physical pose. For example, a pose where the
// muscle length is shorter than the tendon slack-length.
// the muscle will throw an Exception in this case.
aModel.equilibrateMuscles(s);
}
catch (const std::exception& e) {
cout << "WARNING- AnalyzeTool::run() unable to equilibrate muscles ";
cout << "at time = " << t <<"." << endl;
cout << "Reason: " << e.what() << endl;
}
}
// Make sure model is at least ready to provide kinematics
aModel.getMultibodySystem().realize(s, SimTK::Stage::Velocity);
if(i==iInitial) {
analysisSet.begin(s);
} else if(i==iFinal) {
analysisSet.end(s);
// Step
} else {
analysisSet.step(s,i);
}
}
}
//=============================================================================
// UTILITY
//=============================================================================
//_____________________________________________________________________________
bool MuscleOptimizer::processModel(Model* inputModel, Model* referenceModel, const std::string& aPathToSubject)
{
if (!get_apply()) return false;
try
{
OpenSim::Array<std::string> musclesInput, musclesReference;
referenceModel->getMuscles().getNames(musclesReference);
inputModel->getMuscles().getNames(musclesInput);
int nEnabledMuscles = 0;
// Check that all enabled input model's muscles can be found in the reference model
for (int im = 0; im < musclesInput.getSize(); ++im)
{
if (isEnabledMuscle(musclesInput[im]))
{
++nEnabledMuscles; // also, count the total number of enabled muscles, so that we can provide this info to the user to check progress
if (musclesReference.findIndex(musclesInput[im]) < 0)
{
cout << "Muscle optimizer: ERROR- Muscle " << musclesInput[im] << " could not be found in reference model! Aborting." << std::endl;
return false;
}
}
}
int curMuscleOrdinal = 0;
for (int im = 0; im < musclesInput.getSize(); ++im) {
std::string currentMuscleName = musclesInput[im];
if (isEnabledMuscle(currentMuscleName))
{
std::cout << "Optimizing muscle " << ++curMuscleOrdinal << "/" << nEnabledMuscles << ": " << currentMuscleName << ";";
// Reset models' poses
SimTK::State& referenceInitialState = referenceModel->initSystem();
SimTK::State& inputInitialState = inputModel->initSystem();
MuscleOptimizer::CoordinateCombinations coordCombinations = sampleROMsForMuscle(*inputModel, inputInitialState, currentMuscleName, get_n_evaluation_points());
if (coordCombinations.size()>0)
{
std::cout << "using coordinates [ ";
for (auto& cit : coordCombinations)
std::cout << cit.first << " ";
std::cout << "], total no. of combinations " << coordCombinations.at(0).second.size() << endl;
}
else
{
std::cout << " No coordinates for " << currentMuscleName << ", skipping optimization" << std::endl;
continue;
}
if (coordCombinations.size() > 0 && coordCombinations.at(0).second.size() < get_n_evaluation_points() / 2) //just a check that the sampling did not fail
std::cout << "WARNING! no. of coordinate combinations is less than half the number of eval points" << endl;
std::vector<TemplateMuscleInfo> templateQuantities = sampleTemplateQuantities(*referenceModel, referenceInitialState, currentMuscleName, coordCombinations);
SimTK::Vector targetMTUlength = sampleMTULength(*inputModel, inputInitialState, currentMuscleName, coordCombinations);
if (targetMTUlength.size() != templateQuantities.size()) {
std::cout << " Could not sample target MTU lengths on the same poses as reference model" << std::endl;
std::cout << " There might be some inconsistencies between joints/coordinates definitions in the two models" << std::endl;
continue;
}
SimTK::Matrix A(templateQuantities.size(), 2);
for (size_t i = 0; i < templateQuantities.size(); ++i)
{
A(i, 0) = templateQuantities.at(i).normalizedFiberLength*cos(templateQuantities.at(i).pennationAngle);
A(i, 1) = templateQuantities.at(i).normalizedTendonLength;
}
SimTK::Vector x;
SimTK::FactorQTZ qtz(A);
qtz.solve(targetMTUlength, x);
if (x[0] <= 0 || x[1] <= 0)
{
if (x[0] <= 0)
std::cout << " Negative estimate for optimal fiber length of muscle " << currentMuscleName << std::endl;
if (x[1] <= 0)
std::cout << " Negative estimate for tendon slack length of muscle " << currentMuscleName << std::endl;
if (max(A.col(1)) - min(A.col(1)) < 0.0001)
std::cout << " Tendon length not changing throughout range of motion" << std::endl;
SimTK::Vector templateMTUlength = sampleMTULength(*referenceModel, referenceInitialState, currentMuscleName, coordCombinations);
SimTK::Vector Lfib_targ(targetMTUlength.size());
for (int i = 0; i < Lfib_targ.size(); ++i)
{
double Lten_fraction = A(i, 1)*referenceModel->getMuscles().get(currentMuscleName).getTendonSlackLength() / templateMTUlength(i);
Lfib_targ(i) = (1 - Lten_fraction)*targetMTUlength(i);
}
std::cout << " Fallback: optimize optimal fiber length assuming same proportion between fiber and tendon as in reference muscle" << std::endl;
// first round - estimate Lopt, assuming that the same proportion between fiber and tendon in kept as in reference muscle
SimTK::Matrix A_1(A.col(0));
SimTK::FactorQTZ qtz1(A_1);
SimTK::Vector x1(1);
qtz1.solve(Lfib_targ, x1);
//second round - estimate Lts, calculating tendon length from previous results on fiber length
SimTK::Matrix A_2(A.col(1));
SimTK::Vector b_2 = targetMTUlength - (A_1*x1(0)).col(0);
SimTK::FactorQTZ qtz2(A_2);
SimTK::Vector x2(1);
qtz2.solve(b_2, x2);
x(0) = x1(0);
x(1) = x2(0);
}
inputModel->getMuscles()[im].setOptimalFiberLength(x(0));
inputModel->getMuscles()[im].setTendonSlackLength(x(1));
}
}
if (_printResultFiles) {
if (!getProperty_output_model_file().getValueIsDefault() && get_output_model_file() != "")
{
std::string outputModelFileName = isAbsolute(get_output_model_file().c_str()) ? get_output_model_file() : aPathToSubject + get_output_model_file();
if (inputModel->print(outputModelFileName))
cout << "Wrote model file " << outputModelFileName << " from model " << inputModel->getName() << endl;
}
}
inputModel->initializeState(); //this is to bring coordinates back to default, for the GUI
}
catch (const Exception& x)
{
x.print(cout);
return false;
}
return true;
}