예제 #1
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;
}
/** 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);
}
/**
 * Compute the Joint power.
 */
SimTK::Vector JointInternalPowerProbe::computeProbeInputs(const State& s) const
{
    int nJ = getJointNames().size();
    SimTK::Vector TotalP;

    if (getSumPowersTogether()) {
        TotalP.resize(1);
        TotalP(0) = 0;       // Initialize to zero
    }
    else
        TotalP.resize(nJ);

    // Loop through each joint in the list of joint_names
    for (int i=0; i<nJ; ++i)
    {
        string jointName = getJointNames()[i];
        int k = _model->getJointSet().getIndex(jointName);

        // Get the "Joint" power from the Joint object
        double jointPower = _model->getJointSet().get(k).calcPower(s);
        
        // Append to output vector
        if (getSumPowersTogether())
            TotalP(0) += std::pow(jointPower, getExponent());
        else
            TotalP(i) = std::pow(jointPower, getExponent());
    }

    return TotalP;
}
/**
  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;
}
/*Detailed Computational Costs
                Comparisons     Div     Mult     Additions   Assignments
    splineGuess log(n,2)                2        3           1
    (n currently set to 100)

    Newton Iter
        f                               21       20          13 
        df                              20       19          11
        update  4               1                3           6    
        total   4               1       41       42          30
    \endverbatim

    To evaluate u to SimTK::Eps*100 this typically involves 2 Newton 
    iterations, yielding a total cost of

    \verbatim
                Comparisons     Div     Mult    Additions   Assignments
    eval U      7+8=15          2       82      42          60
*/
double SegmentedQuinticBezierToolkit::calcU(double ax, const SimTK::Vector& bezierPtsX, 
                                 const SimTK::Spline& splineUX, double tol, 
                                 int maxIter)
{
    //Check to make sure that ax is in the curve domain
    double minX = 1e100;
    double maxX = -1e100;
    for(int i=0; i<bezierPtsX.nrow(); i++){
        if(bezierPtsX(i) > maxX)
            maxX = bezierPtsX(i);
        if(bezierPtsX(i) < minX)
            minX = bezierPtsX(i);
    }

    SimTK_ERRCHK_ALWAYS( ax >= minX && ax <= maxX, 
        "SegmentedQuinticBezierToolkit::calcU", 
        "Error: input ax was not in the domain of the Bezier curve specified \n"
        "by the control points in bezierPtsX.");

    double u = splineUX.calcValue(ax);
    u = clampU(u);
    double f = 0;


    f = calcQuinticBezierCurveVal(u,bezierPtsX)-ax;
    double df= 0;
    double du= 0;
    int iter = 0;
    bool pathologic = false;
    
    //Newton iterate to the desired tolerance
    while(abs(f) > tol && iter < maxIter && pathologic == false){
       //Take a Newton step
        df = calcQuinticBezierCurveDerivU(u,bezierPtsX,1);
        if(abs(df) > 0){
            du = -f/df;
            u  = u + du;
            u  = clampU(u); 
            f = calcQuinticBezierCurveVal(u,bezierPtsX)-ax;
        }else{
            pathologic = true;
        }
        iter++;
    }

    //Check for convergence
    SimTK_ERRCHK2_ALWAYS( (f <= tol), 
        "SegmentedQuinticBezierToolkit::calcU", 
        "Error: desired tolerance of %f on U not met by the Newton iteration."
        " A tolerance of %f was reached.",tol, f);

    SimTK_ERRCHK_ALWAYS( (pathologic==false), 
        "SegmentedQuinticBezierToolkit::calcU", 
        "Error: Newton iteration went pathologic: df = 0 to machine precision.");
      
    
    //Return the value
    return u;
}
예제 #6
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();
}
예제 #7
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;
}
예제 #9
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());
}
예제 #10
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);
        }
    }
}
/**
  @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 numerically for continuity of a curve. The test is 
  performed by taking a point on the curve, and then two points (called the 
  shoulder points) to the left and right of the point in question. The value
  of the functions derivative is evaluated at each of the shoulder points and
  used to linearly extrapolate from the shoulder points back to the original 
  point. If the original point and the linear extrapolations of each of the 
  shoulder points agree within tol, then the curve is assumed to be 
  continuous.


  @param xV       Values to test for continuity, note that the first and last
  points cannot be tested

  @param yV       Function values at the test points

  @param dydxV    Function derivative values evaluated at xV

  @param d2ydx2V  Function 2nd derivative values (or estimates) evaluated at
  xV

  @param minTol   The minimum error allowed - this prevents the second order
  error term from going to zero

  @param taylorErrorMult  This scales the error tolerance. The default error
  tolerance is the 2nd-order Taylor series
  term.
  */
