/*
Cost: n comparisons, for a quintic Bezier curve with n-spline sections
Comp Div Mult Add Assignments
Cost 3*n+2 1*n 3
*/
int SegmentedQuinticBezierToolkit::calcIndex(double x,
const SimTK::Matrix& bezierPtsX)
{
int idx = 0;
bool flag_found = false;
for(int i=0; i<bezierPtsX.ncol(); i++){
if( x >= bezierPtsX(0,i) && x < bezierPtsX(5,i) ){
idx = i;
i = bezierPtsX.ncol();
flag_found = true;
}
}
//Check if the value x is identically the last point
if(flag_found == false && x == bezierPtsX(5,bezierPtsX.ncol()-1)){
idx = bezierPtsX.ncol()-1;
flag_found = true;
}
SimTK_ERRCHK_ALWAYS( (flag_found == true),
"SegmentedQuinticBezierToolkit::calcIndex",
"Error: A value of x was used that is not within the Bezier curve set.");
return idx;
}
/**
This function will print cvs file of the matrix
data
@params data: A matrix of data
@params filename: The name of the file to print
*/
void printMatrixToFile( const SimTK::Matrix& data, const std::string& filename)
{
ofstream datafile;
datafile.open(filename.c_str());
for(int i = 0; i < data.nrow(); i++){
for(int j = 0; j < data.ncol(); j++){
if(j<data.ncol()-1)
datafile << data(i,j) << ",";
else
datafile << data(i,j) << "\n";
}
}
datafile.close();
}
/**
* This function will print cvs file of the column vector col0 and the matrix data
*
* @params col0: A vector that must have the same number of rows as the data matrix
* This column vector is printed as the first column
* @params data: A matrix of data
* @params filename: The name of the file to print
*/
void printMatrixToFile(SimTK::Vector col0,SimTK::Matrix data, string filename){
ofstream datafile;
datafile.open(filename.c_str());
for(int i = 0; i < data.nrow(); i++){
datafile << col0(i) << ",";
for(int j = 0; j < data.ncol(); j++){
if(j<data.ncol()-1)
datafile << data(i,j) << ",";
else
datafile << data(i,j) << "\n";
}
}
datafile.close();
}
/**
* This function will print cvs file of the column vector col0 and the matrix data
*
* @params col0: A vector that must have the same number of rows as the data matrix
* This column vector is printed as the first column
* @params data: A matrix of data
* @params filename: The name of the file to print
*/
void SegmentedQuinticBezierToolkit::
printMatrixToFile(const SimTK::Vector& col0,
const SimTK::Matrix& data, std::string& filename)
{
ofstream datafile;
datafile.open(filename.c_str());
for(int i = 0; i < data.nrow(); i++){
datafile << col0(i) << ",";
for(int j = 0; j < data.ncol(); j++){
if(j<data.ncol()-1)
datafile << data(i,j) << ",";
else
datafile << data(i,j) << "\n";
}
}
datafile.close();
}
void SegmentedQuinticBezierToolkit::
printBezierSplineFitCurves(const SimTK::Function_<double>& curveFit,
SimTK::Matrix& ctrlPts, SimTK::Vector& xVal, SimTK::Vector& yVal,
std::string& filename)
{
std::string caller = "printBezierSplineFitCurves";
int nbezier = int(ctrlPts.ncol()/2.0);
int rows = NUM_SAMPLE_PTS*nbezier - (nbezier-1);
SimTK::Vector y1Val(rows);
SimTK::Vector y2Val(rows);
SimTK::Vector ySVal(rows);
SimTK::Vector y1SVal(rows);
SimTK::Vector y2SVal(rows);
SimTK::Matrix printMatrix(rows,6);
SimTK::Vector tmp(1);
std::vector<int> deriv1(1);
std::vector<int> deriv2(2);
deriv1[0] = 0;
deriv2[0] = 0;
deriv2[1] = 0;
double u = 0;
int oidx = 0;
int offset = 0;
for(int j=0; j < nbezier ; j++)
{
if(j > 0){
offset = 1;
}
for(int i=0; i<NUM_SAMPLE_PTS-offset; i++)
{
oidx = i + j*NUM_SAMPLE_PTS - offset*(j-1);
u = ( (double)(i+offset) )/( (double)(NUM_SAMPLE_PTS-1) );
y1Val(oidx) = calcQuinticBezierCurveDerivDYDX(u,
ctrlPts(2*j),ctrlPts(2*j+1),1);
y2Val(oidx) = calcQuinticBezierCurveDerivDYDX(u,
ctrlPts(2*j),ctrlPts(2*j+1),2);
tmp(0) = xVal(oidx);
ySVal(oidx) = curveFit.calcValue( tmp );
y1SVal(oidx) = curveFit.calcDerivative(deriv1,tmp);
y2SVal(oidx) = curveFit.calcDerivative(deriv2,tmp);
printMatrix(oidx,0) = yVal(oidx);
printMatrix(oidx,1) = y1Val(oidx);
printMatrix(oidx,2) = y2Val(oidx);
printMatrix(oidx,3) = ySVal(oidx);
printMatrix(oidx,4) = y1SVal(oidx);
printMatrix(oidx,5) = y2SVal(oidx);
}
}
printMatrixToFile(xVal,printMatrix,filename);
}
/**
@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;
}
int main(int argc, char* argv[])
{
try {
SimTK_START_TEST("Testing MuscleFirstOrderDynamicModel");
// Test property bounds.
{
MuscleFirstOrderActivationDynamicModel actMdl;
actMdl.set_activation_time_constant(0.0);
SimTK_TEST_MUST_THROW_EXC(actMdl.finalizeFromProperties(),
SimTK::Exception::ErrorCheck);
}
{
MuscleFirstOrderActivationDynamicModel actMdl;
actMdl.set_deactivation_time_constant(0.0);
SimTK_TEST_MUST_THROW_EXC(actMdl.finalizeFromProperties(),
SimTK::Exception::ErrorCheck);
}
{
MuscleFirstOrderActivationDynamicModel actMdl;
actMdl.set_minimum_activation(-SimTK::SignificantReal);
SimTK_TEST_MUST_THROW_EXC(actMdl.finalizeFromProperties(),
SimTK::Exception::ValueOutOfRange);
}
{
MuscleFirstOrderActivationDynamicModel actMdl;
actMdl.set_minimum_activation(1.0);
SimTK_TEST_MUST_THROW_EXC(actMdl.finalizeFromProperties(),
SimTK::Exception::ValueOutOfRange);
}
/*
Test conditions:
1. Generate the step response and make sure that it stays between
the minimum activation and the maximum (amin, and 1)
2. Ensure that the derivative function is continuous
3. Ensure that the 10%-90% rise time is about 3 time constants
Ensure that the fall time is about 2.25 time constants.
*/
int pts = 100;
double stepStart = 0.2;
double stepEnd = 0.5;
double amin = 0.01;
double tauA = 0.01;
double tauD = 0.04;
////////////////////////////
//generate a step input
////////////////////////////
SimTK::Vector timeV(pts);
SimTK::Matrix xM(pts,2);
for(int i=0; i<pts; i++){
timeV(i) = ((double)i)/((double)pts-1.0);
xM(i,0) = amin;
if( timeV(i)<=stepStart || timeV(i)>=stepEnd ){
xM(i,1) = 0;
}else{
xM(i,1) = 1;
}
}
MuscleFirstOrderActivationDynamicModel actMdl;
MuscleFirstOrderActivationDynamicModel actMdl2;
actMdl2.set_activation_time_constant(2*tauA);
actMdl2.set_deactivation_time_constant(2*tauD);
actMdl2.set_minimum_activation(2*amin);
cout << endl;
cout<<"*****************************************************"<<endl;
cout << "TEST: Serialization"<<endl;
cout << endl;
actMdl.print("default_MuscleFirstOrderActivationDynamicModel.xml");
Object* tmpObj = Object::
makeObjectFromFile("default_MuscleFirstOrderActivationDynamicModel.xml");
actMdl2 = *dynamic_cast<MuscleFirstOrderActivationDynamicModel*>(tmpObj);
delete tmpObj;
SimTK_TEST(actMdl ==actMdl2);
remove("default_MuscleFirstOrderActivationDynamicModel.xml");
cout << endl;
cout<<"*****************************************************"<<endl;
printf("TEST: Activation bounds %f and 1 \n",amin);
cout<<" respected during step response"<<endl;
cout << endl;
SimTK::Matrix stepResponse = calcFunctionTimeIntegral( timeV,
xM,
actMdl,
amin,
0,
0,
1,
1e-12);
//printMatrixToFile( stepResponse, "stepResponse.csv");
//TEST 1: Check bounds on the step response
bool boundsRespected= true;
for(int i=0; i<stepResponse.nrow(); i++){
if(stepResponse(i,1) < amin-SimTK::Eps)
boundsRespected = false;
if(stepResponse(i,1) > 1+SimTK::Eps)
boundsRespected = false;
}
SimTK_TEST(boundsRespected);
printf("PASSED to a tol of %fe-16",SimTK::Eps*1e16);
cout << endl;
cout<<"*****************************************************"<<endl;
cout<<"TEST: 10%-90% Rise time 3*tau_act +/- 10%"<<endl;
cout<<" 90%-10% Fall time 2.17*tau_dact +/- 10%" <<endl;
cout<<" And causality" << endl;
cout << endl;
double rt10 = 0;
double rt90 = 0;
double ft90 = 0;
double ft10 = 0;
double v10 = (1-amin)*0.10 + amin;
double v90 = (1-amin)*0.90 + amin;
for(int i=0; i<stepResponse.nrow()-1; i++){
if(stepResponse(i,1) <= v10 && stepResponse(i+1,1) > v10)
rt10 = stepResponse(i+1,0);
if(stepResponse(i,1) <= v90 && stepResponse(i+1,1) > v90)
rt90 = stepResponse(i+1,0);
if(stepResponse(i,1) > v10 && stepResponse(i+1,1) <= v10)
ft10 = stepResponse(i+1,0);
if(stepResponse(i,1) > v90 && stepResponse(i+1,1) <= v90)
ft90 = stepResponse(i+1,0);
}
double normRiseTime = (rt90-rt10)/tauA;
double normFallTime = (ft10-ft90)/tauD;
//Checking for causality
SimTK_TEST( (rt10>stepStart) && (rt90>rt10)
&& (ft90>rt90) && (ft90>stepEnd) && (ft10>ft90));
SimTK_TEST_EQ_TOL(normRiseTime,3,0.3);
SimTK_TEST_EQ_TOL(normFallTime,2.17,0.2);
printf("PASSED: with a normalized 10-90 percent rise time of %f\n"
" and a normalized 90-10 percent fall time of %f\n",
normRiseTime,normFallTime);
cout<<"*****************************************************"<<endl;
cout<<"TEST: Continuity of d/dt activation w.r.t. excitation"<<endl;
cout<<" and constant activation" <<endl;
cout << endl;
//Generate a range of activation values
SimTK::Vector actV(10);
for(int i=0; i<actV.size(); i++){
actV(i) = (1-amin)*((double)i)/((double)actV.size()-1.0) + amin;
}
//Generate a detailed range of excitations
SimTK::Vector uV(100);
for(int i=0; i<uV.size(); i++){
uV(i) = ((double)i)/((double)uV.size()-1.0);
}
//For each activation value, generate the derivative curve, and
//check its continuity
SimTK::Vector dx(uV.size()), d2x(uV.size()), d3x(uV.size());
dx = 0;
d2x = 0;
d3x = 0;
xM.resize(uV.size(),2);
/*
calcCentralDifference(const SimTK::Matrix& xM,
const SimTK::Function& yF,
int dim,int order)
*/
bool continuous = false;
double tmp = 0;
double maxDxDiff = 0;
double minTol = 0.5;
double taylorMult = 2.0000001;
for(int i=0;i<actV.size();i++){
for(int j=0; j<uV.size(); j++){
xM(j,0) = actV(i);
xM(j,1) = uV(j);
dx(j) = actMdl.calcDerivative(actV(i), uV(j));
}
d2x = calcCentralDifference(xM(1), dx,true);
for(int j=1;j<uV.size();j++){
tmp = abs(d2x(j)-d2x(j-1));
if( tmp > maxDxDiff)
maxDxDiff = tmp;
}
d3x = calcCentralDifference(xM(1), d2x, true);
continuous = isFunctionContinuous(xM(1), dx, d2x, d3x,
minTol,taylorMult);
SimTK_TEST(continuous);
}
printf("PASSED: with an allowable error of minTol of %f\n"
" or %f x the next Taylor series term. These values\n"
" are reasonable given the elbow in the function and\n"
" the maximum difference in slope of %f\n"
,minTol,taylorMult,maxDxDiff);
cout << endl;
cout<<"*****************************************************"<<endl;
cout<<"TEST: Continuity of d/dt activation w.r.t. activation"<<endl;
cout<<" and constant excitation" <<endl;
cout << endl;
//Generate a range of activation values
actV.resize(100);
amin = actMdl.get_minimum_activation();
for(int i=0; i<actV.size(); i++){
actV(i) = (1-amin)*((double)i)/((double)actV.size()-1.0) + amin;
}
//Generate a detailed range of excitations
uV.resize(10);
for(int i=0; i<uV.size(); i++){
uV(i) = ((double)i)/((double)uV.size()-1.0);
}
maxDxDiff = 0;
for(int i=0;i<uV.size();i++){
for(int j=0; j<actV.size(); j++){
xM(j,0) = actV(j);
xM(j,1) = uV(i);
dx(j) = actMdl.calcDerivative(actV(j), uV(i));
}
d2x = calcCentralDifference(xM(0), dx,true);
for(int j=1;j<uV.size();j++){
tmp = abs(d2x(j)-d2x(j-1));
if( tmp > maxDxDiff)
maxDxDiff = tmp;
}
d3x = calcCentralDifference(xM(0), d2x, true);
continuous = isFunctionContinuous(xM(0), dx, d2x, d3x,
minTol,taylorMult);
SimTK_TEST(continuous);
}
printf("PASSED: with an allowable error of minTol of %f\n"
" or %f x the next Taylor series term. These values\n"
" are reasonable given the elbow in the function and\n"
" the maximum difference in slope of %f\n"
,minTol,taylorMult,maxDxDiff);
cout<<"*****************************************************"<<endl;
cout<<"TEST: Exceptions thrown correctly. "<<endl;
cout << endl;
SimTK_END_TEST();
}
catch (const OpenSim::Exception& ex)
{
cout << ex.getMessage() << endl;
cin.get();
return 1;
}
catch (const std::exception& ex)
{
cout << ex.what() << endl;
cin.get();
return 1;
}
catch (...)
{
cout << "UNRECOGNIZED EXCEPTION" << endl;
cin.get();
return 1;
}
cout << "\ntestMuscleFirstOrderDynamicModel completed successfully .\n";
return 0;
}
