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

726 lines
24 KiB
C++
Raw Permalink Normal View History

Added RBDL to 3rdparty, added empty rig model, used better coding style in CharacterModule
2017-02-22 21:35:14 +01:00
/* *
* TorqueMuscle
* Copyright (c) 2016 Matthew Millard <matthew.millard@iwr.uni-heidelberg.de>
*
* Licensed under the zlib license. See LICENSE for more details.
*/
//==============================================================================
// INCLUDES
//==============================================================================
#include "../Millard2016TorqueMuscle.h"
#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(abs( 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.fiberActiveTorqueAngularVelocityMultiplier);
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.DjointTorqueDjointAngle);
printf("%f\n",tmiG.DjointTorqueDjointAngularVelocity);
printf("%f\n",tmiG.DjointTorqueDactivation);
}
//Zero out the passive forces so that calcMuscleTorque reports
//just the active force - this allows us to test its correctness.
tq.setPassiveTorqueScale(0.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);
CHECK(abs( tq.getMaximumActiveIsometricTorque()-tauMax)
< TOL );
//getParametersC1C2C3C4C5C6() has been removed and so this
//test is no longer possible. It is the responsibility of
//the underlying active-torque-angle curve to ensure that
//it peaks at 1.0
/*
RigidBodyDynamics::Math::VectorNd c1c2c3c4c5c6 =
tq.getParametersC1C2C3C4C5C6();
double thetaAtTauMax = c1c2c3c4c5c6[2];
*/
//TorqueMuscleInfo tqInfo;
//getParametersC1C2C3C4C5C6() has been removed. It is the
//responsibility of the underlying curve test code to
//check for correctness.
//double torque = tq.calcJointTorque(thetaAtTauMax,0.0,1.0);
//err = abs(torque -tauMax);
//CHECK(err< TOL);
//These tests have been updated because Anderson
//torque velocity curve had to be replaced because it
//can behave badly (e.g. the concentric curve of the ankle
//extensors of a young man does not interest the x axis.)
//
//The new curves do not pass exactly through the points
//0.5*tauMax and 0.75*tauMax, but within 5% of this
//value.
//getParametersC1C2C3C4C5C6() has been removed. It is the
//responsibility of the underlying curve test code to
//check for correctness.
//double omegaAt75TauMax = c1c2c3c4c5c6[3];
//torque = tq.calcJointTorque(thetaAtTauMax,omegaAt75TauMax,1.0);
//err = abs(torque - 0.75*tauMax)/tauMax;
//CHECK( err < 0.05);
//double omegaAt50TauMax = c1c2c3c4c5c6[4];
//torque = tq.calcJointTorque(thetaAtTauMax,omegaAt50TauMax,1.0);
//err = abs(torque -0.50*tauMax)/tauMax;
//CHECK(err < 0.05);
}
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);
double jointAngle = 0.;
double jointVelocity = 0.;
double activation = 1.0;
tq.setPassiveTorqueScale(0.5);
tq.calcJointTorque(0,0,1.0);
tq.setPassiveTorqueScale(1.0);
tq.calcJointTorque(0,0,1.0);
double tauMax = tq.getMaximumActiveIsometricTorque();
//RigidBodyDynamics::Math::VectorNd c1c2c3c4c5c6 =
// tq.getParametersC1C2C3C4C5C6();
//RigidBodyDynamics::Math::VectorNd b1k1b2k2 =
// tq.getParametersB1K1B2K2();
/*
int idx = -1;
if(b1k1b2k2[0] > 0){
idx = 0;
}else if(b1k1b2k2[2] > 0){
idx = 2;
}
*/
/*
double b = b1k1b2k2[idx];
double k = b1k1b2k2[idx+1];
double thetaAtPassiveTauMax = log(tauMax/b)/k;
double thetaAtTauMax = c1c2c3c4c5c6[2];
double omegaAt75TauMax = c1c2c3c4c5c6[3];
double omegaAt50TauMax = c1c2c3c4c5c6[4];
double jointAngleAtTauMax =
signOfJointAngle*thetaAtTauMax+jointAngleOffset;
*/
double jointAngleAtTauMax = tq.getJointAngleAtMaximumActiveIsometricTorque();
TorqueMuscleInfo tmi;
tq.calcTorqueMuscleInfo(jointAngleAtTauMax,
0.,
activation,
tmi);
//Keypoint check: active force components + fiber kinematics
CHECK(abs(tmi.activation-1) < EPSILON);
CHECK(abs(tmi.jointAngle-jointAngleAtTauMax)< TOL);
CHECK(abs(tmi.jointAngularVelocity-0.) < TOL);
CHECK(abs(tmi.activation-1) < EPSILON);
//CHECK(abs(tmi.fiberAngle-thetaAtTauMax) < TOL);
CHECK(abs(tmi.fiberAngularVelocity-0.) < TOL);
CHECK(abs(tmi.fiberActiveTorque - tauMax) < TOL);
CHECK(abs(tmi.fiberActiveTorqueAngleMultiplier-1.0) < TOL);
CHECK(abs(tmi.fiberActiveTorqueAngularVelocityMultiplier-1.0)<TOL);
//Total force check
double torque = tq.calcJointTorque(jointAngleAtTauMax,0,activation);
double err = abs(torque
- signOfJointTorque*(
tmi.fiberActiveTorque+tmi.fiberPassiveTorque));
CHECK(abs(torque
- signOfJointTorque*(
tmi.fiberActiveTorque+tmi.fiberPassiveTorque)) < TOL);
//Total active force scales with activation
tq.calcTorqueMuscleInfo( jointAngleAtTauMax,
0.,
activation*0.5,
tmi);
CHECK(abs(tmi.fiberActiveTorque - tauMax*0.5) < TOL);
//Keypoint check - at omega at 50% tau max
//tq.calcTorqueMuscleInfo(jointAngleAtTauMax,
// signOfJointVelocity*omegaAt50TauMax,
// activation,
// tmi);
//CHECK(abs(tmi.jointAngularVelocity
// -signOfJointVelocity*omegaAt50TauMax) < TOL);
//CHECK(abs(tmi.fiberAngularVelocity-omegaAt50TauMax) < TOL);
//This test was updated because Anderson's curve is no longer
//used exactly as it is written in the paper because it
//has very odd behavior: the parameters used for ankle extension
//of the young men is such that the force velocity curve on
//the concentric side never goes to 0.
//CHECK(abs(tmi.fiberActiveTorque - tauMax*0.5) < 0.05*tauMax);
//Keypoint check - power
CHECK(abs(tmi.jointPower-tmi.fiberPower) < TOL);
//CHECK(abs(tmi.jointPower-tmi.fiberTorque*omegaAt50TauMax) < TOL);
//Keypoint check - where passive curve hits tau max.
double jointAngleAtPassiveTauMax =
tq.getJointAngleAtOneNormalizedPassiveIsometricTorque();
//tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
// signOfJointVelocity*omegaAt50TauMax,
// activation,
// tmi);
//CHECK(abs(tmi.fiberPassiveTorqueAngleMultiplier-1) < 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(abs(tmi.jointStiffness-jointK) < 1e-5);
double fiberK = signOfJointAngle*(tmiR.fiberTorque-tmiL.fiberTorque)/(2*h);
err = tmi.fiberStiffness - fiberK;
CHECK(abs(tmi.fiberStiffness - fiberK) < 1e-5);
tq.setPassiveTorqueScale(1.5);
CHECK(abs(tq.getPassiveTorqueScale()-1.5)<TOL);
tq.setPassiveTorqueScale(1.0);
TorqueMuscleInfo tmi0;
TorqueMuscleInfo tmi1;
TorqueMuscleInfo tmi2;
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.,
activation,
tmi0);
tq.setPassiveTorqueScale(2.0);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.,
activation,
tmi1);
CHECK(abs(tmi0.fiberPassiveTorqueAngleMultiplier -
0.5*tmi1.fiberPassiveTorqueAngleMultiplier) < TOL);
CHECK(abs(tmi0.fiberPassiveTorque -
0.5*tmi1.fiberPassiveTorque) < TOL);
double jtq = tq.calcJointTorque(jointAngleAtPassiveTauMax,
0.,
activation);
err = jtq-tmi1.jointTorque;
CHECK(abs(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.DjointTorqueDactivation;
double DtqDa_NUM = (tmi2.jointTorque-tmi0.jointTorque)/(2*SQRTEPSILON);
err = abs(DtqDa-DtqDa_NUM);
CHECK(abs(DtqDa-DtqDa_NUM) < abs(DtqDa)*1e-5 );
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax-SQRTEPSILON,
0.,
activation,
tmi0);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax+SQRTEPSILON,
0.,
activation,
tmi2);
double DtqDq = tmi1.DjointTorqueDjointAngle;
double DtqDq_NUM = (tmi2.jointTorque-tmi0.jointTorque)/(2*SQRTEPSILON);
err = abs(DtqDq-DtqDq_NUM);
CHECK(abs(DtqDq-DtqDq_NUM) < abs(DtqDq)*1e-5 );
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.-SQRTEPSILON,
activation,
tmi0);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.+SQRTEPSILON,
activation,
tmi2);
double DtqDqdot = tmi1.DjointTorqueDjointAngularVelocity;
double DtqDqdot_NUM = (tmi2.jointTorque-tmi0.jointTorque)/(2*SQRTEPSILON);
err = abs(DtqDqdot-DtqDqdot_NUM);
CHECK(abs(DtqDqdot-DtqDqdot_NUM) < abs(DtqDqdot)*1e-5 );
tq.setPassiveTorqueScale(1.0);
tq.setPassiveCurveAngleOffset(0.0);
jointAngleAtPassiveTauMax = tq.getJointAngleAtOneNormalizedPassiveIsometricTorque();
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.-SQRTEPSILON,
activation,
tmi0);
tq.setPassiveCurveAngleOffset(M_PI/3.0);
double updJointAngleAtPassiveTauMax =
tq.getJointAngleAtOneNormalizedPassiveIsometricTorque() ;
CHECK( abs(updJointAngleAtPassiveTauMax-jointAngleAtPassiveTauMax-M_PI/3.0)
< SQRTEPSILON);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax+M_PI/3.0,
0.-SQRTEPSILON,
activation,
tmi1);
CHECK( abs(tmi0.fiberPassiveTorqueAngleMultiplier
-tmi1.fiberPassiveTorqueAngleMultiplier) < SQRTEPSILON);
//fitPassiveCurveAngleOffset: Extension test
tauMax = tq.getMaximumActiveIsometricTorque();
jointAngleAtPassiveTauMax
= tq.getJointAngleAtOneNormalizedPassiveIsometricTorque();
tq.fitPassiveCurveAngleOffset(1.0,
tauMax);
tq.calcTorqueMuscleInfo(1.0,
0.,
0.,
tmi0);
CHECK(abs(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(abs(tmi0.fiberPassiveTorque - tauMax) < SQRTEPSILON);
tauMax = tq.getMaximumActiveIsometricTorque();
jointAngleAtPassiveTauMax
= tq.getJointAngleAtOneNormalizedPassiveIsometricTorque();
tq.fitPassiveTorqueScale(jointAngleAtPassiveTauMax, tauMax*0.5);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.,
0.,
tmi0);
CHECK(abs(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(abs(tmi0.fiberPassiveTorque - tauMax*0.5) < SQRTEPSILON);
//Test the damping term
double beta = tq.getNormalizedDampingCoefficient();
tq.setNormalizedDampingCoefficient(beta+0.1);
CHECK(abs(beta+0.1-tq.getNormalizedDampingCoefficient())<SQRTEPSILON);
double omegaMax = tq.getMaximumJointAngularVelocity();
double tau = tq.calcJointTorque(0., omegaMax,0);
CHECK(abs(tau) < SQRTEPSILON );
tq.calcTorqueMuscleInfo(0.,omegaMax,0.1,tmi0);
err = abs(tmi0.activation
*tmi0.fiberActiveTorqueAngleMultiplier
*tmi0.fiberActiveTorqueAngularVelocityMultiplier
+tmi0.fiberPassiveTorqueAngleMultiplier
*tq.getPassiveTorqueScale()
+tmi0.fiberNormDampingTorque);
CHECK( err < SQRTEPSILON);
beta = tq.getNormalizedDampingCoefficient();
tauMax = tq.getMaximumActiveIsometricTorque();
tq.calcTorqueMuscleInfo(0.,-omegaMax,0,tmi0);
CHECK( abs(tmi0.fiberDampingTorque - beta*1*tauMax) < SQRTEPSILON );
CHECK( abs(tmi0.fiberNormDampingTorque -beta*1) < SQRTEPSILON );
}
TEST(exampleUsage){
bool printCurves = true;
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 ();
}