bool isFunctionContinuous(const SimTK::Vector& xV, const SimTK::Vector& yV,
        const SimTK::Vector& dydxV, const SimTK::Vector& d2ydx2V, 
        double minTol, double taylorErrorMult)
{
    bool flag_continuous = true;
    //double contErr = 0;

    double xL = 0;      // left shoulder point
    double xR = 0;      // right shoulder point
    double yL = 0;      // left shoulder point function value
    double yR = 0;      // right shoulder point function value
    double dydxL = 0;   // left shoulder point derivative value
    double dydxR = 0;   // right shoulder point derivative value

    double xVal = 0;    //x value to test
    double yVal = 0;    //Y(x) value to test

    double yValEL = 0;  //Extrapolation to yVal from the left
    double yValER = 0;  //Extrapolation to yVal from the right

    double errL = 0;
    double errR = 0;

    double errLMX = 0;
    double errRMX = 0;


    for(int i =1; i < xV.nelt()-1; i++){
        xVal = xV(i);
        yVal = yV(i);

        xL = xV(i-1);
        xR = xV(i+1);

        yL = yV(i-1);
        yR = yV(i+1);

        dydxL = dydxV(i-1);
        dydxR = dydxV(i+1);


        yValEL = yL + dydxL*(xVal-xL);
        yValER = yR - dydxR*(xR-xVal);

        errL = abs(yValEL-yVal);
        errR = abs(yValER-yVal);

        errLMX = abs(d2ydx2V(i-1)*0.5*(xVal-xL)*(xVal-xL));
        errRMX = abs(d2ydx2V(i+1)*0.5*(xR-xVal)*(xR-xVal));

        errLMX*=taylorErrorMult;
        errRMX*=taylorErrorMult;

        if(errLMX < minTol)
            errLMX = minTol;

        if(errRMX < minTol)
            errRMX = minTol; // to accommodate numerical
        //error in errL

        if(errL > errLMX || errR > errRMX){
            /*if(errL > errR){
              if(errL > contErr)
              contErr = errL;

              }else{
              if(errR > contErr)
              contErr = errR;
              }*/

            flag_continuous = false;
        }
    }

    return flag_continuous;//contErr;//
}
예제 #13
0
/* Calculate the equivalent spatial force, FB_G, acting on a mobilized body specified 
   by index acting at its mobilizer frame B, expressed in ground.  */
