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