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