1720 lines
66 KiB
C++
Executable File
1720 lines
66 KiB
C++
Executable File
/* *
|
|
*
|
|
* 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 ();
|
|
}
|
|
|