780 lines
27 KiB
C++
780 lines
27 KiB
C++
/* -------------------------------------------------------------------------- *
|
|
* OpenSim: testSmoothSegmentedFunctionFactory.cpp *
|
|
* -------------------------------------------------------------------------- *
|
|
* The OpenSim API is a toolkit for musculoskeletal modeling and simulation. *
|
|
* See http://opensim.stanford.edu and the NOTICE file for more information. *
|
|
* OpenSim is developed at Stanford University and supported by the US *
|
|
* National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA *
|
|
* through the Warrior Web program. *
|
|
* *
|
|
* Copyright (c) 2005-2012 Stanford University and the Authors *
|
|
* Author(s): Matthew Millard *
|
|
* *
|
|
* Licensed under the Apache License, Version 2.0 (the "License"); you may *
|
|
* not use this file except in compliance with the License. You may obtain a *
|
|
* copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
|
|
* *
|
|
* Unless required by applicable law or agreed to in writing, software *
|
|
* distributed under the License is distributed on an "AS IS" BASIS, *
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
|
|
* See the License for the specific language governing permissions and *
|
|
* limitations under the License. *
|
|
* -------------------------------------------------------------------------- */
|
|
/*
|
|
Update:
|
|
This is a port of the original code so that it will work with
|
|
the multibody code RBDL written by Martin Felis.
|
|
|
|
This also includes additional curves (the Anderson2007 curves)
|
|
which are not presently in OpenSim.
|
|
|
|
Author:
|
|
Matthew Millard
|
|
|
|
Date:
|
|
Nov 2015
|
|
|
|
*/
|
|
/*
|
|
Below is a basic bench mark simulation for the SmoothSegmentedFunctionFactory
|
|
class, a class that enables the easy generation of C2 continuous curves
|
|
that define the various characteristic curves required in a muscle model
|
|
*/
|
|
|
|
// Author: Matthew Millard
|
|
|
|
//==============================================================================
|
|
// INCLUDES
|
|
//==============================================================================
|
|
|
|
#include "../TorqueMuscleFunctionFactory.h"
|
|
#include "../../geometry/geometry.h"
|
|
#include "../../geometry/tests/numericalTestFunctions.h"
|
|
|
|
|
|
#include <UnitTest++.h>
|
|
#include <rbdl/rbdl_math.h>
|
|
#include <ctime>
|
|
#include <string>
|
|
#include <stdio.h>
|
|
#include <exception>
|
|
#include <cassert>
|
|
|
|
using namespace RigidBodyDynamics::Addons::Geometry;
|
|
using namespace RigidBodyDynamics::Addons::Muscle;
|
|
using namespace std;
|
|
|
|
/*
|
|
static double EPSILON = numeric_limits<double>::epsilon();
|
|
|
|
static bool FLAG_PLOT_CURVES = false;
|
|
static string FILE_PATH = "";
|
|
static double TOL_DX = 5e-3;
|
|
static double TOL_DX_BIG = 1e-2;
|
|
static double TOL_BIG = 1e-6;
|
|
static double TOL_SMALL = 1e-12;
|
|
*/
|
|
|
|
TEST(Anderson2007ActiveTorqueAngleCurve)
|
|
{
|
|
double subjectWeight = 75.0*9.81;
|
|
double subjectHeight = 1.75;
|
|
double scale = subjectHeight*subjectWeight;
|
|
|
|
//These parameters are taken from table 3 for hip extension for
|
|
//men between the ages of 18-25
|
|
double c1 = 0.161; //normalized maximum hip joint torque
|
|
double c2 = 0.958; // pi/(theta_max - theta_min)
|
|
double c3 = 0.932; //theta_max_iso_torque
|
|
double c4 = 1.578; //omega_1: angular velocity at 75% tq_iso_max
|
|
double c5 = 3.190; //omega_2: angular velocity at 50% tq_iso_max
|
|
double c6 = 0.242; //E, where eccentric slope = (1+E)*concentricSlope
|
|
//Passive torque angle curve parameters
|
|
double b1 =-1.210; // torque_passive = b1*exp(k1*theta)
|
|
double k1 =-6.351; // +b2*exp(k2*theta)
|
|
double b2 = 0.476;
|
|
double k2 = 5.910;
|
|
|
|
|
|
|
|
//cout <<endl;
|
|
//cout <<endl;
|
|
//cout <<"**************************************************"<<endl;
|
|
//cout <<"ANDERSON 2007 ACTIVE TORQUE ANGLE CURVE TESTING "<<endl;
|
|
|
|
SmoothSegmentedFunction andersonTaCurve = SmoothSegmentedFunction();
|
|
TorqueMuscleFunctionFactory::
|
|
createAnderson2007ActiveTorqueAngleCurve(c2,c3,
|
|
"test_Anderson2007TorqueAngleCurve",
|
|
andersonTaCurve);
|
|
|
|
double angleRange = (M_PI/c4);
|
|
double angleActiveMin = c3 - angleRange*0.75;
|
|
double angleActiveMax = c3 + angleRange*0.75;
|
|
|
|
RigidBodyDynamics::Math::MatrixNd andersonTaCurveSample
|
|
= andersonTaCurve.calcSampledCurve( 6,
|
|
angleActiveMin,
|
|
angleActiveMax);
|
|
|
|
//cout << " Keypoint Testing" << endl;
|
|
|
|
CHECK(abs(andersonTaCurve.calcValue(c3) - 1.0) < TOL_SMALL);
|
|
CHECK(abs(andersonTaCurve.calcDerivative(c3,1)) < TOL_BIG);
|
|
CHECK(abs(andersonTaCurve.calcDerivative(c3,2)) < TOL_BIG);
|
|
|
|
RigidBodyDynamics::Math::VectorNd curveDomain
|
|
= andersonTaCurve.getCurveDomain();
|
|
|
|
CHECK(abs(andersonTaCurve.calcValue(curveDomain[0])) < TOL_SMALL);
|
|
CHECK(abs(andersonTaCurve.calcDerivative(curveDomain[0],1)) < TOL_BIG);
|
|
CHECK(abs(andersonTaCurve.calcValue(curveDomain[1])) < TOL_SMALL);
|
|
CHECK(abs(andersonTaCurve.calcDerivative(curveDomain[1],1)) < TOL_BIG);
|
|
//cout << " passed " << endl;
|
|
|
|
//cout << " Continuity and Smoothness Testing" << endl;
|
|
bool areCurveDerivativesGood =
|
|
areCurveDerivativesCloseToNumericDerivatives(
|
|
andersonTaCurve,
|
|
andersonTaCurveSample,
|
|
TOL_DX);
|
|
CHECK(areCurveDerivativesGood);
|
|
|
|
bool curveIsContinuous = isCurveC2Continuous( andersonTaCurve,
|
|
andersonTaCurveSample,
|
|
TOL_BIG);
|
|
CHECK(curveIsContinuous);
|
|
|
|
if(FLAG_PLOT_CURVES){
|
|
andersonTaCurve.printCurveToCSVFile(
|
|
FILE_PATH,
|
|
"anderson2007ActiveTorqueAngleCurve",
|
|
angleActiveMin,
|
|
angleActiveMax);
|
|
}
|
|
|
|
|
|
}
|
|
|
|
TEST(Anderson2007PassiveTorqueAngleCurve)
|
|
{
|
|
double subjectWeight = 75.0*9.81;
|
|
double subjectHeight = 1.75;
|
|
double scale = subjectHeight*subjectWeight;
|
|
|
|
//These parameters are taken from table 3 for hip extension for
|
|
//men between the ages of 18-25
|
|
double c1 = 0.161; //normalized maximum hip joint torque
|
|
double c2 = 0.958; // pi/(theta_max - theta_min)
|
|
double c3 = 0.932; //theta_max_iso_torque
|
|
double c4 = 1.578; //omega_1: angular velocity at 75% tq_iso_max
|
|
double c5 = 3.190; //omega_2: angular velocity at 50% tq_iso_max
|
|
double c6 = 0.242; //E, where eccentric slope = (1+E)*concentricSlope
|
|
//Passive torque angle curve parameters
|
|
double b1 =-1.210; // torque_passive = b1*exp(k1*theta)
|
|
double k1 =-6.351; // +b2*exp(k2*theta)
|
|
double b2 = 0.476;
|
|
double k2 = 5.910;
|
|
|
|
|
|
//cout <<endl;
|
|
//cout <<endl;
|
|
//cout <<"**************************************************"<<endl;
|
|
//cout <<"ANDERSON 2007 PASSIVE TORQUE ANGLE CURVE TESTING "<<endl;
|
|
|
|
double curveSign = 1.0;
|
|
|
|
|
|
|
|
for(int z = 0; z<2; ++z){
|
|
|
|
if(z == 0){
|
|
curveSign = 1.0;
|
|
//cout <<" TESTING SIDE 1"<<endl;
|
|
}else{
|
|
curveSign = -1.0;
|
|
//cout <<" TESTING SIDE 2"<<endl;
|
|
|
|
}
|
|
SmoothSegmentedFunction andersonTpCurve = SmoothSegmentedFunction();
|
|
TorqueMuscleFunctionFactory::
|
|
createAnderson2007PassiveTorqueAngleCurve(
|
|
scale,
|
|
c1,
|
|
b1*curveSign,
|
|
k1,
|
|
b2*curveSign,
|
|
k2,
|
|
"test_passiveTorqueAngleCurve",
|
|
andersonTpCurve);
|
|
|
|
RigidBodyDynamics::Math::VectorNd curveDomain
|
|
= andersonTpCurve.getCurveDomain();
|
|
|
|
double angleMin = curveDomain[0];
|
|
double angleMax = curveDomain[1];
|
|
|
|
RigidBodyDynamics::Math::MatrixNd andersonTpCurveSample
|
|
= andersonTpCurve.calcSampledCurve( 6,
|
|
angleMin-0.1,
|
|
angleMax+0.1);
|
|
|
|
//cout << " Keypoint Testing" << endl;
|
|
|
|
double tauMin = andersonTpCurve.calcValue(angleMin);
|
|
double tauMax = andersonTpCurve.calcValue(angleMax);
|
|
double tauMinAngle = angleMin;
|
|
|
|
if(tauMin > tauMax){
|
|
double tmp = tauMin;
|
|
tauMin = tauMax;
|
|
tauMax = tmp;
|
|
tauMinAngle = angleMax;
|
|
}
|
|
|
|
CHECK( abs(tauMin) < TOL_SMALL);
|
|
CHECK( abs(tauMax - 1.0) < TOL_SMALL);
|
|
CHECK( abs(andersonTpCurve.calcDerivative(tauMinAngle,1)) < TOL_SMALL);
|
|
|
|
//cout << " passed " << endl;
|
|
|
|
//cout << " Continuity and Smoothness Testing " << endl;
|
|
bool areCurveDerivativesGood =
|
|
areCurveDerivativesCloseToNumericDerivatives(
|
|
andersonTpCurve,
|
|
andersonTpCurveSample,
|
|
TOL_DX);
|
|
CHECK(areCurveDerivativesGood);
|
|
|
|
bool curveIsContinuous = isCurveC2Continuous(andersonTpCurve,
|
|
andersonTpCurveSample,
|
|
TOL_BIG);
|
|
CHECK(curveIsContinuous);
|
|
|
|
bool curveIsMonotonic = isCurveMontonic(andersonTpCurveSample);
|
|
CHECK(curveIsMonotonic);
|
|
//cout << " passed " << endl;
|
|
|
|
|
|
}
|
|
|
|
SmoothSegmentedFunction andersonTpCurve = SmoothSegmentedFunction();
|
|
TorqueMuscleFunctionFactory::
|
|
createAnderson2007PassiveTorqueAngleCurve(
|
|
scale,
|
|
c1,
|
|
b1,
|
|
k1,
|
|
b2,
|
|
k2,
|
|
"test_passiveTorqueAngleCurve",
|
|
andersonTpCurve);
|
|
|
|
if(FLAG_PLOT_CURVES){
|
|
andersonTpCurve.printCurveToCSVFile(
|
|
FILE_PATH,
|
|
"anderson2007PassiveTorqueAngleCurve",
|
|
andersonTpCurve.getCurveDomain()[0]-0.1,
|
|
andersonTpCurve.getCurveDomain()[1]+0.1);
|
|
}
|
|
|
|
}
|
|
|
|
TEST(Anderson2007ActiveTorqueVelocityCurve)
|
|
{
|
|
double subjectWeight = 75.0*9.81;
|
|
double subjectHeight = 1.75;
|
|
double scale = subjectHeight*subjectWeight;
|
|
|
|
//These parameters are taken from table 3 for hip extension for
|
|
//men between the ages of 18-25
|
|
double c1 = 0.161; //normalized maximum hip joint torque
|
|
double c2 = 0.958; // pi/(theta_max - theta_min)
|
|
double c3 = 0.932; //theta_max_iso_torque
|
|
double c4 = 1.578; //omega_1: angular velocity at 75% tq_iso_max
|
|
double c5 = 3.190; //omega_2: angular velocity at 50% tq_iso_max
|
|
double c6 = 0.242; //E, where eccentric slope = (1+E)*concentricSlope
|
|
//Passive torque angle curve parameters
|
|
double b1 =-1.210; // torque_passive = b1*exp(k1*theta)
|
|
double k1 =-6.351; // +b2*exp(k2*theta)
|
|
double b2 = 0.476;
|
|
double k2 = 5.910;
|
|
|
|
//cout <<endl;
|
|
//cout <<endl;
|
|
//cout <<"**************************************************"<<endl;
|
|
//cout <<"ANDERSON 2007 ACTIVE TORQUE VELOCITY CURVE TESTING"<<endl;
|
|
|
|
double minEccentricMultiplier = 1.1;
|
|
double maxEccentricMultiplier = 1.4;
|
|
|
|
SmoothSegmentedFunction andersonTvCurve = SmoothSegmentedFunction();
|
|
TorqueMuscleFunctionFactory::
|
|
createAnderson2007ActiveTorqueVelocityCurve(
|
|
c4,c5,c6,minEccentricMultiplier,maxEccentricMultiplier,
|
|
"test_anderson2007ActiveTorqueVelocityCurve",
|
|
andersonTvCurve);
|
|
|
|
RigidBodyDynamics::Math::VectorNd curveDomain
|
|
= andersonTvCurve.getCurveDomain();
|
|
|
|
double angularVelocityMin = curveDomain[0];
|
|
double angularVelocityMax = curveDomain[1];
|
|
|
|
|
|
RigidBodyDynamics::Math::MatrixNd andersonTvCurveSample
|
|
= andersonTvCurve.calcSampledCurve( 6,
|
|
angularVelocityMin-0.1,
|
|
angularVelocityMax+0.1);
|
|
|
|
//cout << " Keypoint Testing" << endl;
|
|
|
|
CHECK(abs(andersonTvCurve.calcValue(0) - 1.0) < TOL_SMALL);
|
|
//CHECK(abs(andersonTvCurve.calcValue(c4) - 0.75) < TOL_BIG);
|
|
//CHECK(abs(andersonTvCurve.calcValue(c5) - 0.5) < TOL_BIG);
|
|
double val = abs(andersonTvCurve.calcValue(
|
|
angularVelocityMax/angularVelocityMax));
|
|
CHECK(val < TOL_BIG);
|
|
|
|
double maxTv = andersonTvCurve.calcValue(
|
|
angularVelocityMin/angularVelocityMax);
|
|
CHECK(maxTv >= minEccentricMultiplier);
|
|
CHECK(maxTv <= maxEccentricMultiplier);
|
|
|
|
CHECK(abs(andersonTvCurve.calcDerivative
|
|
(angularVelocityMax/angularVelocityMax,1))<TOL_SMALL);
|
|
CHECK(abs(andersonTvCurve.calcDerivative
|
|
(angularVelocityMin/angularVelocityMax,1))<TOL_SMALL);
|
|
|
|
//cout << " passed " << endl;
|
|
//cout << " Continuity and Smoothness Testing" << endl;
|
|
|
|
bool areCurveDerivativesGood =
|
|
areCurveDerivativesCloseToNumericDerivatives(
|
|
andersonTvCurve,
|
|
andersonTvCurveSample,
|
|
TOL_DX);
|
|
|
|
CHECK(areCurveDerivativesGood);
|
|
|
|
bool curveIsContinuous = isCurveC2Continuous( andersonTvCurve,
|
|
andersonTvCurveSample,
|
|
TOL_BIG);
|
|
CHECK(curveIsContinuous);
|
|
|
|
bool curveIsMonotonic = isCurveMontonic(andersonTvCurveSample);
|
|
CHECK(curveIsMonotonic);
|
|
|
|
|
|
if(FLAG_PLOT_CURVES){
|
|
andersonTvCurve.printCurveToCSVFile(
|
|
FILE_PATH,
|
|
"anderson2007ActiveTorqueVelocityCurve",
|
|
angularVelocityMin,
|
|
angularVelocityMax);
|
|
}
|
|
|
|
}
|
|
//==============================================================================
|
|
TEST(TorqueAngularVelocityCurve)
|
|
{
|
|
SmoothSegmentedFunction tv = SmoothSegmentedFunction();
|
|
SmoothSegmentedFunction tvX = SmoothSegmentedFunction();
|
|
double tvAtEccentricOmegaMax = 1.3;
|
|
double tvAtHalfConcentricOmegaMax = 0.3;
|
|
std::string curveName0("tvTest0");
|
|
std::string curveName1("tvTest1");
|
|
|
|
TorqueMuscleFunctionFactory::createTorqueVelocityCurve(
|
|
tvAtEccentricOmegaMax,
|
|
tvAtHalfConcentricOmegaMax,
|
|
curveName0,
|
|
tv);
|
|
double slopeAtConcentricOmegaMax = -0.1;
|
|
double slopeAtEccentricOmegaMax = -0.1;
|
|
double slopeNearEccentricOmegaMax = -0.01;
|
|
double eccentricCurviness = 0.75;
|
|
TorqueMuscleFunctionFactory::createTorqueVelocityCurve(
|
|
tvAtEccentricOmegaMax,
|
|
tvAtHalfConcentricOmegaMax,
|
|
slopeAtConcentricOmegaMax,
|
|
slopeNearEccentricOmegaMax,
|
|
slopeAtEccentricOmegaMax,
|
|
eccentricCurviness,
|
|
curveName1,
|
|
tvX);
|
|
|
|
double wmin = -1.1;
|
|
double wmax = 1.1;
|
|
int npts = 100;
|
|
double delta = (wmax-wmin)/((double)npts-1.0);
|
|
|
|
|
|
//Get the parameters for the Hill type curve
|
|
double wmaxC = 1.0;
|
|
double wmaxE = -1.0;
|
|
double fiso = 1.0;
|
|
double w = 0.5*wmaxC;
|
|
double a = -tvAtHalfConcentricOmegaMax*w*fiso
|
|
/ (wmaxC*tvAtHalfConcentricOmegaMax
|
|
- fiso*wmaxC + fiso*w);
|
|
double b = a*wmaxC/fiso;
|
|
double tvHill = 0;
|
|
double errHill = 0;
|
|
for(int i=0; i<npts; ++i){
|
|
w = wmin + i*delta;
|
|
if(w > wmaxC){
|
|
CHECK( abs( tv.calcValue(w) ) < TOL_SMALL);
|
|
CHECK( abs( tv.calcDerivative(w,1) ) < TOL_SMALL);
|
|
CHECK( abs(tvX.calcDerivative(w,1)
|
|
- slopeAtConcentricOmegaMax ) < TOL_BIG);
|
|
}else if(w > 0 && w <= wmaxC){
|
|
tvHill = (b*fiso-a*w)/(b+w);
|
|
errHill = abs(tv.calcValue(w)-tvHill);
|
|
//printf("%i. Err %f, ",i,errHill);
|
|
CHECK( errHill < 0.02);
|
|
errHill = abs(tvX.calcValue(w)-tvHill);
|
|
//printf(" Err %f\n",errHill);
|
|
CHECK( errHill < 0.02);
|
|
}else if(w < 0 & w > wmaxE){
|
|
CHECK(tv.calcValue(w) > 1.0);
|
|
}else if(w < wmaxE){
|
|
CHECK(abs( tv.calcValue(w) - tvAtEccentricOmegaMax ) < TOL_SMALL);
|
|
CHECK(abs( tv.calcDerivative(w,1) - 0.0 ) < TOL_SMALL);
|
|
//CHECK(abs( tvX.calcValue(w) - tvAtEccentricOmegaMax ) );
|
|
CHECK(abs( tvX.calcDerivative(w,1)
|
|
- slopeAtEccentricOmegaMax ) < TOL_SMALL );
|
|
}
|
|
}
|
|
|
|
RigidBodyDynamics::Math::VectorNd curveDomain = tv.getCurveDomain();
|
|
|
|
double angularVelocityMin = curveDomain[0];
|
|
double angularVelocityMax = curveDomain[1];
|
|
|
|
|
|
RigidBodyDynamics::Math::MatrixNd tvCurveSample
|
|
= tv.calcSampledCurve( 6,
|
|
angularVelocityMin-0.1,
|
|
angularVelocityMax+0.1);
|
|
|
|
bool areCurveDerivativesGood =
|
|
areCurveDerivativesCloseToNumericDerivatives(
|
|
tv,
|
|
tvCurveSample,
|
|
TOL_DX);
|
|
|
|
CHECK(areCurveDerivativesGood);
|
|
|
|
bool curveIsContinuous = isCurveC2Continuous( tv,
|
|
tvCurveSample,
|
|
TOL_BIG);
|
|
CHECK(curveIsContinuous);
|
|
|
|
bool curveIsMonotonic = isCurveMontonic(tvCurveSample);
|
|
CHECK(curveIsMonotonic);
|
|
|
|
if(FLAG_PLOT_CURVES){
|
|
tv.printCurveToCSVFile(
|
|
FILE_PATH,
|
|
"millard2016TorqueVelocityCurve",
|
|
-1.1,
|
|
1.1);
|
|
}
|
|
tv.printCurveToCSVFile(
|
|
"",
|
|
"millard2016TorqueVelocityCurve",
|
|
-1.1,
|
|
1.1);
|
|
|
|
}
|
|
|
|
//==============================================================================
|
|
TEST(PassiveTorqueAngleCurve)
|
|
{
|
|
SmoothSegmentedFunction tp = SmoothSegmentedFunction();
|
|
SmoothSegmentedFunction tpX = SmoothSegmentedFunction();
|
|
double angleAtZeroTorque0 = 0;
|
|
double angleAtOneNormTorque0 = -M_PI;
|
|
|
|
double angleAtZeroTorque1 = 0;
|
|
double angleAtOneNormTorque1 = M_PI;
|
|
|
|
double stiffnessAtOneNormTorque1 =
|
|
5.6/(angleAtOneNormTorque1-angleAtZeroTorque1);
|
|
double stiffnessAtLowTorque1 =
|
|
0.05*stiffnessAtOneNormTorque1;
|
|
double curviness1 = 0.75;
|
|
|
|
std::string curveName0("tpTest0");
|
|
std::string curveName1("tpTest1");
|
|
|
|
|
|
TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve(
|
|
angleAtZeroTorque0,
|
|
angleAtOneNormTorque0,
|
|
curveName0,
|
|
tp);
|
|
|
|
TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve(
|
|
angleAtZeroTorque1,
|
|
angleAtOneNormTorque1,
|
|
stiffnessAtLowTorque1,
|
|
stiffnessAtOneNormTorque1,
|
|
curviness1,
|
|
curveName1,
|
|
tpX);
|
|
|
|
CHECK( abs(tp.calcValue(angleAtZeroTorque0)) < TOL_SMALL);
|
|
CHECK( abs(tp.calcValue(angleAtOneNormTorque0)-1.0) < TOL_SMALL);
|
|
|
|
CHECK( abs(tpX.calcValue(angleAtZeroTorque1)) < TOL_SMALL);
|
|
CHECK( abs(tpX.calcValue(angleAtOneNormTorque1) - 1.0) < TOL_SMALL);
|
|
CHECK( abs(tpX.calcDerivative(angleAtZeroTorque1,1)) < TOL_SMALL);
|
|
CHECK( abs(tpX.calcDerivative(angleAtOneNormTorque1,1)
|
|
-stiffnessAtOneNormTorque1) < TOL_SMALL);
|
|
|
|
RigidBodyDynamics::Math::VectorNd curveDomain0 = tp.getCurveDomain();
|
|
RigidBodyDynamics::Math::VectorNd curveDomain1 = tpX.getCurveDomain();
|
|
|
|
RigidBodyDynamics::Math::MatrixNd tpSample0
|
|
= tp.calcSampledCurve( 6,
|
|
curveDomain0[0]-0.1,
|
|
curveDomain0[1]+0.1);
|
|
|
|
RigidBodyDynamics::Math::MatrixNd tpSample1
|
|
= tpX.calcSampledCurve( 6,
|
|
curveDomain1[0]-0.1,
|
|
curveDomain1[1]+0.1);
|
|
|
|
bool areCurveDerivativesGood =
|
|
areCurveDerivativesCloseToNumericDerivatives(
|
|
tp,
|
|
tpSample0,
|
|
TOL_DX);
|
|
|
|
CHECK(areCurveDerivativesGood);
|
|
|
|
areCurveDerivativesGood =
|
|
areCurveDerivativesCloseToNumericDerivatives(
|
|
tpX,
|
|
tpSample1,
|
|
TOL_DX);
|
|
|
|
CHECK(areCurveDerivativesGood);
|
|
bool curveIsContinuous = isCurveC2Continuous( tp,
|
|
tpSample0,
|
|
TOL_BIG);
|
|
CHECK(curveIsContinuous);
|
|
curveIsContinuous = isCurveC2Continuous(tpX,
|
|
tpSample1,
|
|
TOL_BIG);
|
|
CHECK(curveIsContinuous);
|
|
|
|
bool curveIsMonotonic = isCurveMontonic(tpSample0);
|
|
CHECK(curveIsMonotonic);
|
|
|
|
curveIsMonotonic = isCurveMontonic(tpSample1);
|
|
CHECK(curveIsMonotonic);
|
|
|
|
|
|
if(FLAG_PLOT_CURVES){
|
|
tp.printCurveToCSVFile(
|
|
FILE_PATH,
|
|
"millard2016PassiveTorqueAngleCurve",
|
|
curveDomain0[0]-0.1,
|
|
curveDomain0[1]+0.1);
|
|
}
|
|
}
|
|
|
|
//==============================================================================
|
|
TEST(ActiveTorqueAngleCurve)
|
|
{
|
|
SmoothSegmentedFunction ta = SmoothSegmentedFunction();
|
|
SmoothSegmentedFunction taX = SmoothSegmentedFunction();
|
|
double angleAtOneNormTorque = 1.5;
|
|
double angularStandardDeviation = 1.0;
|
|
double angularStandardDeviationSq =
|
|
angularStandardDeviation*angularStandardDeviation;
|
|
|
|
double minSlopeAtShoulders = 0.2;
|
|
double minValueAtShoulders = 0.1;
|
|
double xTrans = sqrt(-log(minValueAtShoulders)*2*angularStandardDeviationSq);
|
|
double delta = abs(xTrans+angleAtOneNormTorque);
|
|
double curviness = 0.75;
|
|
|
|
std::string curveName0("tpTest0");
|
|
std::string curveName1("tpTest1");
|
|
|
|
|
|
TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve(
|
|
angleAtOneNormTorque,
|
|
angularStandardDeviation,
|
|
curveName0,
|
|
ta);
|
|
|
|
TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve(
|
|
angleAtOneNormTorque,
|
|
angularStandardDeviation,
|
|
minSlopeAtShoulders,
|
|
minValueAtShoulders,
|
|
curviness,
|
|
curveName1,
|
|
taX);
|
|
|
|
CHECK(abs(ta.calcValue(angleAtOneNormTorque)-1.0) < TOL_SMALL);
|
|
|
|
CHECK(abs(ta.calcValue(angleAtOneNormTorque
|
|
+10.0*angularStandardDeviation)) < TOL_SMALL);
|
|
CHECK(abs(ta.calcValue(angleAtOneNormTorque
|
|
-10.0*angularStandardDeviation)) < TOL_SMALL);
|
|
|
|
CHECK(abs(taX.calcValue(angleAtOneNormTorque)-1.0) < TOL_SMALL);
|
|
|
|
double err = abs(taX.calcDerivative(angleAtOneNormTorque + delta,1)
|
|
+ minSlopeAtShoulders);
|
|
CHECK(err < TOL_SMALL);
|
|
err = abs(taX.calcDerivative(angleAtOneNormTorque - delta,1)
|
|
- minSlopeAtShoulders);
|
|
CHECK(err < TOL_SMALL);
|
|
|
|
|
|
RigidBodyDynamics::Math::VectorNd curveDomain0 = ta.getCurveDomain();
|
|
RigidBodyDynamics::Math::VectorNd curveDomain1 = taX.getCurveDomain();
|
|
|
|
RigidBodyDynamics::Math::MatrixNd taSample0
|
|
= ta.calcSampledCurve( 6,
|
|
curveDomain0[0]-0.1,
|
|
curveDomain0[1]+0.1);
|
|
|
|
RigidBodyDynamics::Math::MatrixNd taSample1
|
|
= taX.calcSampledCurve( 6,
|
|
curveDomain1[0]-0.1,
|
|
curveDomain1[1]+0.1);
|
|
|
|
bool areCurveDerivativesGood =
|
|
areCurveDerivativesCloseToNumericDerivatives(
|
|
ta,
|
|
taSample0,
|
|
TOL_DX);
|
|
|
|
|
|
CHECK(areCurveDerivativesGood);
|
|
|
|
areCurveDerivativesGood =
|
|
areCurveDerivativesCloseToNumericDerivatives(
|
|
taX,
|
|
taSample1,
|
|
TOL_DX);
|
|
|
|
CHECK(areCurveDerivativesGood);
|
|
bool curveIsContinuous = isCurveC2Continuous( ta,
|
|
taSample0,
|
|
TOL_BIG);
|
|
CHECK(curveIsContinuous);
|
|
curveIsContinuous = isCurveC2Continuous(taX,
|
|
taSample1,
|
|
TOL_BIG);
|
|
CHECK(curveIsContinuous);
|
|
|
|
if(FLAG_PLOT_CURVES){
|
|
ta.printCurveToCSVFile(
|
|
FILE_PATH,
|
|
"millard2016ActiveTorqueAngleCurve",
|
|
curveDomain0[0]-0.1,
|
|
curveDomain0[1]+0.1);
|
|
}
|
|
|
|
}
|
|
|
|
//==============================================================================
|
|
TEST(TendonTorqueAngleCurve)
|
|
{
|
|
SmoothSegmentedFunction tt = SmoothSegmentedFunction();
|
|
SmoothSegmentedFunction ttX = SmoothSegmentedFunction();
|
|
|
|
double angularStretchAtOneNormTorque = M_PI/2.0;
|
|
double stiffnessAtOneNormTorque = 2.5/angularStretchAtOneNormTorque;
|
|
double normTorqueAtToeEnd = 2.0/3.0;
|
|
double curviness = 0.5;
|
|
|
|
std::string curveName0("ttTest0");
|
|
std::string curveName1("ttTest1");
|
|
|
|
|
|
TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve(
|
|
angularStretchAtOneNormTorque,
|
|
curveName0,
|
|
tt);
|
|
|
|
TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve(
|
|
angularStretchAtOneNormTorque,
|
|
stiffnessAtOneNormTorque,
|
|
normTorqueAtToeEnd,
|
|
curviness,
|
|
curveName1,
|
|
ttX);
|
|
|
|
CHECK(abs(tt.calcValue(0)) < TOL_SMALL);
|
|
CHECK(abs(ttX.calcValue(0)) < TOL_SMALL);
|
|
|
|
CHECK(abs(tt.calcValue(angularStretchAtOneNormTorque)-1.0) < TOL_SMALL);
|
|
CHECK(abs(ttX.calcValue(angularStretchAtOneNormTorque)-1.0) < TOL_SMALL);
|
|
|
|
CHECK(abs(ttX.calcDerivative(angularStretchAtOneNormTorque,1)
|
|
- stiffnessAtOneNormTorque) < TOL_SMALL);
|
|
|
|
RigidBodyDynamics::Math::VectorNd curveDomain0 = tt.getCurveDomain();
|
|
RigidBodyDynamics::Math::VectorNd curveDomain1 = ttX.getCurveDomain();
|
|
|
|
RigidBodyDynamics::Math::MatrixNd ttSample0
|
|
= tt.calcSampledCurve( 6,
|
|
curveDomain0[0]-0.1,
|
|
curveDomain0[1]+0.1);
|
|
|
|
RigidBodyDynamics::Math::MatrixNd ttSample1
|
|
= ttX.calcSampledCurve( 6,
|
|
curveDomain1[0]-0.1,
|
|
curveDomain1[1]+0.1);
|
|
|
|
bool areCurveDerivativesGood =
|
|
areCurveDerivativesCloseToNumericDerivatives(
|
|
tt,
|
|
ttSample0,
|
|
TOL_DX);
|
|
|
|
CHECK(areCurveDerivativesGood);
|
|
|
|
areCurveDerivativesGood =
|
|
areCurveDerivativesCloseToNumericDerivatives(
|
|
ttX,
|
|
ttSample1,
|
|
TOL_DX);
|
|
|
|
CHECK(areCurveDerivativesGood);
|
|
bool curveIsContinuous = isCurveC2Continuous( tt,
|
|
ttSample0,
|
|
TOL_BIG);
|
|
CHECK(curveIsContinuous);
|
|
curveIsContinuous = isCurveC2Continuous(ttX,
|
|
ttSample1,
|
|
TOL_BIG);
|
|
CHECK(curveIsContinuous);
|
|
|
|
bool curveIsMonotonic = isCurveMontonic(ttSample0);
|
|
CHECK(curveIsMonotonic);
|
|
|
|
curveIsMonotonic = isCurveMontonic(ttSample1);
|
|
CHECK(curveIsMonotonic);
|
|
|
|
|
|
if(FLAG_PLOT_CURVES){
|
|
tt.printCurveToCSVFile(
|
|
FILE_PATH,
|
|
"millard2016ActiveTorqueAngleCurve",
|
|
curveDomain0[0]-0.1,
|
|
angularStretchAtOneNormTorque);
|
|
}
|
|
|
|
}
|