/**
  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);
}
Пример #3
0
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());
}
Пример #4
0
/**
 * 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;
}
Пример #5
0
//==============================================================================
// 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();
}
Пример #6
0
/* 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;
}
Пример #11
0
 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;
}
Пример #13
0
/**
* 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;

}
Пример #14
0
//=============================================================================
// 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;
}