martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
rbdlsim/3rdparty/rbdl/addons/muscle/tests/testMillard2016TorqueMuscle.cc

1720 lines
66 KiB
C++
Raw Normal View History

Initial commit
2020-10-03 22:55:14 +02:00
/* *
*
* Copyright (c) 2016 Matthew Millard <matthew.millard@iwr.uni-heidelberg.de>
*
* Licensed under the zlib license. See LICENSE for more details.
*/
//Some particularly aggressive experimental data
unsigned int TorqueMuscleFittingHardTestCaseRows = 100;
unsigned int TorqueMuscleFittingHardTestCaseCols = 4;
double const TorqueMuscleFittingHardTestCase[100][4] =
{{0.,1.9148,-17.618,107.25 },
{0.0044242,1.8318,-18.277,108.39 },
{0.0088485,1.7485,-18.949,110.49 },
{0.013273,1.6636,-19.618,114.57 },
{0.017697,1.5761,-20.243,121.32 },
{0.022121,1.486,-20.758,130.97 },
{0.026545,1.3938,-21.085,143.31 },
{0.03097,1.3004,-21.144,157.68 },
{0.035394,1.2074,-20.866,173.11 },
{0.039818,1.1164,-20.199,188.42 },
{0.044242,1.0292,-19.118,202.36 },
{0.048667,0.94777,-17.626,213.77 },
{0.053091,0.87376,-15.762,221.64 },
{0.057515,0.80872,-13.594,225.25 },
{0.061939,0.75378,-11.22,224.18 },
{0.066364,0.70958,-8.7587,218.42 },
{0.070788,0.67624,-6.3387,208.34 },
{0.075212,0.65329,-4.0821,194.73 },
{0.079636,0.63975,-2.0906,178.7 },
{0.084061,0.63427,-0.43189,161.6 },
{0.088485,0.63528,0.87052,144.75 },
{0.092909,0.6412,1.8387,129.18 },
{0.097333,0.6507,2.5306,115.24 },
{0.10176,0.66277,3.015,102.36 },
{0.10618,0.67685,3.3393,88.889 },
{0.11061,0.69264,3.517,72.481 },
{0.11503,0.71007,3.5756,50.979 },
{0.11945,0.72927,3.6372,23.504 },
{0.12388,0.75048,3.852,-8.9796 },
{0.1283,0.77332,4.1461,-43.995 },
{0.13273,0.79611,4.1835,-78.49 },
{0.13715,0.81635,3.6883,-109.71 },
{0.14158,0.83172,2.6498,-135.63 },
{0.146,0.84066,1.2202,-154.98 },
{0.15042,0.84231,-0.42406,-167.16 },
{0.15485,0.83637,-2.1355,-172.03 },
{0.15927,0.82297,-3.8027,-169.8 },
{0.1637,0.80254,-5.3408,-160.95 },
{0.16812,0.7758,-6.6819,-146.16 },
{0.17255,0.74371,-7.771,-126.31 },
{0.17697,0.70746,-8.5684,-102.56 },
{0.18139,0.66837,-9.0566,-76.369 },
{0.18582,0.62778,-9.2487,-49.623 },
{0.19024,0.58691,-9.1941,-24.563 },
{0.19467,0.54667,-8.9737,-3.6038 },
{0.19909,0.50763,-8.6741,11.165 },
{0.20352,0.47001,-8.3313,18.713 },
{0.20794,0.43418,-7.8528,19.766 },
{0.21236,0.40128,-6.9609,16.835 },
{0.21679,0.37397,-5.266,12.889 },
{0.22121,0.35642,-2.5349,9.4338 },
{0.22564,0.35296,1.0546,6.2364 },
{0.23006,0.36634,4.9935,2.8315 },
{0.23448,0.39689,8.7586,-0.69647 },
{0.23891,0.44304,11.998,-4.1689 },
{0.24333,0.50198,14.522,-7.5873 },
{0.24776,0.57037,16.269,-11.01 },
{0.25218,0.64488,17.306,-14.511 },
{0.25661,0.72269,17.793,-18.219 },
{0.26103,0.8018,17.929,-22.301 },
{0.26545,0.8811,17.902,-26.91 },
{0.26988,0.96017,17.846,-32.128 },
{0.2743,1.0391,17.834,-37.93 },
{0.27873,1.1181,17.879,-44.158 },
{0.28315,1.1973,17.961,-50.527 },
{0.28758,1.277,18.039,-56.666 },
{0.292,1.3569,18.078,-62.174 },
{0.29642,1.4368,18.054,-66.692 },
{0.30085,1.5165,17.955,-69.98 },
{0.30527,1.5956,17.784,-71.983 },
{0.3097,1.6738,17.552,-72.861 },
{0.31412,1.7509,17.272,-72.976 },
{0.31855,1.8266,16.952,-72.834 },
{0.32297,1.9008,16.594,-72.982 },
{0.32739,1.9733,16.191,-73.899 },
{0.33182,2.0439,15.724,-75.903 },
{0.33624,2.1123,15.162,-79.088 },
{0.34067,2.1779,14.47,-83.322 },
{0.34509,2.24,13.609,-88.273 },
{0.34952,2.2979,12.55,-93.472 },
{0.35394,2.3507,11.283,-98.369 },
{0.35836,2.3974,9.8194,-102.39 },
{0.36279,2.4373,8.1988,-105.01 },
{0.36721,2.4698,6.4835,-105.77 },
{0.37164,2.4947,4.7502,-104.37 },
{0.37606,2.512,3.0759,-100.74 },
{0.38048,2.5221,1.5245,-95.063 },
{0.38491,2.5257,0.13371,-87.783 },
{0.38933,2.5235,-1.0913,-79.537 },
{0.39376,2.5163,-2.178,-71.042 },
{0.39818,2.5045,-3.1764,-62.958 },
{0.40261,2.4884,-4.1428,-55.768 },
{0.40703,2.468,-5.122,-49.731 },
{0.41145,2.443,-6.136,-44.898 },
{0.41588,2.4132,-7.1827,-41.179 },
{0.4203,2.3784,-8.2437,-38.413 },
{0.42473,2.3383,-9.2979,-36.422 },
{0.42915,2.2933,-10.332,-35.021 },
{0.43358,2.2443,-11.347,-34.007 },
{0.438,2.193,-12.35,-33.158 }};
//==============================================================================
// INCLUDES
//==============================================================================
#include "../Millard2016TorqueMuscle.h"
#ifdef RBDL_BUILD_ADDON_MUSCLE_FITTING
#include "../TorqueMuscleFittingToolkit.h"
#endif
#include "../csvtools.h"
#include "../../geometry/tests/numericalTestFunctions.h"
#include <UnitTest++.h>
#include <rbdl/rbdl_math.h>
#include <ctime>
#include <string>
#include <ostream>
#include <sstream>
#include <stdio.h>
#include <exception>
#include <cassert>
#include <vector>
using namespace RigidBodyDynamics::Addons::Muscle;
using namespace RigidBodyDynamics::Addons::Geometry;
using namespace std;
/*
Constructor tests:
1. Coefficients are copied over correctly.
2. Curves are made correctly
calcTorqueMuscleInfo test
stiffness calculation
power calculation
*/
TEST(ConstructorRegularCallCheck)
{
//Check that the 3 constructors when called properly
//do not abort
Millard2016TorqueMuscle test0 = Millard2016TorqueMuscle();
SubjectInformation subjectInfo;
subjectInfo.gender = GenderSet::Male;
subjectInfo.ageGroup = AgeGroupSet::Young18To25;
subjectInfo.heightInMeters = 1.732;
subjectInfo.massInKg = 69.0;
Millard2016TorqueMuscle test2 =
Millard2016TorqueMuscle(
DataSet::Anderson2007,
subjectInfo,
Anderson2007::HipExtension,
0.0,
1.0,
1.0,
"test_easyConstructor");
CHECK(fabs( test2.getPassiveTorqueScale()-1.0) < TOL);
}
TEST(calcJointTorqueCorrectnessTests){
double jointAngleOffset = 0;
double signOfJointAngle = 1;
double signOfJointTorque = 1;
double err = 0.0;
std::string name("test");
SubjectInformation subjectInfo;
subjectInfo.gender = GenderSet::Male;
subjectInfo.ageGroup = AgeGroupSet::Young18To25;
subjectInfo.heightInMeters = 1.732;
subjectInfo.massInKg = 69.0;
Millard2016TorqueMuscle tq =
Millard2016TorqueMuscle(DataSet::Anderson2007,
subjectInfo,
Anderson2007::HipExtension,
jointAngleOffset,
signOfJointAngle,
signOfJointTorque,
name);
bool flagMakeTestVector = false;
if(flagMakeTestVector){
Millard2016TorqueMuscle tqG =
Millard2016TorqueMuscle(DataSet::Gymnast,
subjectInfo,
Gymnast::HipExtension,
jointAngleOffset,
signOfJointAngle,
signOfJointTorque,
name);
TorqueMuscleInfo tmiG;
tqG.calcTorqueMuscleInfo(M_PI/3.0,0.1,0.77,tmiG);
printf("%f\n",tmiG.fiberAngle);
printf("%f\n",tmiG.fiberAngularVelocity);
printf("%f\n",tmiG.activation);
printf("%f\n",tmiG.fiberTorque);
printf("%f\n",tmiG.fiberStiffness);
printf("%f\n",tmiG.fiberPassiveTorqueAngleMultiplier);
printf("%f\n",tmiG.fiberActiveTorqueAngleMultiplier);
printf("%f\n",tmiG.fiberTorqueAngularVelocityMultiplier);
printf("%f\n",tmiG.fiberPassiveTorque);
printf("%f\n",tmiG.fiberActiveTorque);
printf("%f\n",tmiG.fiberDampingTorque);
printf("%f\n",tmiG.fiberNormDampingTorque);
printf("%f\n",tmiG.fiberActivePower);
printf("%f\n",tmiG.fiberPassivePower);
printf("%f\n",tmiG.fiberPower);
printf("%f\n",tmiG.DjointTorque_DjointAngle);
printf("%f\n",tmiG.DjointTorque_DjointAngularVelocity);
printf("%f\n",tmiG.DjointTorque_Dactivation);
}
//Zero out the passive forces so that calcMuscleTorque reports
//just the active force - this allows us to test its correctness.
tq.setPassiveTorqueScale(0.0);
double tmp = tq.calcJointTorque(0,0,1.0);
//Test that the get and set functions work for
//maximum isometric torque
double tauMaxOld = tq.getMaximumActiveIsometricTorque();
double tauMax = tauMaxOld*10.0;
tq.setMaximumActiveIsometricTorque(tauMax);
tmp = tq.calcJointTorque(0,0,1.0);
//ensures updateTorqueMuscleCurves is called
CHECK(fabs( tq.getMaximumActiveIsometricTorque()-tauMax)
< TOL );
double omegaMaxOld = tq.getMaximumConcentricJointAngularVelocity();
double omegaMax = 2.0*fabs(omegaMaxOld);
tq.setMaximumConcentricJointAngularVelocity(omegaMax);
tmp = tq.calcJointTorque(0,0,1.0);
//ensures updateTorqueMuscleCurves is called
CHECK(fabs( fabs(tq.getMaximumConcentricJointAngularVelocity())-omegaMax)
< TOL );
double taAngleScalingOld = tq.getActiveTorqueAngleCurveAngleScaling();
double taAngleScaling = 2.0*taAngleScalingOld;
tq.setActiveTorqueAngleCurveAngleScaling(taAngleScaling);
CHECK(fabs(taAngleScaling-tq.getActiveTorqueAngleCurveAngleScaling()) <TOL);
}
TEST(dampingTermTests){
double err = 0.;
double jointAngleOffset = 0;
double signOfJointAngle = 1;
double signOfJointTorque = 1;
double signOfJointVelocity = signOfJointTorque;
std::string name("test");
SubjectInformation subjectInfo;
subjectInfo.gender = GenderSet::Female;
subjectInfo.ageGroup = AgeGroupSet::SeniorOver65;
subjectInfo.heightInMeters = 1.732;
subjectInfo.massInKg = 69.0;
Millard2016TorqueMuscle tq =
Millard2016TorqueMuscle(DataSet::Anderson2007,
subjectInfo,
Anderson2007::HipExtension,
jointAngleOffset,
signOfJointAngle,
signOfJointTorque,
name);
TorqueMuscleInfo tmi0;
//Test the damping term
double beta = tq.getNormalizedDampingCoefficient();
tq.setNormalizedDampingCoefficient(beta+0.1);
CHECK(fabs(beta+0.1-tq.getNormalizedDampingCoefficient())<SQRTEPSILON);
double omegaMax = tq.getMaximumConcentricJointAngularVelocity();
double tau = tq.calcJointTorque(-M_PI/3.0, omegaMax,0);
CHECK(fabs(tau) < SQRTEPSILON );
tq.calcTorqueMuscleInfo(-M_PI/3.0,omegaMax,0.1,tmi0);
err = fabs(tmi0.activation
*tmi0.fiberActiveTorqueAngleMultiplier
*tmi0.fiberTorqueAngularVelocityMultiplier
+tmi0.fiberPassiveTorqueAngleMultiplier
*tq.getPassiveTorqueScale()
+tmi0.fiberNormDampingTorque);
CHECK( err < SQRTEPSILON);
beta = tq.getNormalizedDampingCoefficient();
double tauMax = tq.getMaximumActiveIsometricTorque();
tq.calcTorqueMuscleInfo(tq.getJointAngleAtOneNormalizedPassiveIsometricTorque(),
-omegaMax,
0,
tmi0);
CHECK( fabs(tmi0.fiberDampingTorque
- 1.0*beta*1.0*tauMax) < SQRTEPSILON );
CHECK( fabs(tmi0.fiberNormDampingTorque -beta*1) < SQRTEPSILON );
}
TEST(simpleFittingFunctionTests){
double err = 0.;
double jointAngleOffset = 0;
double signOfJointAngle = 1;
double signOfJointTorque = 1;
double signOfJointVelocity = signOfJointTorque;
std::string name("test");
SubjectInformation subjectInfo;
subjectInfo.gender = GenderSet::Female;
subjectInfo.ageGroup = AgeGroupSet::SeniorOver65;
subjectInfo.heightInMeters = 1.732;
subjectInfo.massInKg = 69.0;
Millard2016TorqueMuscle tq =
Millard2016TorqueMuscle(DataSet::Anderson2007,
subjectInfo,
Anderson2007::HipExtension,
jointAngleOffset,
signOfJointAngle,
signOfJointTorque,
name);
TorqueMuscleInfo tmi0, tmi1;
tq.setPassiveTorqueScale(1.0);
tq.setPassiveCurveAngleOffset(0.0);
double jointAngleAtPassiveTauMax =
tq.getJointAngleAtOneNormalizedPassiveIsometricTorque();
double activation = 0.1;
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.-SQRTEPSILON,
activation,
tmi0);
tq.setPassiveCurveAngleOffset(M_PI/3.0);
double updJointAngleAtPassiveTauMax =
tq.getJointAngleAtOneNormalizedPassiveIsometricTorque() ;
CHECK( fabs(updJointAngleAtPassiveTauMax-jointAngleAtPassiveTauMax-M_PI/3.0)
< SQRTEPSILON);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax+M_PI/3.0,
0.-SQRTEPSILON,
activation,
tmi1);
CHECK( fabs(tmi0.fiberPassiveTorqueAngleMultiplier
-tmi1.fiberPassiveTorqueAngleMultiplier) < SQRTEPSILON);
//fitPassiveCurveAngleOffset: Extension test
double tauMax = tq.getMaximumActiveIsometricTorque();
jointAngleAtPassiveTauMax
= tq.getJointAngleAtOneNormalizedPassiveIsometricTorque();
tq.fitPassiveCurveAngleOffset(1.0,
tauMax);
tq.calcTorqueMuscleInfo(1.0,
0.,
0.,
tmi0);
CHECK(fabs(tmi0.fiberPassiveTorque - tauMax) < SQRTEPSILON);
//fitPassiveCurveAngleOffset: flexion test
Millard2016TorqueMuscle tqF =
Millard2016TorqueMuscle(DataSet::Anderson2007,
subjectInfo,
Anderson2007::HipFlexion,
jointAngleOffset,
signOfJointAngle,
-1*signOfJointTorque,
"flexion");
tauMax = tqF.getMaximumActiveIsometricTorque();
jointAngleAtPassiveTauMax =
tqF.getJointAngleAtOneNormalizedPassiveIsometricTorque();
tqF.fitPassiveCurveAngleOffset(-1.0, tauMax);
tqF.calcTorqueMuscleInfo(-1.0,
0.,
0.,
tmi0);
CHECK(fabs(tmi0.fiberPassiveTorque - tauMax) < SQRTEPSILON);
tauMax = tq.getMaximumActiveIsometricTorque();
jointAngleAtPassiveTauMax
= tq.getJointAngleAtOneNormalizedPassiveIsometricTorque();
tq.fitPassiveTorqueScale(jointAngleAtPassiveTauMax, tauMax*0.5);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.,
0.,
tmi0);
CHECK(fabs(tmi0.fiberPassiveTorque - tauMax*0.5) < SQRTEPSILON);
//Now for the flexor ...
tauMax = tqF.getMaximumActiveIsometricTorque();
jointAngleAtPassiveTauMax
= tqF.getJointAngleAtOneNormalizedPassiveIsometricTorque();
tqF.fitPassiveTorqueScale(jointAngleAtPassiveTauMax, tauMax*0.5);
tqF.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.,
0.,
tmi0);
CHECK(fabs(tmi0.fiberPassiveTorque - tauMax*0.5) < SQRTEPSILON);
//Now test the calcMaximumActiveIsometricTorqueScalingFactor and
//calcActivation functions
double jointTorque =
tq.calcJointTorque(M_PI/3.0, M_PI/5.0, 0.5);
TorqueMuscleSummary tms;
tq.calcActivation(M_PI/3.0, M_PI/5.0, jointTorque,tms);
activation = tms.activation;
CHECK(fabs(activation-0.5) < SQRTEPSILON );
double scaling = tq.calcMaximumActiveIsometricTorqueScalingFactor(
M_PI/3.0,M_PI/5.0,0.5, jointTorque*1.1);
CHECK(fabs(1.1-scaling) < SQRTEPSILON);
}
TEST(calcTorqueMuscleInfoCorrectnessTests){
double jointAngleOffset = 0;
double signOfJointAngle = 1;
double signOfJointTorque = 1;
double signOfJointVelocity = signOfJointTorque;
std::string name("test");
SubjectInformation subjectInfo;
subjectInfo.gender = GenderSet::Female;
subjectInfo.ageGroup = AgeGroupSet::SeniorOver65;
subjectInfo.heightInMeters = 1.732;
subjectInfo.massInKg = 69.0;
Millard2016TorqueMuscle tq =
Millard2016TorqueMuscle(DataSet::Anderson2007,
subjectInfo,
Anderson2007::HipExtension,
jointAngleOffset,
signOfJointAngle,
signOfJointTorque,
name);
tq.setActiveTorqueAngleCurveBlendingVariable(0.);
tq.setPassiveTorqueAngleCurveBlendingVariable(0.);
tq.setTorqueAngularVelocityCurveBlendingVariable(0.);
double jointAngle = 0.;
double jointVelocity = 0.;
double activation = 1.0;
tq.setPassiveTorqueScale(0.5);
double tmp = tq.calcJointTorque(0,0,1.0);
tq.setPassiveTorqueScale(1.0);
tmp = tq.calcJointTorque(0,0,1.0);
double tauMax = tq.getMaximumActiveIsometricTorque();
double jointAngleAtTauMax = tq.getJointAngleAtMaximumActiveIsometricTorque();
TorqueMuscleInfo tmi;
tq.calcTorqueMuscleInfo(jointAngleAtTauMax,
0.,
activation,
tmi);
//Keypoint check: active force components + fiber kinematics
CHECK(fabs(tmi.activation-1) < EPSILON);
CHECK(fabs(tmi.jointAngle-jointAngleAtTauMax)< TOL);
CHECK(fabs(tmi.jointAngularVelocity-0.) < TOL);
CHECK(fabs(tmi.activation-1) < EPSILON);
//CHECK(fabs(tmi.fiberAngle-thetaAtTauMax) < TOL);
CHECK(fabs(tmi.fiberAngularVelocity-0.) < TOL);
CHECK(fabs(tmi.fiberActiveTorque - tauMax) < TOL);
CHECK(fabs(tmi.fiberActiveTorqueAngleMultiplier-1.0) < TOL);
CHECK(fabs(tmi.fiberTorqueAngularVelocityMultiplier-1.0)<TOL);
//Total force check
double torque = tq.calcJointTorque(jointAngleAtTauMax,0,activation);
double err = fabs(torque
- signOfJointTorque*(
tmi.fiberActiveTorque+tmi.fiberPassiveTorque));
CHECK(fabs(torque
- signOfJointTorque*(
tmi.fiberActiveTorque+tmi.fiberPassiveTorque)) < TOL);
//Total active force scales with activation
tq.calcTorqueMuscleInfo( jointAngleAtTauMax,
0.,
activation*0.5,
tmi);
CHECK(fabs(tmi.fiberActiveTorque - tauMax*0.5) < TOL);
//Keypoint check - power
CHECK(fabs(tmi.jointPower-tmi.fiberPower) < TOL);
//Numerically check the active and passive fiber stiffnesses
double h = sqrt(EPSILON);
tq.calcTorqueMuscleInfo(jointAngleAtTauMax*0.75,
0.,
activation,
tmi);
TorqueMuscleInfo tmiL;
TorqueMuscleInfo tmiR;
tq.calcTorqueMuscleInfo(jointAngleAtTauMax*0.75-h,
0.,
activation,
tmiL);
tq.calcTorqueMuscleInfo(jointAngleAtTauMax*0.75+h,
0.,
activation,
tmiR);
double jointK = (tmiR.jointTorque-tmiL.jointTorque)/(2*h);
err = tmi.jointStiffness - jointK;
CHECK(fabs(tmi.jointStiffness-jointK) < 1e-5);
double fiberK = signOfJointAngle*(tmiR.fiberTorque-tmiL.fiberTorque)/(2*h);
err = tmi.fiberStiffness - fiberK;
CHECK(fabs(tmi.fiberStiffness - fiberK) < 1e-5);
tq.setPassiveTorqueScale(1.5);
tmp = tq.calcJointTorque(0,0,1.0);
CHECK(fabs(tq.getPassiveTorqueScale()-1.5)<TOL);
tq.setPassiveTorqueScale(1.0);
tmp = tq.calcJointTorque(0,0,1.0);
TorqueMuscleInfo tmi0;
TorqueMuscleInfo tmi1;
TorqueMuscleInfo tmi2;
double jointAngleAtPassiveTauMax =
tq.getJointAngleAtOneNormalizedPassiveIsometricTorque();
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.,
activation,
tmi0);
tq.setPassiveTorqueScale(2.0);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.,
activation,
tmi1);
CHECK(fabs(tmi0.fiberPassiveTorqueAngleMultiplier -
0.5*tmi1.fiberPassiveTorqueAngleMultiplier) < TOL);
CHECK(fabs(tmi0.fiberPassiveTorque -
0.5*tmi1.fiberPassiveTorque) < TOL);
double jtq = tq.calcJointTorque(jointAngleAtPassiveTauMax,
0.,
activation);
err = jtq-tmi1.jointTorque;
CHECK(fabs(jtq-tmi1.jointTorque) < TOL );
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.,
activation-SQRTEPSILON,
tmi0);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.,
activation,
tmi1);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.,
activation+SQRTEPSILON,
tmi2);
double DtqDa = tmi1.DjointTorque_Dactivation;
double DtqDa_NUM = (tmi2.jointTorque-tmi0.jointTorque)/(2*SQRTEPSILON);
err = fabs(DtqDa-DtqDa_NUM);
CHECK(fabs(DtqDa-DtqDa_NUM) < fabs(DtqDa)*1e-5 );
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax-SQRTEPSILON,
0.,
activation,
tmi0);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax+SQRTEPSILON,
0.,
activation,
tmi2);
double DtqDq = tmi1.DjointTorque_DjointAngle;
double DtqDq_NUM = (tmi2.jointTorque-tmi0.jointTorque)/(2*SQRTEPSILON);
err = fabs(DtqDq-DtqDq_NUM);
CHECK(fabs(DtqDq-DtqDq_NUM) < fabs(DtqDq)*1e-5 );
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.-SQRTEPSILON,
activation,
tmi0);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.+SQRTEPSILON,
activation,
tmi2);
double DtqDqdot = tmi1.DjointTorque_DjointAngularVelocity;
double DtqDqdot_NUM = (tmi2.jointTorque-tmi0.jointTorque)/(2*SQRTEPSILON);
err = fabs(DtqDqdot-DtqDqdot_NUM);
CHECK(fabs(DtqDqdot-DtqDqdot_NUM) < fabs(DtqDqdot)*1e-5 );
}
TEST(calcTorqueMuscleInfoFittingVariableCorrectnessTests){
double jointAngleOffset = 0;
double signOfJointAngle = 1;
double signOfJointTorque = 1;
double signOfJointVelocity = signOfJointTorque;
std::string name("test");
SubjectInformation subjectInfo;
subjectInfo.gender = GenderSet::Male;
subjectInfo.ageGroup = AgeGroupSet::Young18To25;
subjectInfo.heightInMeters = 1.732;
subjectInfo.massInKg = 69.0;
Millard2016TorqueMuscle tq =
Millard2016TorqueMuscle(DataSet::Gymnast,
subjectInfo,
Gymnast::HipExtension,
jointAngleOffset,
signOfJointAngle,
signOfJointTorque,
name);
TorqueMuscleInfo tmi;
double tvAtHalfOmegaMax =
tq.getTorqueVelocityMultiplierAtHalfOmegaMax();
double omegaMax =
tq.getMaximumConcentricJointAngularVelocity();
double angleAtTaMax =
tq.getJointAngleAtMaximumActiveIsometricTorque();
tq.calcTorqueMuscleInfo(angleAtTaMax,omegaMax*0.5,1.0,tmi);
//We can only get within 0.001 of the tvAtHalfOmega target because
//this itself is a fitting problem to match Hill's hyperbola as closely
//as possible while passing through this target point.
CHECK(fabs(tmi.fiberTorqueAngularVelocityMultiplier
-tvAtHalfOmegaMax)<0.005);
tvAtHalfOmegaMax = 0.4;
tq.setTorqueVelocityMultiplierAtHalfOmegaMax(tvAtHalfOmegaMax);
tq.calcTorqueMuscleInfo(angleAtTaMax,omegaMax*0.5,1.0,tmi);
CHECK(fabs(tmi.fiberTorqueAngularVelocityMultiplier
-tvAtHalfOmegaMax)<0.005);
tq.setActiveTorqueAngleCurveBlendingVariable(0.10);
tq.setPassiveTorqueAngleCurveBlendingVariable(0.20);
tq.setTorqueAngularVelocityCurveBlendingVariable(0.30);
CHECK(fabs(tq.getActiveTorqueAngleCurveBlendingVariable()-0.1)
< SQRTEPSILON);
CHECK(fabs(tq.getPassiveTorqueAngleCurveBlendingVariable()-0.2)
< SQRTEPSILON);
CHECK(fabs(tq.getTorqueAngularVelocityCurveBlendingVariable()-0.3)
< SQRTEPSILON);
//Get reference kinematic and torque quantities for later tests.
double jointAngleTaTiso =
tq.getJointAngleAtMaximumActiveIsometricTorque();
//Using the plots of the hip extension curves to get this minimum value.
double jointAngleTaMin = jointAngleTaTiso - M_PI;
double jointAngleTpTiso =
tq.getJointAngleAtOneNormalizedPassiveIsometricTorque();
double jointAngleTpZero =
tq.getJointAngleAtSmallestNormalizedPassiveIsometricTorque();
omegaMax = tq.getMaximumConcentricJointAngularVelocity();
double tiso = tq.getMaximumActiveIsometricTorque();
//Test that the blending coefficients are modifying the correct variables
//and in the correct ways.
tq.setActiveTorqueAngleCurveBlendingVariable(0.0);
tq.setPassiveTorqueAngleCurveBlendingVariable(0.0);
tq.setTorqueAngularVelocityCurveBlendingVariable(0.0);
double taLambda = 0.1;
double tvLambda = 0.2;
double tpLambda = 0.3;
//Check the values of the blendable torque-angle-curve
tq.setActiveTorqueAngleCurveBlendingVariable(0.0);
tq.calcTorqueMuscleInfo(jointAngleTaMin,0,1.0,tmi);
double multiplier0 = tmi.fiberActiveTorqueAngleMultiplier;
tq.setActiveTorqueAngleCurveBlendingVariable(taLambda);
tq.calcTorqueMuscleInfo(jointAngleTaMin,0,1.0,tmi);
double multiplier1 = tmi.fiberActiveTorqueAngleMultiplier;
double err = fabs( (taLambda+(1-taLambda)*multiplier0)-multiplier1);
CHECK( err < SQRTEPSILON);
tq.setActiveTorqueAngleCurveBlendingVariable(0.0);
tq.calcTorqueMuscleInfo(jointAngleTaTiso,0,1.0,tmi);
multiplier0 = tmi.fiberActiveTorqueAngleMultiplier;
tq.setActiveTorqueAngleCurveBlendingVariable(taLambda);
tq.calcTorqueMuscleInfo(jointAngleTaTiso,0,1.0,tmi);
multiplier1 = tmi.fiberActiveTorqueAngleMultiplier;
err = fabs(multiplier0-multiplier1);
CHECK( err < SQRTEPSILON);
//Check the values of the blendable torque-angular-velocity-curve
tq.setTorqueAngularVelocityCurveBlendingVariable(0.0);
tq.calcTorqueMuscleInfo(jointAngleTaTiso,-omegaMax,1.0,tmi);
double tvLambdaMax = tmi.fiberTorqueAngularVelocityMultiplier;
tq.calcTorqueMuscleInfo(jointAngleTaTiso,omegaMax,1.0,tmi);
multiplier0 = tmi.fiberTorqueAngularVelocityMultiplier;
tq.setTorqueAngularVelocityCurveBlendingVariable(tvLambda);
tq.calcTorqueMuscleInfo(jointAngleTaTiso,omegaMax, 1.0,tmi);
multiplier1 = tmi.fiberTorqueAngularVelocityMultiplier;
err = fabs( (tvLambdaMax*tvLambda+(1-tvLambda)*multiplier0)-multiplier1);
CHECK( err < SQRTEPSILON);
tq.setTorqueAngularVelocityCurveBlendingVariable(0.0);
tq.calcTorqueMuscleInfo(jointAngleTaTiso, -omegaMax, 1.0, tmi);
multiplier0 = tmi.fiberTorqueAngularVelocityMultiplier;
tq.setTorqueAngularVelocityCurveBlendingVariable(tvLambda);
tq.calcTorqueMuscleInfo(jointAngleTaTiso, -omegaMax, 1.0, tmi);
multiplier1 = tmi.fiberTorqueAngularVelocityMultiplier;
err = fabs(multiplier0-multiplier1);
CHECK( err < SQRTEPSILON);
//Check the values of the blendable passive-torque-angle curve
tq.setPassiveTorqueAngleCurveBlendingVariable(0.0);
tq.calcTorqueMuscleInfo(jointAngleTpTiso,0,0.0,tmi);
multiplier0 = tmi.fiberPassiveTorqueAngleMultiplier;
tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda);
tq.calcTorqueMuscleInfo(jointAngleTpTiso,0,0.0,tmi);
multiplier1 = tmi.fiberPassiveTorqueAngleMultiplier;
err = fabs(((1-tpLambda)*multiplier0)-multiplier1 );
CHECK( err < SQRTEPSILON);
tq.setPassiveTorqueAngleCurveBlendingVariable(0.0);
tq.calcTorqueMuscleInfo(jointAngleTpZero,0,0.0,tmi);
multiplier0 = tmi.fiberPassiveTorqueAngleMultiplier;
tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda);
tq.calcTorqueMuscleInfo(jointAngleTpZero,0,0.0,tmi);
multiplier1 = tmi.fiberPassiveTorqueAngleMultiplier;
err = fabs(multiplier0-multiplier1 );
CHECK( err < SQRTEPSILON);
//Check the values of the active-torque-angle curve when the scaling is
//changed
tq.setActiveTorqueAngleCurveBlendingVariable(0.0);
tq.setPassiveTorqueAngleCurveBlendingVariable(0.0);
tq.setTorqueAngularVelocityCurveBlendingVariable(0.0);
tq.setActiveTorqueAngleCurveAngleScaling(1.0);
//Values at the peak should not change.
tq.setActiveTorqueAngleCurveAngleScaling(1.0);
double jointAngleAtOneNormTorque =
tq.getJointAngleAtMaximumActiveIsometricTorque();
tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque,0.,1.0,tmi);
double ta1 = tmi.fiberActiveTorqueAngleMultiplier;
tq.setActiveTorqueAngleCurveAngleScaling(2.0);
tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque,0.,1.0,tmi);
double ta2 = tmi.fiberActiveTorqueAngleMultiplier;
err = fabs(ta1-ta2);
CHECK(err < SQRTEPSILON);
//Values should change in proportion to their distance from the peak
//divided by the scaling factor.
tq.setActiveTorqueAngleCurveAngleScaling(1.0);
tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque+0.5,0.,1.0,tmi);
ta1 = tmi.fiberActiveTorqueAngleMultiplier;
tq.setActiveTorqueAngleCurveAngleScaling(2.0);
tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque+1.0,0.,1.0,tmi);
ta2 = tmi.fiberActiveTorqueAngleMultiplier;
err = fabs(ta1-ta2);
CHECK(err < SQRTEPSILON);
//Check the values of the torque-velocity curve when omega max is
//changed.
tq.setActiveTorqueAngleCurveAngleScaling(1.0);
omegaMax = tq.getMaximumConcentricJointAngularVelocity();
tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque, omegaMax*0.5,1.0,tmi);
double tv1 = tmi.fiberTorqueAngularVelocityMultiplier;
tq.setMaximumConcentricJointAngularVelocity(omegaMax*2.0);
tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque, omegaMax,1.0,tmi);
double tv2 = tmi.fiberTorqueAngularVelocityMultiplier;
err = fabs(tv1-tv2);
CHECK(err < SQRTEPSILON);
TorqueMuscleInfo tmiL,tmiR; //Here we have 3 to do numerical derivatives.
double h = SQRTEPSILON;
tq.setActiveTorqueAngleCurveBlendingVariable(taLambda);
tq.setPassiveTorqueAngleCurveBlendingVariable(tvLambda);
tq.setTorqueAngularVelocityCurveBlendingVariable(tpLambda);
//Get reference points where the curves have a non-zero value and where
//the lambda parameter makes a difference
double jointAngleTaMid = jointAngleTaTiso-M_PI*0.25;
double jointAngleTpMid = (jointAngleTpTiso + jointAngleTpZero)*0.5;
double omegaMid = omegaMax*0.5;
//Calc. numerical derivative of DjointTorque_DtaLambda
tq.calcTorqueMuscleInfo(jointAngleTaMid,0,1.0,tmi);
tq.setActiveTorqueAngleCurveBlendingVariable(taLambda-h);
tq.calcTorqueMuscleInfo(jointAngleTaMid,0,1.0,tmiL);
tq.setActiveTorqueAngleCurveBlendingVariable(taLambda+h);
tq.calcTorqueMuscleInfo(jointAngleTaMid,0,1.0,tmiR);
tq.setActiveTorqueAngleCurveBlendingVariable(taLambda);
double Dtau_DtaLambda = (tmiR.jointTorque-tmiL.jointTorque)/(2*h);
err = fabs(Dtau_DtaLambda
- tmi.DjointTorque_DactiveTorqueAngleBlendingVariable);
CHECK( err < SQRTEPSILON*100.0 );
//Calc. numerical derivative of DjointTorque_DtvLambda
tq.setTorqueAngularVelocityCurveBlendingVariable(tvLambda);
tq.calcTorqueMuscleInfo(jointAngleTaMid,omegaMid,1.0,tmi);
tq.setTorqueAngularVelocityCurveBlendingVariable(tvLambda-h);
tq.calcTorqueMuscleInfo(jointAngleTaMid,omegaMid,1.0,tmiL);
tq.setTorqueAngularVelocityCurveBlendingVariable(tvLambda+h);
tq.calcTorqueMuscleInfo(jointAngleTaMid,omegaMid,1.0,tmiR);
tq.setTorqueAngularVelocityCurveBlendingVariable(tvLambda);
double Dtau_DtvLambda = (tmiR.jointTorque - tmiL.jointTorque)/(2*h);
err = fabs(Dtau_DtvLambda
- tmi.DjointTorque_DtorqueAngularVelocityBlendingVariable);
CHECK( err < SQRTEPSILON*100.0 );
//Calc. numerical derivative of DjointTorque_DtpLambda
tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,0.0,tmi);
tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda-h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,0.0,tmiL);
tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda+h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,0.0,tmiR);
double Dtau_DtpLambda = (tmiR.jointTorque - tmiL.jointTorque)/(2*h);
err = fabs(Dtau_DtpLambda
- tmi.DjointTorque_DpassiveTorqueAngleBlendingVariable);
CHECK( err < SQRTEPSILON*100.0 );
//Calc. numerical derivatives of DjointTorque_DtpOffset
double angleOffset = 0.;
tq.setPassiveCurveAngleOffset(angleOffset);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,0.0,tmi);
tq.setPassiveCurveAngleOffset(angleOffset-h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,0.0,tmiL);
tq.setPassiveCurveAngleOffset(angleOffset+h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,0.0,tmiR);
double Dtp_DtpOffset = (tmiR.fiberPassiveTorqueAngleMultiplier
-tmiL.fiberPassiveTorqueAngleMultiplier)/(2*h);
err = fabs(Dtp_DtpOffset
- tmi.DfiberPassiveTorqueAngleMultiplier_DangleOffset);
CHECK(err < SQRTEPSILON*100.0);
double Dtau_DtpOffset = (tmiR.jointTorque - tmiL.jointTorque)/(2*h);
err = fabs(Dtau_DtpOffset
- tmi.DjointTorque_DpassiveTorqueAngleCurveAngleOffset);
CHECK(err < SQRTEPSILON*100.0);
//Calc. numerical derivative of DjointTorque_DtaAngleScaling
tq.setActiveTorqueAngleCurveBlendingVariable(0.0);
tq.setPassiveTorqueAngleCurveBlendingVariable(0.0);
tq.setTorqueAngularVelocityCurveBlendingVariable(0.0);
tq.setActiveTorqueAngleCurveAngleScaling(1.0);
tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque+1.0,0.0,1.0,tmi);
tq.setActiveTorqueAngleCurveAngleScaling(1.0-h);
tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque+1.0,0.0,1.0,tmiL);
tq.setActiveTorqueAngleCurveAngleScaling(1.0+h);
tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque+1.0,0.0,1.0,tmiR);
double Dtau_DtaScaling = (tmiR.jointTorque-tmiL.jointTorque)/(2*h);
err = fabs(Dtau_DtaScaling
-tmi.DjointTorque_DactiveTorqueAngleAngleScaling);
CHECK(err < SQRTEPSILON*100.0);
//Calc. numerical derivative of DjointTorque_DomegaMax
tq.setActiveTorqueAngleCurveAngleScaling(1.0);
omegaMax = tq.getMaximumConcentricJointAngularVelocity();
tq.setMaximumConcentricJointAngularVelocity(omegaMax);
tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque,omegaMax*0.5,1.0,tmi);
tq.setMaximumConcentricJointAngularVelocity(omegaMax-h);
tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque,omegaMax*0.5,1.0,tmiL);
tq.setMaximumConcentricJointAngularVelocity(omegaMax+h);
tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque,omegaMax*0.5,1.0,tmiR);
double Dtau_DomegaMax = (tmiR.jointTorque-tmiL.jointTorque)/(2*h);
err = fabs( Dtau_DomegaMax
-tmi.DjointTorque_DmaximumAngularVelocity);
CHECK(err < SQRTEPSILON*100.0);
#ifdef RBDL_BUILD_ADDON_MUSCLE_FITTING
//Check the second derivatives.
/*
The lower right triangle of the Hessian of
jointTorque(angle,angularVelocity,activation,x) where x
x = (taLambda,tpLambda,tvLambda,tpOffsetAngle)
is the vector of fitting variables is stored in the vector fittingInfo.
For brevity, the short forms will be used just in the comments below
x = [a, v, p, o]
For reference:
t(a,v,p,o) = mSignOfJointTorque*(
(maxActIsoTorque*(
activation*ta(a)*tv(v)) + (tp(p,o)+tb(p,o))))
Where 't' is the shortform used to represent joint-torque.
The lower right triangle for the Hessian of tau(a,v,p,o) w.r.t. x is
stored, row-wise, in the vector fittingInfo. Thus we have
a v p o
H(tau(a,v,p,o),x) = a [ 0: d2t/da2
v [ 1: d2t/dadv 2: d2t/dv2
p [ 3: d2t/dadp 4: d2t/dvdp 5: d2t/dp2
o [ 6: d2t/dado 7: d2t/dvdo 8: d2t/dpdo 9: d2t/do2
The numbers indicate the index of the quantity in fittingInfo. Proceeding
to numerically check that these quantities are correct.
*/
//Make sure the size of fittingInfo is at least large enough for .
// 10 :Hessian with x = [a,v,p,o] for joint torque related constraints
// 3 :Hessian for constraints on the passive-torque-angle multiplier
// 10 :Hessian with x = [s,m,p,o] for joint torque related constraints
CHECK(tmi.fittingInfo.rows() >= 23);
//For joint-torque related constrains that use x = [a,v,p,o]
//(see the code for the parts of calcTorqueMuscleInfo that evaluate
//the second derivatives for further detail.
//Test the first column of the Hessian
//0: D^2 tau / D lambdaTa^2
tq.setPassiveTorqueAngleCurveBlendingVariable( tpLambda);
tq.setTorqueAngularVelocityCurveBlendingVariable( tvLambda);
tq.setActiveTorqueAngleCurveBlendingVariable( taLambda);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi);
tq.setActiveTorqueAngleCurveBlendingVariable( taLambda-h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL);
tq.setActiveTorqueAngleCurveBlendingVariable( taLambda+h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR);
double D2tau_DtaLambda2 =
(tmiR.DjointTorque_DactiveTorqueAngleBlendingVariable
- tmiL.DjointTorque_DactiveTorqueAngleBlendingVariable)/(2.0*h);
err = fabs(D2tau_DtaLambda2
- tmi.fittingInfo[0]);
CHECK(err < SQRTEPSILON*100);
//1: Test D^2 tau / D TaLambda D TvLambda
double D2tau_DtaLambdaDtvLambda =
(tmiR.DjointTorque_DtorqueAngularVelocityBlendingVariable
- tmiL.DjointTorque_DtorqueAngularVelocityBlendingVariable)/(2.0*h);
err = fabs(D2tau_DtaLambdaDtvLambda
-tmi.fittingInfo[1]);
CHECK(err < SQRTEPSILON*100);
//3: Test D^2 tau/ D TaLambda D TpLambda
double D2tau_DtaLambdaDtpLambda =
(tmiR.DjointTorque_DpassiveTorqueAngleBlendingVariable
-tmiL.DjointTorque_DpassiveTorqueAngleBlendingVariable)/(2.0*h);
err = fabs(D2tau_DtaLambdaDtpLambda
-tmi.fittingInfo[3]);
CHECK(err < SQRTEPSILON*100);
//6: Test D^2 tau/ D TaLambda DTpOffset
double D2tau_DtaLambdaDtpOffset =
(tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset
-tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h);
err = fabs(D2tau_DtaLambdaDtpOffset -
tmi.fittingInfo[6]);
//Check the 2nd column of the Hessian
//2: Test D^2 tau/ D Tvlambda^2
tq.setActiveTorqueAngleCurveBlendingVariable( taLambda);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi);
tq.setTorqueAngularVelocityCurveBlendingVariable( tvLambda - h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL);
tq.setTorqueAngularVelocityCurveBlendingVariable( tvLambda + h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR);
double D2tau_DtvLambda2 =
(tmiR.DjointTorque_DtorqueAngularVelocityBlendingVariable
- tmiL.DjointTorque_DtorqueAngularVelocityBlendingVariable)/(2.0*h);
err = fabs(D2tau_DtvLambda2
- tmi.fittingInfo[2]);
CHECK(err < SQRTEPSILON*100);
//4: Test D^2 tau/ D TvLambda D Tplambda
double D2tau_DtvLambdaDtpLambda =
(tmiR.DjointTorque_DpassiveTorqueAngleBlendingVariable
- tmiL.DjointTorque_DpassiveTorqueAngleBlendingVariable)/(2.0*h);
err = fabs(D2tau_DtvLambdaDtpLambda
- tmi.fittingInfo[4]);
CHECK(err < SQRTEPSILON*100);
//7: Test D^2 tau/ D TvLambda D TpOffset
double D2tau_DtvLambdaDtpOffset =
(tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset
- tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h);
err = fabs(D2tau_DtvLambdaDtpOffset
- tmi.fittingInfo[7]);
CHECK(err < SQRTEPSILON*100);
//Check the 3rd column of the Hessian.
//5: D^2 tau / D TpLambda^2
tq.setTorqueAngularVelocityCurveBlendingVariable(tvLambda);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi);
tq.setPassiveTorqueAngleCurveBlendingVariable( tpLambda - h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL);
tq.setPassiveTorqueAngleCurveBlendingVariable( tpLambda + h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR);
double D2tau_DtpLambda2 =
(tmiR.DjointTorque_DpassiveTorqueAngleBlendingVariable
- tmiL.DjointTorque_DpassiveTorqueAngleBlendingVariable)/(2.0*h);
err = fabs(D2tau_DtpLambda2
- tmi.fittingInfo[5]);
CHECK(err < SQRTEPSILON*100);
//8: D^2 tau/ D TpLambda DTpOffset
double D2tau_DtpLambdaDtpOffset =
(tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset
- tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h);
err = fabs(D2tau_DtpLambdaDtpOffset
- tmi.fittingInfo[8]);
CHECK(err < SQRTEPSILON*100);
//Check the 4th column of the Hessian.
//9: D^2 tau/ D TpOffset^2
tq.setTorqueAngularVelocityCurveBlendingVariable(tvLambda);
tq.setPassiveCurveAngleOffset(0.);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi);
tq.setPassiveCurveAngleOffset(-h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL);
tq.setPassiveCurveAngleOffset( h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR);
double D2tau_DtpOffset2 =
(tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset
- tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h);
err = fabs(D2tau_DtpOffset2
- tmi.fittingInfo[9]);
CHECK(err < SQRTEPSILON*1000);
// For constraints on the value of the passive element
// 10: d2p/dp2
tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi);
tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda-h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL);
tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda+h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR);
double D2tp_DtpLambda2 =
(tmiR.DfiberPassiveTorqueAngleMultiplier_DblendingVariable
- tmiL.DfiberPassiveTorqueAngleMultiplier_DblendingVariable)/(2.0*h);
err = fabs(D2tp_DtpLambda2
- tmi.fittingInfo[10]);
CHECK(err < SQRTEPSILON*1000);
// 11: d2p/dpdo
double D2tp_DtpLambda_DangleOffset =
(tmiR.DfiberPassiveTorqueAngleMultiplier_DangleOffset
-tmiL.DfiberPassiveTorqueAngleMultiplier_DangleOffset)/(2.0*h);
err = fabs(D2tp_DtpLambda_DangleOffset
-tmi.fittingInfo[11]);
CHECK(err < SQRTEPSILON*1000);
// 12: d2p/do2
tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda);
tq.setPassiveCurveAngleOffset(0.);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi);
tq.setPassiveCurveAngleOffset(-h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL);
tq.setPassiveCurveAngleOffset( h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR);
double D2tp_DtpOffset2 =
(tmiR.DfiberPassiveTorqueAngleMultiplier_DangleOffset
- tmiL.DfiberPassiveTorqueAngleMultiplier_DangleOffset)/(2.0*h);
err = fabs(D2tp_DtpOffset2
- tmi.fittingInfo[12]);
CHECK(err < SQRTEPSILON*1000);
// For joint-torque related constraints that use x = [s,m,p,o]
// 13: d2t/ds2
tq.setPassiveTorqueAngleCurveBlendingVariable( 0.);
tq.setTorqueAngularVelocityCurveBlendingVariable( 0.);
tq.setActiveTorqueAngleCurveBlendingVariable( 0.);
jointAngleAtOneNormTorque = tq.getJointAngleAtMaximumActiveIsometricTorque();
tq.setActiveTorqueAngleCurveAngleScaling(1.0);
tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque+0.5,omegaMid,1.0,tmi);
tq.setActiveTorqueAngleCurveAngleScaling(1.0-h);
tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque+0.5,omegaMid,1.0,tmiL);
tq.setActiveTorqueAngleCurveAngleScaling(1.0+h);
tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque+0.5,omegaMid,1.0,tmiR);
double D2tau_DtaAngleScaling2 =
(tmiR.DjointTorque_DactiveTorqueAngleAngleScaling
- tmiL.DjointTorque_DactiveTorqueAngleAngleScaling)/(2.0*h);
err = fabs(D2tau_DtaAngleScaling2
- tmi.fittingInfo[13]);
CHECK(err < SQRTEPSILON*1000);
// 14: d2t/dsdm
double D2tau_DtaAngleScaling_DomegaMax =
(tmiR.DjointTorque_DmaximumAngularVelocity
- tmiL.DjointTorque_DmaximumAngularVelocity)/(2.0*h);
err = fabs(D2tau_DtaAngleScaling_DomegaMax
- tmi.fittingInfo[14]);
CHECK(err < SQRTEPSILON*100);
// 16: d2t/dsdp
double D2tau_DtaAngleScaling_DtpLambda =
(tmiR.DjointTorque_DpassiveTorqueAngleBlendingVariable
-tmiL.DjointTorque_DpassiveTorqueAngleBlendingVariable)/(2.0*h);
err = fabs(D2tau_DtaAngleScaling_DtpLambda
-tmi.fittingInfo[16]);
CHECK(err < SQRTEPSILON*100);
// 19: d2t/dsdo
double D2tau_DtaAngleScaling_DtpOffset =
(tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset
-tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h);
err = fabs(D2tau_DtaAngleScaling_DtpOffset
-tmi.fittingInfo[19]);
CHECK(err < SQRTEPSILON*100);
// 15: d2t/dm2
tq.setActiveTorqueAngleCurveAngleScaling(1.0);
omegaMax = tq.getMaximumConcentricJointAngularVelocity();
omegaMid = 0.5*omegaMax;
tq.setMaximumConcentricJointAngularVelocity(omegaMax);
tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque,omegaMid,1.0,tmi);
tq.setMaximumConcentricJointAngularVelocity(omegaMax-h);
tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque,omegaMid,1.0,tmiL);
tq.setMaximumConcentricJointAngularVelocity(omegaMax+h);
tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque,omegaMid,1.0,tmiR);
double D2tau_DomegaMax2 =
(tmiR.DjointTorque_DmaximumAngularVelocity
- tmiL.DjointTorque_DmaximumAngularVelocity)/(2.0*h);
err = fabs(D2tau_DomegaMax2
- tmi.fittingInfo[15]);
CHECK(err < SQRTEPSILON*100);
// 17: d2t/dmdp
double D2tau_DomegaMax_DtpLambda =
(tmiR.DjointTorque_DpassiveTorqueAngleBlendingVariable
-tmiL.DjointTorque_DpassiveTorqueAngleBlendingVariable)/(2.0*h);
err = fabs(D2tau_DomegaMax_DtpLambda
- tmi.fittingInfo[17]);
CHECK(err < SQRTEPSILON*100);
// 20: d2t/dmdo
double D2tau_DomegaMax_DtpOffset =
(tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset
-tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h);
err = fabs(D2tau_DomegaMax_DtpOffset
- tmi.fittingInfo[20]);
CHECK(err < SQRTEPSILON*100);
// 18: d2t/dp2
tq.setActiveTorqueAngleCurveBlendingVariable(taLambda);
tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda);
tq.setTorqueAngularVelocityCurveBlendingVariable(tvLambda);
tq.setPassiveCurveAngleOffset(0.0);
tq.setActiveTorqueAngleCurveAngleScaling(1.0);
tq.setMaximumConcentricJointAngularVelocity(omegaMax);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi);
tq.setPassiveTorqueAngleCurveBlendingVariable( tpLambda - h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL);
tq.setPassiveTorqueAngleCurveBlendingVariable( tpLambda + h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR);
D2tau_DtpLambda2 =
(tmiR.DjointTorque_DpassiveTorqueAngleBlendingVariable
- tmiL.DjointTorque_DpassiveTorqueAngleBlendingVariable)/(2.0*h);
err = fabs(D2tau_DtpLambda2
- tmi.fittingInfo[18]);
CHECK(err < SQRTEPSILON*100);
// 21: d2t/dpdo
D2tau_DtpLambdaDtpOffset =
(tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset
- tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h);
err = fabs(D2tau_DtpLambdaDtpOffset
- tmi.fittingInfo[21]);
CHECK(err < SQRTEPSILON*100);
// 22: d2t/do2
tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda);
tq.setPassiveCurveAngleOffset(0.);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi);
tq.setPassiveCurveAngleOffset(-h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL);
tq.setPassiveCurveAngleOffset( h);
tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR);
D2tau_DtpOffset2 =
(tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset
- tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h);
err = fabs(D2tau_DtpOffset2
- tmi.fittingInfo[22]);
CHECK(err < SQRTEPSILON*1000);
#endif
}
#ifdef RBDL_BUILD_ADDON_MUSCLE_FITTING
TEST(fittingEasyTest){
double jointAngleOffset = 0;
double signOfJointAngle = 1;
double signOfJointTorque = 1;
double signOfJointVelocity = signOfJointTorque;
std::string name("test");
SubjectInformation subjectInfo;
subjectInfo.gender = GenderSet::Male;
subjectInfo.ageGroup = AgeGroupSet::Young18To25;
subjectInfo.heightInMeters = 1.732;
subjectInfo.massInKg = 69.0;
Millard2016TorqueMuscle tq =
Millard2016TorqueMuscle(DataSet::Gymnast,
subjectInfo,
Gymnast::HipExtension,
jointAngleOffset,
signOfJointAngle,
signOfJointTorque,
name);
// Testing approach
// 1. Generate a data set consistent with a muscle that is more flexible,
// is stronger, and has modified curves from the default.
// 2. Return the muscle back to its default values of flexibility, maximum
// isometric torque and blending variables.
// 3. Run the fitting algorithm to see if the parameters from step 1 can be
// identified.
double tisoOriginal = tq.getMaximumActiveIsometricTorque();
double tisoUpd = 1.2*tisoOriginal;
double omegaMaxOrig = tq.getMaximumConcentricJointAngularVelocity();
double omegaMaxUpd = 2.5*omegaMaxOrig;
double taAngleScalingOrig = tq.getActiveTorqueAngleCurveAngleScaling();
double taAngleScalingUpd = taAngleScalingOrig*1.35;
double taLambda = 0.0;
double tpLambda = 0.0;
double tvLambda = 0.0;
double angleToOneOrig =
tq.getJointAngleAtOneNormalizedPassiveIsometricTorque();
tq.setActiveTorqueAngleCurveBlendingVariable(taLambda);
tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda);
tq.setTorqueAngularVelocityCurveBlendingVariable(tvLambda);
tq.setMaximumActiveIsometricTorque(tisoUpd);
tq.setMaximumConcentricJointAngularVelocity(omegaMaxUpd);
tq.setActiveTorqueAngleCurveAngleScaling(taAngleScalingUpd);
unsigned int npts = 100;
RigidBodyDynamics::Math::VectorNd angle, angularVelocity, torque;
angle.resize(npts);
angularVelocity.resize(npts);
torque.resize(npts);
//Generate the data set.
double activationUpperBound = 0.95;
double passiveTorqueAngleMultiplierUpperBound = 0.;
TorqueMuscleInfo tmi;
TorqueMuscleSummary tms;
double angularRange = M_PI/3.0;
double angleTpOne =
tq.getJointAngleAtOneNormalizedPassiveIsometricTorque();
double angularOffset = angleTpOne-angularRange;
double time = 0.;
double normTime = 0.;
double omega = omegaMaxUpd/angularRange;
for(unsigned int i = 0; i<npts;++i){
normTime = (double)(i)/ (double)(npts-1);
time = normTime*2.0*M_PI/omega;
angle[i] = angularOffset + angularRange*sin(omega*time);
angularVelocity[i] = angularRange*omega*cos(omega*time);
tq.calcTorqueMuscleInfo(angle[i],angularVelocity[i],
activationUpperBound, tmi);
torque[i] = tmi.jointTorque;
if(tmi.fiberPassiveTorqueAngleMultiplier
> passiveTorqueAngleMultiplierUpperBound){
passiveTorqueAngleMultiplierUpperBound =
tmi.fiberPassiveTorqueAngleMultiplier;
}
}
//Return the muscle back to its defaults.
tq.setActiveTorqueAngleCurveBlendingVariable(0.0);
tq.setPassiveTorqueAngleCurveBlendingVariable(0.0);
tq.setTorqueAngularVelocityCurveBlendingVariable(0.0);
tq.setMaximumActiveIsometricTorque(tisoOriginal);
tq.setMaximumConcentricJointAngularVelocity(omegaMaxOrig);
tq.setActiveTorqueAngleCurveAngleScaling(taAngleScalingOrig);
//Run the fitting routine.
passiveTorqueAngleMultiplierUpperBound *= 0.75;
TorqueMuscleParameterFittingData tmFittingData;
TorqueMuscleFittingToolkit::fitTorqueMuscleParameters(
tq, angle,angularVelocity,torque,
activationUpperBound,
passiveTorqueAngleMultiplierUpperBound,
tmFittingData, false);
//Now to test the result, update the parameters of the
//model and run through all of the test vectors. The muscle
//should produce a torque that is >= the desired torque
//of the same sign. There should be one value where it
//,to numerical precision produces just the required torque
//and no more.
CHECK(tmFittingData.fittingConverged == true);
tq.setFittedParameters(tmFittingData);
double minActivation = 1.e20;
double maxActivation = -1.e20;
double maxPassiveTorqueAngleMultiplier = -1.0e20;
for(unsigned int i=0; i<angle.rows();++i){
if(torque[i]*tq.getJointTorqueSign() > 0){
tq.calcActivation(angle[i],angularVelocity[i],torque[i],tms);
if(tms.activation < minActivation){
minActivation = tms.activation;
}
if(tms.activation > maxActivation){
maxActivation = tms.activation;
}
if(tms.fiberPassiveTorqueAngleMultiplier
> maxPassiveTorqueAngleMultiplier){
maxPassiveTorqueAngleMultiplier = tms.fiberPassiveTorqueAngleMultiplier;
}
}
}
double err = maxActivation-activationUpperBound;
CHECK(err <= 10.0*SQRTEPSILON);
err = minActivation;
CHECK(err >= -10.0*SQRTEPSILON);
err = (maxPassiveTorqueAngleMultiplier
- passiveTorqueAngleMultiplierUpperBound);
CHECK(err < SQRTEPSILON);
}
TEST(fittingHardTest)
{
//std::vector< std::vector< double > > data;
//std::string fileName("TorqueMuscleFittingToolkitHardTestCase_TimeQQDotTau.csv");
//data = readMatrixFromFile(fileName, 0);
std::string name("hardTest");
SubjectInformation subjectInfo;
subjectInfo.gender = GenderSet::Male;
subjectInfo.ageGroup = AgeGroupSet::Young18To25;
subjectInfo.heightInMeters = 1.732;
subjectInfo.massInKg = 69.0;
double angleOffset = 0.;
double angleSign = 1;
double torqueSign =-1;
Millard2016TorqueMuscle kneeExt(DataSet::Gymnast,
subjectInfo,
Gymnast::KneeExtension,
angleOffset,
angleSign,
torqueSign,
name);
RigidBodyDynamics::Math::VectorNd q, qDot, tau;
q.resize( TorqueMuscleFittingHardTestCaseRows);
qDot.resize(TorqueMuscleFittingHardTestCaseRows);
tau.resize( TorqueMuscleFittingHardTestCaseRows);
double tauMax = 0;
double omegaMax = 0;
for(unsigned int i=0; i<TorqueMuscleFittingHardTestCaseRows;++i){
q[i] = TorqueMuscleFittingHardTestCase[i][1];
qDot[i] = TorqueMuscleFittingHardTestCase[i][2];
tau[i] = TorqueMuscleFittingHardTestCase[i][3];
if(fabs(qDot[i])>omegaMax){
omegaMax = fabs(qDot[i]);
}
if(fabs(tau[i])>tauMax){
tauMax = fabs(tau[i]);
}
}
double activationUB = 0.9;
double tpUB = 0.75;
bool verbose = false;
TorqueMuscleParameterFittingData fittingData;
TorqueMuscleFittingToolkit::fitTorqueMuscleParameters(kneeExt,q,qDot,tau,
activationUB,tpUB,
fittingData,verbose);
CHECK(fittingData.fittingConverged == true);
}
#endif
TEST(exampleUsage){
bool printCurves = false;
bool printAllCurves = false;
//int dataSet = DataSetAnderson2007;
//int gender = 0; //male
//int age = 0; //young
double jointAngleOffset = 0;
double signOfJointAngle = 1;
double signOfJointTorque = 1;
std::string name("test");
SubjectInformation subjectInfo;
subjectInfo.gender = GenderSet::Male;
subjectInfo.ageGroup = AgeGroupSet::Young18To25;
subjectInfo.heightInMeters = 1.732;
subjectInfo.massInKg = 69.0;
std::vector< Millard2016TorqueMuscle > muscleVector;
bool exception = false;
double angleTorqueSigns[][2] = {{-1, 1},
{-1,-1},
{ 1,-1},
{ 1, 1},
{-1, 1},
{-1,-1}};
Millard2016TorqueMuscle tqMuscle;
std::stringstream tqName;
int tqIdx;
for(int i=0; i < Anderson2007::LastJointTorque; ++i){
tqName.str("");
tqName << DataSet.names[0]
<<Anderson2007::JointTorqueNames[i];
tqMuscle = Millard2016TorqueMuscle(
DataSet::Anderson2007,
subjectInfo,
Anderson2007::JointTorque(i),
0.0,
1.0,
1.0,
tqName.str() );
if(printCurves)
tqMuscle.printJointTorqueProfileToFile("",tqMuscle.getName() ,100);
}
for(int i=0; i < Gymnast::LastJointTorque; ++i){
tqName.str("");
tqName << DataSet.names[1]
<< Gymnast::JointTorqueNames[i];
tqMuscle = Millard2016TorqueMuscle(
DataSet::Gymnast,
subjectInfo,
Gymnast::JointTorque(i),
0.0,
1.0,
1.0,
tqName.str() );
if(printCurves)
tqMuscle.printJointTorqueProfileToFile("",tqMuscle.getName() ,100);
}
tqIdx = -1;
if(printAllCurves){
std::stringstream muscleName;
Millard2016TorqueMuscle muscle;
int genderIdx,ageIdx,tqIdx;
for(int age =0; age < Anderson2007::LastAgeGroup; ++age){
for(int gender=0; gender < Anderson2007::LastGender; ++gender){
for( int tqDir = 0; tqDir < Anderson2007::LastJointTorque; ++tqDir){
//for(int joint=0; joint < 3; ++joint){
// for(int dir = 0; dir < 2; ++dir){
muscleName.str(std::string());
genderIdx = Anderson2007::Gender(gender);
ageIdx = Anderson2007::AgeGroup(age);
tqIdx = Anderson2007::JointTorque(tqDir);
muscleName << "Anderson2007_"
<< AgeGroupSet::names[ageIdx]
<< "_"
<< GenderSet::names[genderIdx]
<< "_"
<< JointTorqueSet::names[tqIdx];
subjectInfo.ageGroup = AgeGroupSet::item(age);
subjectInfo.gender = GenderSet::item(gender);
muscle = Millard2016TorqueMuscle(
DataSet::Anderson2007,
subjectInfo,
Anderson2007::JointTorque(tqDir),
0,
1.0,
1.0,
muscleName.str());
const SmoothSegmentedFunction &tp
= muscle.getPassiveTorqueAngleCurve();
const SmoothSegmentedFunction &ta
= muscle.getActiveTorqueAngleCurve();
const SmoothSegmentedFunction &tv
= muscle.getTorqueAngularVelocityCurve();
RigidBodyDynamics::Math::VectorNd tpDomain
= tp.getCurveDomain();
RigidBodyDynamics::Math::VectorNd tvDomain
= tv.getCurveDomain();
RigidBodyDynamics::Math::VectorNd taDomain
= ta.getCurveDomain();
tp.printCurveToCSVFile(
"",
tp.getName(),
tpDomain[0]-0.1*(tpDomain[1]-tpDomain[0]),
tpDomain[1]+0.1*(tpDomain[1]-tpDomain[0]));
tv.printCurveToCSVFile(
"",
tv.getName(),
tvDomain[0]-0.1*(tvDomain[1]-tvDomain[0]),
tvDomain[1]+0.1*(tvDomain[1]-tvDomain[0]));
ta.printCurveToCSVFile(
"",
ta.getName(),
taDomain[0]-0.1*(taDomain[1]-taDomain[0]),
taDomain[1]+0.1*(taDomain[1]-taDomain[0]));
//}
//}
}
}
}
}
//catch(...){
// exceptionThrown = true;
// }
CHECK(true);
}
int main (int argc, char *argv[])
{
return UnitTest::RunAllTests ();
}