SimTK::SpatialVec Joint::calcEquivalentSpatialForceForMobilizedBody(const SimTK::State &s, 
	const SimTK::MobilizedBodyIndex mbx, const SimTK::Vector &mobilityForces) const
{
	// Get the mobilized body
	const SimTK::MobilizedBody mbd    = getModel().getMatterSubsystem().getMobilizedBody(mbx);
	const SimTK::UIndex        ustart = mbd.getFirstUIndex(s);
	const int                  nu     = mbd.getNumU(s);

	const SimTK::MobilizedBody ground = getModel().getMatterSubsystem().getGround();

    if (nu == 0) // No mobility forces (weld joint?).
        return SimTK::SpatialVec(SimTK::Vec3(0), SimTK::Vec3(0));

	// Construct the H (joint jacobian, joint transition) matrrix for this mobilizer
	SimTK::Matrix transposeH_PB_w(nu, 3);
	SimTK::Matrix transposeH_PB_v(nu, 3);
	// from individual columns
	SimTK::SpatialVec Hcol;
	
	// To obtain the joint Jacobian, H_PB (H_FM in Simbody) need to be realized to at least position
	_model->getMultibodySystem().realize(s, SimTK::Stage::Position);

	SimTK::Vector f(nu, 0.0);
	for(int i =0; i<nu; ++i){
		f[i] = mobilityForces[ustart + i];
		// Get the H matrix for this Joint by constructing it from the operator H*u
		Hcol = mbd.getH_FMCol(s, SimTK::MobilizerUIndex(i));
		const SimTK::Vector hcolw(Hcol[0]);
		const SimTK::Vector hcolv(Hcol[1]);

		transposeH_PB_w[i] = ~hcolw;
		transposeH_PB_v[i] = ~hcolv;
	}

	// Spatial force and torque vectors
	SimTK::Vector Fv(3, 0.0), Fw(3, 0.0);

	// Solve the pseudoinverse problem of Fv = pinv(~H_PB_G_v)*f;
	SimTK::FactorQTZ pinvForce(transposeH_PB_v);

	//if rank = 0, body force cannot contribute to the mobility force
	if(pinvForce.getRank() > 0)
		pinvForce.solve(f, Fv);
	
	// Now solve the pseudoinverse for torque for any unaccounted f: Fw = pinv(~H_PB_G_w)*(f - ~H_PB_G_v*Fv);
	SimTK::FactorQTZ pinvTorq(transposeH_PB_w);

	//if rank = 0, body torque cannot contribute to the mobility force
	if(pinvTorq.getRank() > 0)
		pinvTorq.solve(f, Fw);
	
	// Now we have two solution with either the body force Fv or body torque accounting for some or all of f
	SimTK::Vector fv =  transposeH_PB_v*Fv;
	SimTK::Vector fw =  transposeH_PB_w*Fw; 

	// which to choose? Choose the more effective as fx.norm/Fx.norm
	if(fv.norm() > SimTK::SignificantReal){ // if body force can contributes at all
		// if body torque can contribute too and it is more effective
		if(fw.norm() > SimTK::SignificantReal){
			if (fw.norm()/Fw.norm() > fv.norm()/Fv.norm() ){ 
				// account for f using torque, Fw, so compute Fv with remainder
				pinvForce.solve(f-fw, Fv);		
			}else{
				// account for f using force, Fv, first and Fw from remainder
				pinvTorq.solve(f-fv, Fw);
			}
		}
		// else no torque contribution and Fw should be zero
	}
	// no force contribution but have a torque
	else if(fw.norm() > SimTK::SignificantReal){
		// just Fw
	}
	else{
		// should be the case where gen force is zero.
		assert(f.norm() < SimTK::SignificantReal);
	}

	// The spatial forces above are expresseded in the joint frame of the parent
	// Transform from parent joint frame, P into the parent body frame, Po
	const SimTK::Rotation R_PPo = (mbd.getInboardFrame(s).R());

	// Re-express forces in ground, first by describing force in the parent, Po, 
	// frame instead of joint frame
	SimTK::Vec3 vecFw = R_PPo*SimTK::Vec3::getAs(&Fw[0]);
	SimTK::Vec3 vecFv = R_PPo*SimTK::Vec3::getAs(&Fv[0]);

	//Force Acting on joint frame, B,  in child body expressed in Parent body, Po
	SimTK::SpatialVec FB_Po(vecFw, vecFv);

	const MobilizedBody parent = mbd.getParentMobilizedBody();
	// to apply spatial forces on bodies they must be expressed in ground
	vecFw = parent.expressVectorInAnotherBodyFrame(s, FB_Po[0], ground);
	vecFv = parent.expressVectorInAnotherBodyFrame(s, FB_Po[1], ground);

	// Package resulting torque and force as a spatial vec
	SimTK::SpatialVec FB_G(vecFw, vecFv);

	return FB_G;
}
예제 #14
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;

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


}
예제 #17
0
 SimTK::Real calcValue(const SimTK::Vector& x) const {
     assert(x.size() == argumentSize);
     return value;
 }
/*
            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;
}
//=============================================================================
// 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;
}