/** * 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; }
//============================================================================== // Common Functions //============================================================================== int initTestStates(SimTK::Vector &qi, SimTK::Vector &ui) { using namespace SimTK; Random::Uniform randomAngle(-Pi/4, Pi/4); Random::Uniform randomSpeed(-1.0, 1.0); // Provide initial states as random angles and speeds for OpenSim and // Simbody models for(int i = 0; i<qi.size(); i++) qi[i] = randomAngle.getValue(); for(int i = 0; i<ui.size(); i++) ui[i] = randomSpeed.getValue(); return qi.size(); }
/* Calculate the equivalent spatial force, FB_G, acting on the body connected by this joint at its location B, expressed in ground. */ SimTK::SpatialVec Joint::calcEquivalentSpatialForce(const SimTK::State &s, const SimTK::Vector &mobilityForces) const { // The number of mobilities for the entire system. int nm = _model->getMatterSubsystem().getNumMobilities(); if(nm != mobilityForces.size()){ throw Exception("Joint::calcEquivalentSpatialForce(): input mobilityForces does not match model's mobilities"); } const SimTK::MobilizedBodyIndex &mbx = getChildBody().getIndex(); // build a unique list of underlying MobilizedBodies that are involved // with this Joint in addition to and not including that of the child body std::set<SimTK::MobilizedBodyIndex> mbds; const CoordinateSet& coordinateSet = get_CoordinateSet(); for(int i=0; i<coordinateSet.getSize(); ++i){ const MobilizedBodyIndex& coordsMbx = coordinateSet[i].getBodyIndex(); if (coordsMbx != mbx){ mbds.insert(coordsMbx); } } SimTK::SpatialVec FB_G = calcEquivalentSpatialForceForMobilizedBody(s, mbx, mobilityForces); SimTK::SpatialVec FBx_G; std::set<SimTK::MobilizedBodyIndex>::const_iterator it = mbds.begin(); const SimTK::MobilizedBody &G = getModel().getMatterSubsystem().getGround(); const SimTK::MobilizedBody &B = getModel().getMatterSubsystem().getMobilizedBody(mbx); SimTK::Vec3 r_BG = B.expressVectorInAnotherBodyFrame(s, B.getOutboardFrame(s).p(), G); while(it != mbds.end()){ FBx_G = calcEquivalentSpatialForceForMobilizedBody(s, *it, mobilityForces); const SimTK::MobilizedBody &b = getModel().getMatterSubsystem().getMobilizedBody(*it); SimTK::Vec3 r_bG = b.expressVectorInAnotherBodyFrame(s, b.getOutboardFrame(s).p(), G); // Torques add and include term due to offset in forces FB_G += FBx_G; // shiftForceFromTo(FBx_G, r_bG, r_BG); ++it; } return FB_G; }
/* Multiplications Additions Assignments 21 20 13 */ double SegmentedQuinticBezierToolkit:: calcQuinticBezierCurveVal(double u, const SimTK::Vector& pts) { double val = -1; SimTK_ERRCHK1_ALWAYS( (u>=0 && u <= 1) , "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal", "Error: double argument u must be between 0.0 and 1.0" "but %f was entered.",u); SimTK_ERRCHK_ALWAYS( (pts.size() == 6) , "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal", "Error: vector argument pts must have a length of 6."); //Compute the Bezier point double p0 = pts(0); double p1 = pts(1); double p2 = pts(2); double p3 = pts(3); double p4 = pts(4); double p5 = pts(5); double u5 = 1; double u4 = u; double u3 = u4*u; double u2 = u3*u; double u1 = u2*u; double u0 = u1*u; //See lines 1-6 of MuscleCurveCodeOpt_20120210 double t2 = u1 * 0.5e1; double t3 = u2 * 0.10e2; double t4 = u3 * 0.10e2; double t5 = u4 * 0.5e1; double t9 = u0 * 0.5e1; double t10 = u1 * 0.20e2; double t11 = u2 * 0.30e2; double t15 = u0 * 0.10e2; val = p0 * (u0 * (-0.1e1) + t2 - t3 + t4 - t5 + u5 * 0.1e1) + p1 * (t9 - t10 + t11 + u3 * (-0.20e2) + t5) + p2 * (-t15 + u1 * 0.30e2 - t11 + t4) + p3 * (t15 - t10 + t3) + p4 * (-t9 + t2) + p5 * u0 * 0.1e1; return val; }
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()); }
//============================================================================= // 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;// }
/* 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; }
/** * 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; }
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; }