1446 lines
52 KiB
C++
1446 lines
52 KiB
C++
//==============================================================================
|
|
/*
|
|
* RBDL - Rigid Body Dynamics Library: Addon : muscle
|
|
* Copyright (c) 2016 Matthew Millard <matthew.millard@iwr.uni-heidelberg.de>
|
|
*
|
|
* Licensed under the zlib license. See LICENSE for more details.
|
|
*/
|
|
|
|
#include "Millard2016TorqueMuscle.h"
|
|
#include "TorqueMuscleFunctionFactory.h"
|
|
#include "csvtools.h"
|
|
|
|
#include <limits>
|
|
|
|
#include <cmath>
|
|
#include <cstdio>
|
|
#include <iostream>
|
|
#include <fstream>
|
|
#include <sstream>
|
|
#include <stdexcept>
|
|
#include <vector>
|
|
|
|
#include <ostream>
|
|
|
|
static double EPSILON = std::numeric_limits<double>::epsilon();
|
|
static double SQRTEPSILON = sqrt(EPSILON);
|
|
|
|
using namespace RigidBodyDynamics::Math;
|
|
using namespace RigidBodyDynamics::Addons::Muscle;
|
|
using namespace RigidBodyDynamics::Addons::Geometry;
|
|
using namespace std;
|
|
|
|
/*************************************************************
|
|
Table Access Structure Names
|
|
*************************************************************/
|
|
const double gravity = 9.81; //Needed for the strength scaling used
|
|
//by Anderson et al.
|
|
|
|
const char* DataSet::names[] = { "Anderson2007",
|
|
"Gymnast"};
|
|
|
|
const char* GenderSet::names[] = {"Male",
|
|
"Female"};
|
|
|
|
const char* AgeGroupSet::names[] = {"Young18To25",
|
|
"Middle55To65",
|
|
"SeniorOver65"};
|
|
|
|
const char* JointTorqueSet::names[] = { "HipExtension" ,
|
|
"HipFlexion" ,
|
|
"KneeExtension" ,
|
|
"KneeFlexion" ,
|
|
"AnkleExtension" ,
|
|
"AnkleFlexion" ,
|
|
"ElbowExtension" ,
|
|
"ElbowFlexion" ,
|
|
"ShoulderExtension" ,
|
|
"ShoulderFlexion" ,
|
|
"WristExtension" ,
|
|
"WristFlexion" ,
|
|
"ShoulderHorizontalAdduction",
|
|
"ShoulderHorizontalAbduction",
|
|
"ShoulderInternalRotation" ,
|
|
"ShoulderExternalRotation" ,
|
|
"WristUlnarDeviation" ,
|
|
"WristRadialDeviation" ,
|
|
"WristPronation" ,
|
|
"WristSupination" ,
|
|
"LumbarExtension" ,
|
|
"LumbarFlexion" };
|
|
|
|
const char* Anderson2007::GenderNames[] = {"Male","Female"};
|
|
|
|
const char* Anderson2007::AgeGroupNames[] = { "Young18To25",
|
|
"Middle55To65",
|
|
"SeniorOver65"};
|
|
|
|
const char* Anderson2007::JointTorqueNames[] = {"HipExtension" ,
|
|
"HipFlexion" ,
|
|
"KneeExtension" ,
|
|
"KneeFlexion" ,
|
|
"AnkleExtension",
|
|
"AnkleFlexion" };
|
|
|
|
const char* Gymnast::GenderNames[] = {"Male"};
|
|
const char* Gymnast::AgeGroupNames[] = {"Young18To25"};
|
|
const char* Gymnast::JointTorqueNames[] =
|
|
{ "HipExtension" ,
|
|
"HipFlexion" ,
|
|
"KneeExtension" ,
|
|
"KneeFlexion" ,
|
|
"AnkleExtension" ,
|
|
"AnkleFlexion" ,
|
|
"ElbowExtension" ,
|
|
"ElbowFlexion" ,
|
|
"ShoulderExtension" ,
|
|
"ShoulderFlexion" ,
|
|
"WristExtension" ,
|
|
"WristFlexion" ,
|
|
"ShoulderHorizontalAdduction",
|
|
"ShoulderHorizontalAbduction",
|
|
"ShoulderInternalRotation" ,
|
|
"ShoulderExternalRotation" ,
|
|
"WristUlnarDeviation" ,
|
|
"WristRadialDeviation" ,
|
|
"WristPronation" ,
|
|
"WristSupination" ,
|
|
"LumbarExtension" ,
|
|
"LumbarFlexion"};
|
|
|
|
/*************************************************************
|
|
Coefficient Tables
|
|
*************************************************************/
|
|
|
|
/*
|
|
This data is taken from Table 3 of
|
|
|
|
Anderson, D. E., Madigan, M. L., & Nussbaum, M. A. (2007).
|
|
Maximum voluntary joint torque as a function of joint angle
|
|
and angular velocity: model development and application to
|
|
the lower limb. Journal of biomechanics, 40(14), 3105-3113.
|
|
|
|
Each row contains the coefficients for the active and
|
|
passive torque characteristics for a specific joint,
|
|
direction, gender and age group. Each row corresponds
|
|
to a single block taken from Table 3, as read from
|
|
left to right top to bottom. The first 4 columns have
|
|
been added to describe the joint, direction, gender
|
|
and age group.
|
|
|
|
Column labels:
|
|
Parameter Set Meta Data
|
|
0: joint: hip0_knee1_ankle2,
|
|
1: direction: ext0_flex1,
|
|
2: gender: male0_female1,
|
|
3: age: age18to25_0_55to65_1_g65_2,
|
|
|
|
Active Torque-Angle and Torque-Velocity Curves
|
|
4: c1,
|
|
5: c2,
|
|
6: c3,
|
|
7: c4,
|
|
8: c5,
|
|
9: c6,
|
|
Passive Torque-Angle Curves
|
|
10: b1,
|
|
11: k1,
|
|
12: b2,
|
|
13: k2,
|
|
*/
|
|
double const Millard2016TorqueMuscle::Anderson2007Table3Mean[36][14] = {
|
|
{0,0,0,0,0.161,0.958,0.932,1.578,3.19,0.242,-1.21,-6.351,0.476,5.91 },
|
|
{0,0,1,0,0.181,0.697,1.242,1.567,3.164,0.164,-1.753,-6.358,0.239,3.872 },
|
|
{0,0,0,1,0.171,0.922,1.176,1.601,3.236,0.32,-2.16,-8.073,0.108,4.593 },
|
|
{0,0,1,1,0.14,0.83,1.241,1.444,2.919,0.317,-1.361,-7.128,0.013,6.479 },
|
|
{0,0,0,2,0.144,0.896,1.125,1.561,3.152,0.477,-2.671,-7.85,0.092,5.192 },
|
|
{0,0,1,2,0.138,0.707,1.542,1.613,3.256,0.36,-0.758,-7.545,0.018,6.061 },
|
|
{0,1,0,0,0.113,0.738,-0.214,2.095,4.267,0.218,1.21,-6.351,-0.476,5.91 },
|
|
{0,1,1,0,0.127,0.65,-0.35,2.136,4.349,0.156,1.753,-6.358,-0.239,3.872 },
|
|
{0,1,0,1,0.107,0.712,-0.192,2.038,4.145,0.206,2.16,-8.073,-0.108,4.593 },
|
|
{0,1,1,1,0.091,0.812,-0.196,2.145,4.366,0.186,1.361,-7.128,-0.013,6.479 },
|
|
{0,1,0,2,0.101,0.762,-0.269,1.875,3.819,0.296,2.671,-7.85,-0.092,5.192 },
|
|
{0,1,1,2,0.081,0.625,-0.422,2.084,4.245,0.196,0.758,-7.545,-0.018,6.061 },
|
|
{1,0,0,0,0.163,1.258,1.133,1.517,3.952,0.095,0,0,-6.25,-4.521 },
|
|
{1,0,1,0,0.159,1.187,1.274,1.393,3.623,0.173,0,0,-8.033,-5.25 },
|
|
{1,0,0,1,0.156,1.225,1.173,1.518,3.954,0.266,0,0,-12.83,-5.127 },
|
|
{1,0,1,1,0.128,1.286,1.141,1.332,3.469,0.233,0,0,-6.576,-4.466 },
|
|
{1,0,0,2,0.137,1.31,1.067,1.141,3.152,0.386,0,0,-10.519,-5.662 },
|
|
{1,0,1,2,0.124,1.347,1.14,1.066,2.855,0.464,0,0,-8.8,-6.763 },
|
|
{1,1,0,0,0.087,0.869,0.522,2.008,5.233,0.304,0,0,6.25,-4.521 },
|
|
{1,1,1,0,0.08,0.873,0.635,1.698,4.412,0.175,0,0,8.033,-5.25 },
|
|
{1,1,0,1,0.081,0.986,0.523,1.83,4.777,0.23,0,0,12.83,-5.127 },
|
|
{1,1,1,1,0.06,0.967,0.402,1.693,4.41,0.349,0,0,6.576,-4.466 },
|
|
{1,1,0,2,0.069,0.838,0.437,1.718,4.476,0.414,0,0,10.519,-5.662 },
|
|
{1,1,1,2,0.06,0.897,0.445,1.121,2.922,0.389,0,0,8.8,-6.763 },
|
|
{2,0,0,0,0.095,1.391,0.408,0.987,3.558,0.295,-0.0005781,-5.819,0.967,6.09 },
|
|
{2,0,1,0,0.104,1.399,0.424,0.862,3.109,0.189,-0.005218,-4.875,0.47,6.425 },
|
|
{2,0,0,1,0.114,1.444,0.551,0.593,2.128,0.35,-0.001311,-10.943,0.377,8.916 },
|
|
{2,0,1,1,0.093,1.504,0.381,0.86,3.126,0.349,-2.888e-05,-17.189,0.523,7.888 },
|
|
{2,0,0,2,0.106,1.465,0.498,0.49,1.767,0.571,-5.693e-05,-21.088,0.488,7.309 },
|
|
{2,0,1,2,0.125,1.299,0.58,0.587,1.819,0.348,-2.35e-05,-12.567,0.331,6.629 },
|
|
{2,1,0,0,0.033,1.51,-0.187,0.699,1.94,0.828,0.0005781,-5.819,-0.967,6.09 },
|
|
{2,1,1,0,0.027,1.079,-0.302,0.864,2.399,0.771,0.005218,-4.875,-0.47,6.425 },
|
|
{2,1,0,1,0.028,1.293,-0.284,0.634,1.759,0.999,0.001311,-10.943,-0.377,8.916 },
|
|
{2,1,1,1,0.024,1.308,-0.254,0.596,1.654,1.006,2.888e-05,-17.189,-0.523,7.888},
|
|
{2,1,0,2,0.029,1.419,-0.174,0.561,1.558,1.198,5.693e-05,-21.088,-0.488,7.309},
|
|
{2,1,1,2,0.022,1.096,-0.369,0.458,1.242,1.401,2.35e-05,-12.567,-0.331,6.629 }};
|
|
|
|
|
|
//See the description for the mean data. This table constains the
|
|
//parameter standard deviations
|
|
double const Millard2016TorqueMuscle::Anderson2007Table3Std[36][14] = {
|
|
{0,0,0,0,0.049,0.201,0.358,0.286,0.586,0.272,0.66,0.97,0.547,4.955 },
|
|
{0,0,1,0,0.047,0.13,0.418,0.268,0.542,0.175,1.93,2.828,0.292,1.895 },
|
|
{0,0,0,1,0.043,0.155,0.195,0.306,0.622,0.189,1.297,2.701,0.091,0.854 },
|
|
{0,0,1,1,0.032,0.246,0.365,0.223,0.45,0.14,1.294,2.541,0.02,2.924 },
|
|
{0,0,0,2,0.039,0.124,0.077,0.184,0.372,0.368,0.271,3.402,0.111,1.691 },
|
|
{0,0,1,2,0.003,0.173,0.279,0.135,0.273,0.237,0.613,0.741,0.031,2.265 },
|
|
{0,1,0,0,0.025,0.217,0.245,0.489,0.995,0.225,0.66,0.97,0.547,4.955 },
|
|
{0,1,1,0,0.033,0.178,0.232,0.345,0.702,0.179,1.93,2.828,0.292,1.895 },
|
|
{0,1,0,1,0.02,0.248,0.274,0.318,0.652,0.088,1.297,2.701,0.091,0.854 },
|
|
{0,1,1,1,0.016,0.244,0.209,0.375,0.765,0.262,1.294,2.541,0.02,2.924 },
|
|
{0,1,0,2,0.025,0.151,0.234,0.164,0.335,0.102,0.271,3.402,0.111,1.691 },
|
|
{0,1,1,2,0.008,0.062,0.214,0.321,0.654,0.28,0.613,0.741,0.031,2.265 },
|
|
{1,0,0,0,0.04,0.073,0.073,0.593,1.546,0.171,0,0,2.617,0.553 },
|
|
{1,0,1,0,0.028,0.084,0.181,0.38,0.989,0.27,0,0,3.696,1.512 },
|
|
{1,0,0,1,0.031,0.063,0.048,0.363,0.947,0.06,0,0,2.541,2.148 },
|
|
{1,0,1,1,0.016,0.094,0.077,0.319,0.832,0.133,0,0,1.958,1.63 },
|
|
{1,0,0,2,0.017,0.127,0.024,0.046,0.04,0.124,0,0,1.896,1.517 },
|
|
{1,0,1,2,0.018,0.044,0.124,0.128,0.221,0.129,0,0,6.141,0.742 },
|
|
{1,1,0,0,0.015,0.163,0.317,1.364,3.554,0.598,0,0,2.617,0.553 },
|
|
{1,1,1,0,0.015,0.191,0.287,0.825,2.139,0.319,0,0,3.696,1.512 },
|
|
{1,1,0,1,0.017,0.138,0.212,0.795,2.067,0.094,0,0,2.541,2.148 },
|
|
{1,1,1,1,0.015,0.21,0.273,0.718,1.871,0.143,0,0,1.958,1.63 },
|
|
{1,1,0,2,0.022,0.084,0.357,0.716,1.866,0.201,0,0,1.896,1.517 },
|
|
{1,1,1,2,0.005,0.145,0.21,0.052,0.135,0.078,0,0,6.141,0.742 },
|
|
{2,0,0,0,0.022,0.089,0.083,0.595,2.144,0.214,0.001193,7.384,0.323,1.196 },
|
|
{2,0,1,0,0.034,0.19,0.186,0.487,1.76,0.213,0.01135,6.77,0.328,1.177 },
|
|
{2,0,0,1,0.029,0.136,0.103,0.165,0.578,0.133,0.003331,10.291,0.403,3.119 },
|
|
{2,0,1,1,0.026,0.235,0.143,0.448,1.613,0.27,3.562e-05,7.848,0.394,1.141 },
|
|
{2,0,0,2,0.035,0.136,0.132,0.262,0.944,0.313,3.164e-05,1.786,0.258,0.902 },
|
|
{2,0,1,2,0.006,0.095,0.115,0.258,0.423,0.158,2.535e-05,10.885,0.247,2.186},
|
|
{2,1,0,0,0.005,0.19,0.067,0.108,0.301,0.134,0.001193,7.384,0.323,1.196 },
|
|
{2,1,1,0,0.006,0.271,0.171,0.446,1.236,0.206,0.01135,6.77,0.328,1.177 },
|
|
{2,1,0,1,0.005,0.479,0.178,0.216,0.601,0.214,0.003331,10.291,0.403,3.119 },
|
|
{2,1,1,1,0.002,0.339,0.133,0.148,0.41,0.284,3.562e-05,7.848,0.394,1.141 },
|
|
{2,1,0,2,0.002,0.195,0.056,0.188,0.521,0.29,3.164e-05,1.786,0.258,0.902 },
|
|
{2,1,1,2,0.003,0.297,0.109,0.089,0.213,0.427,2.535e-05,10.885,0.247,2.186}};
|
|
|
|
/*
|
|
This table contains parameters for the newly made torque muscle curves:
|
|
|
|
1. maxIsometricTorque Nm
|
|
2. maxAngularVelocity rad/s
|
|
3. angleAtOneActiveNormTorque rad
|
|
4. angularWidthActiveTorque rad
|
|
5. tvAtMaxEccentricVelocity Nm/Nm
|
|
6. tvAtHalfMaxConcentricVelocity Nm/Nm
|
|
7. angleAtZeroPassiveTorque rad
|
|
8. angleAtOneNormPassiveTorque rad
|
|
|
|
*/
|
|
|
|
|
|
|
|
double const Millard2016TorqueMuscle::GymnastWholeBody[22][12] =
|
|
{ {0,0,0,0,175.746, 9.02335, 1.06465, 1.05941, 1.1, 0.163849, 0.79158, 1.5708 },
|
|
{0,1,0,0,157.293, 9.18043, 0.733038, 1.21999, 1.11905, 0.25, -0.0888019,-0.515207 },
|
|
{1,0,0,0,285.619, 19.2161, 0.942478, 0.509636, 1.13292, 0.115304, 2.00713, 2.70526 },
|
|
{1,1,0,0,98.7579, 16.633, 1.02974, 1.11003, 1.12, 0.19746, 0, -0.174533 },
|
|
{2,0,0,0,127.561, 11.7646, 0.408, 0.660752, 1.159, 0.410591, 0.0292126, 0.785398 },
|
|
{2,1,0,0,44.3106, 17.2746, -0.187, 0.60868, 1.2656, 0.112303, -0.523599, -1.0472 },
|
|
{3,0,0,0,127.401, 16.249, 2.14675, 1.83085, 1.1, 0.250134, 2.8291, 3.55835 },
|
|
{3,1,0,0,91.1388, 19.0764, 0.890118, 1.2898, 1.23011, 0.249656, -0.523599, -1.0472 },
|
|
{5,0,0,0,15.5653, 36.5472, 1.55334, 1.38928, 1.16875, 0.115356, 1.0472, 1.5708 },
|
|
{5,1,0,0,39.2252, 36.3901, 0.663225, 1.71042, 1.14, 0.115456, -0.793739, -1.49714 },
|
|
{3,2,0,0,128.496, 18.05, 0.839204, 1.28041, 1.25856, 0.214179, 1.5708, 2.26893 },
|
|
{3,3,0,0,94.6839, 18 , -0.277611, 2.37086, 1.23042, 0.224227, -0.523599, -1.0472 },
|
|
{3,5,0,0,50.522 , 19.47, -1.18761, 2.80524, 1.27634, 0.285399, 1.39626, 1.91986 },
|
|
{3,4,0,0,43.5837, 18, -0.670796, 1.98361, 1.35664, 0.229104, -1.0472, -1.5708 },
|
|
{4,1,0,0,101.384, 18.1, 0.33, 3.62155, 1.37223, 0.189909, 0, -0.174533 },
|
|
{4,0,0,0,69.8728, 18.45, 1.64319, 1.30795, 1.31709, 0.189676, 2.0944, 2.61799 },
|
|
{5,6,0,0,13.5361, 35.45, -0.209204, 1.33735, 1.23945, 0.250544, -0.785398, -1.5708 },
|
|
{5,7,0,0,12.976 , 27.88, -0.212389, 0.991803, 1.3877, 0.207506, 0.785398, 1.5708 },
|
|
{5,9,0,0,31.4217, 18.02, 0.43, 1.47849, 1.34817, 0.196913, 0, -0.523599},
|
|
{5,8,0,0,23.8345, 21.77, -1.14319, 2.56082, 1.31466, 0.2092, 0.349066, 0.872665 },
|
|
{6,0,0,0,687.864, 1.0472, 1.5506, 1.14543, 1.1, 0.45, 0.306223, 1.35342 },
|
|
{6,1,0,0,211.65 , 0.523599, 0, 6.28319, 1.1, 0.45, 0, -0.785398 }};
|
|
|
|
/*************************************************************
|
|
Map that goes from a single joint-torque-direction index to
|
|
the pair of joint and direction indicies used in the tables
|
|
*************************************************************/
|
|
|
|
const static struct JointSet{
|
|
enum item { Hip = 0,
|
|
Knee,
|
|
Ankle,
|
|
Shoulder,
|
|
Elbow,
|
|
Wrist,
|
|
Lumbar,
|
|
Last};
|
|
JointSet(){}
|
|
} JointSet;
|
|
|
|
|
|
struct DirectionSet{
|
|
enum item {
|
|
Extension = 0,
|
|
Flexion,
|
|
HorizontalAdduction,
|
|
HorizontalAbduction,
|
|
ExternalRotation,
|
|
InternalRotation,
|
|
Supination,
|
|
Pronation,
|
|
RadialDeviation,
|
|
UlnarDeviation,
|
|
Last
|
|
};
|
|
DirectionSet(){}
|
|
} DirectionSet;
|
|
|
|
|
|
const static int JointTorqueMap[22][3] = {
|
|
{(int)JointTorqueSet::HipExtension , (int)JointSet::Hip , (int)DirectionSet::Extension },
|
|
{(int)JointTorqueSet::HipFlexion , (int)JointSet::Hip , (int)DirectionSet::Flexion },
|
|
{(int)JointTorqueSet::KneeExtension , (int)JointSet::Knee , (int)DirectionSet::Extension },
|
|
{(int)JointTorqueSet::KneeFlexion , (int)JointSet::Knee , (int)DirectionSet::Flexion },
|
|
{(int)JointTorqueSet::AnkleExtension , (int)JointSet::Ankle , (int)DirectionSet::Extension },
|
|
{(int)JointTorqueSet::AnkleFlexion , (int)JointSet::Ankle , (int)DirectionSet::Flexion },
|
|
{(int)JointTorqueSet::ElbowExtension , (int)JointSet::Elbow , (int)DirectionSet::Extension },
|
|
{(int)JointTorqueSet::ElbowFlexion , (int)JointSet::Elbow , (int)DirectionSet::Flexion },
|
|
{(int)JointTorqueSet::ShoulderExtension , (int)JointSet::Shoulder , (int)DirectionSet::Extension },
|
|
{(int)JointTorqueSet::ShoulderFlexion , (int)JointSet::Shoulder , (int)DirectionSet::Flexion },
|
|
{(int)JointTorqueSet::WristExtension , (int)JointSet::Wrist , (int)DirectionSet::Extension },
|
|
{(int)JointTorqueSet::WristFlexion , (int)JointSet::Wrist , (int)DirectionSet::Flexion },
|
|
{(int)JointTorqueSet::ShoulderHorizontalAdduction , (int)JointSet::Shoulder , (int)DirectionSet::HorizontalAdduction},
|
|
{(int)JointTorqueSet::ShoulderHorizontalAbduction , (int)JointSet::Shoulder , (int)DirectionSet::HorizontalAbduction},
|
|
{(int)JointTorqueSet::ShoulderInternalRotation , (int)JointSet::Shoulder , (int)DirectionSet::InternalRotation },
|
|
{(int)JointTorqueSet::ShoulderExternalRotation , (int)JointSet::Shoulder , (int)DirectionSet::ExternalRotation },
|
|
{(int)JointTorqueSet::WristUlnarDeviation , (int)JointSet::Wrist , (int)DirectionSet::UlnarDeviation },
|
|
{(int)JointTorqueSet::WristRadialDeviation , (int)JointSet::Wrist , (int)DirectionSet::RadialDeviation },
|
|
{(int)JointTorqueSet::WristPronation , (int)JointSet::Wrist , (int)DirectionSet::Pronation },
|
|
{(int)JointTorqueSet::WristSupination , (int)JointSet::Wrist , (int)DirectionSet::Supination },
|
|
{(int)JointTorqueSet::LumbarExtension , (int)JointSet::Lumbar , (int)DirectionSet::Extension },
|
|
{(int)JointTorqueSet::LumbarFlexion , (int)JointSet::Lumbar , (int)DirectionSet::Flexion }};
|
|
|
|
|
|
/*************************************************************
|
|
Constructors
|
|
*************************************************************/
|
|
|
|
Millard2016TorqueMuscle::
|
|
Millard2016TorqueMuscle( )
|
|
:angleOffset(1.0),
|
|
signOfJointAngle(1.0),
|
|
signOfJointTorque(1.0),
|
|
signOfConcentricAnglularVelocity(1.0),
|
|
muscleName("empty")
|
|
{
|
|
muscleCurvesAreDirty = true;
|
|
}
|
|
|
|
Millard2016TorqueMuscle::Millard2016TorqueMuscle(
|
|
DataSet::item dataSet,
|
|
const SubjectInformation &subjectInfo,
|
|
int jointTorque,
|
|
double jointAngleOffsetRelativeToDoxygenFigures,
|
|
double signOfJointAngleRelativeToDoxygenFigures,
|
|
double signOfJointTorque,
|
|
const std::string& name
|
|
):angleOffset(jointAngleOffsetRelativeToDoxygenFigures),
|
|
signOfJointAngle(signOfJointAngleRelativeToDoxygenFigures),
|
|
signOfJointTorque(signOfJointTorque),
|
|
signOfConcentricAnglularVelocity(signOfJointTorque),
|
|
muscleName(name),
|
|
dataSet(dataSet)
|
|
{
|
|
|
|
subjectHeightInMeters = subjectInfo.heightInMeters;
|
|
subjectMassInKg = subjectInfo.massInKg;
|
|
passiveCurveAngleOffset = 0.;
|
|
beta = 0.1;
|
|
|
|
int gender = (int) subjectInfo.gender;
|
|
int ageGroup = (int) subjectInfo.ageGroup;
|
|
|
|
int joint = -1;
|
|
int jointDirection = -1;
|
|
|
|
for(int i=0; i < JointTorqueSet::Last; ++i){
|
|
if(JointTorqueMap[i][0] == jointTorque){
|
|
joint = JointTorqueMap[i][1];
|
|
jointDirection = JointTorqueMap[i][2];
|
|
}
|
|
}
|
|
|
|
if(joint == -1 || jointDirection == -1){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "Millard2016TorqueMuscle:"
|
|
<< muscleName
|
|
<< ": A jointTorqueDirection of " << jointTorque
|
|
<< " does not exist.";
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( abs(abs(signOfJointAngle)-1) > EPSILON){
|
|
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "Millard2016TorqueMuscle:"
|
|
<< muscleName
|
|
<< ": signOfJointAngleRelativeToAnderson2007 must be [-1,1] not "
|
|
<< signOfJointAngle;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( abs(abs(signOfConcentricAnglularVelocity)-1) > EPSILON){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "Millard2016TorqueMuscle:"
|
|
<< muscleName
|
|
<< ": signOfJointAngularVelocityDuringConcentricContraction "
|
|
<< "must be [-1,1] not "
|
|
<< signOfConcentricAnglularVelocity;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( abs(abs(signOfJointTorque)-1) > EPSILON){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "Millard2016TorqueMuscle:"
|
|
<< muscleName
|
|
<< ": signOfJointTorque must be [-1,1] not "
|
|
<< signOfJointTorque;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
|
|
if(subjectHeightInMeters <= 0){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "Millard2016TorqueMuscle:"
|
|
<< muscleName
|
|
<< ": subjectHeightInMeters > 0, but it's "
|
|
<< subjectHeightInMeters;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if(subjectMassInKg <= 0){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "Millard2016TorqueMuscle:"
|
|
<< muscleName
|
|
<< ": subjectMassInKg > 0, but it's "
|
|
<< subjectMassInKg;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
|
|
|
|
int idx = -1;
|
|
int jointIdx = 0;
|
|
int dirIdx = 1;
|
|
int genderIdx = 2;
|
|
int ageIdx = 3;
|
|
|
|
switch(dataSet){
|
|
case DataSet::Anderson2007:
|
|
{
|
|
c1c2c3c4c5c6Anderson2007.resize(6);
|
|
b1k1b2k2Anderson2007.resize(4);
|
|
|
|
for(int i=0; i<36;++i){
|
|
|
|
if( abs(Anderson2007Table3Mean[i][jointIdx]
|
|
-(double)joint) < EPSILON
|
|
&& abs(Anderson2007Table3Mean[i][dirIdx]
|
|
-(double)jointDirection) < EPSILON
|
|
&& abs(Anderson2007Table3Mean[i][genderIdx]
|
|
-(double)gender) < EPSILON
|
|
&& abs(Anderson2007Table3Mean[i][ageIdx]
|
|
-(double)ageGroup) < EPSILON){
|
|
idx = i;
|
|
}
|
|
}
|
|
|
|
if(idx != -1){
|
|
for(int i=0; i<6; ++i){
|
|
c1c2c3c4c5c6Anderson2007[i] = Anderson2007Table3Mean[idx][i+4];
|
|
}
|
|
for(int i=0; i<4; ++i){
|
|
b1k1b2k2Anderson2007[i] = Anderson2007Table3Mean[idx][i+10];
|
|
}
|
|
}
|
|
|
|
} break;
|
|
|
|
case DataSet::Gymnast:
|
|
{
|
|
gymnastParams.resize(8);
|
|
for(int i=0; i<JointTorqueSet::Last;++i){
|
|
|
|
if( abs(GymnastWholeBody[i][jointIdx]
|
|
-(double)joint) < EPSILON
|
|
&& abs(GymnastWholeBody[i][dirIdx]
|
|
-(double)jointDirection) < EPSILON
|
|
&& abs(GymnastWholeBody[i][genderIdx]
|
|
-(double)gender) < EPSILON
|
|
&& abs(GymnastWholeBody[i][ageIdx]
|
|
-(double)ageGroup) < EPSILON){
|
|
idx = i;
|
|
}
|
|
}
|
|
|
|
if(idx != -1){
|
|
for(int i=0; i<8; ++i){
|
|
gymnastParams[i] = GymnastWholeBody[idx][i+4];
|
|
}
|
|
}
|
|
} break;
|
|
|
|
default:
|
|
{
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "Millard2016TorqueMuscle:"
|
|
<< muscleName
|
|
<< "The requested DataSet does not exist.";
|
|
assert(0);
|
|
abort();
|
|
}
|
|
};
|
|
|
|
|
|
|
|
if(idx == -1){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "Millard2016TorqueMuscle:"
|
|
<< muscleName
|
|
<< "The combination of data set (" << dataSet << ")"
|
|
<< " joint (" << joint << ")"
|
|
<< " joint direction (" << jointDirection << ")"
|
|
<< " gender, (" << gender << ")"
|
|
<< "and age " << ageGroup << ")"
|
|
<< "could not be found";
|
|
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
|
|
|
|
muscleCurvesAreDirty = true;
|
|
passiveTorqueScale = 1.0;
|
|
updateTorqueMuscleCurves();
|
|
|
|
|
|
}
|
|
|
|
|
|
/*************************************************************
|
|
Muscle Model Code
|
|
*************************************************************/
|
|
|
|
|
|
double Millard2016TorqueMuscle::
|
|
calcJointTorque( double jointAngle,
|
|
double jointAngularVelocity,
|
|
double activation) const
|
|
{
|
|
|
|
if(muscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
|
|
double fiberAngle = calcFiberAngle(jointAngle);
|
|
double fiberVelocity = calcFiberAngularVelocity(jointAngularVelocity);
|
|
double ta = taCurve.calcValue(fiberAngle);
|
|
double tp = tpCurve.calcValue(fiberAngle);
|
|
double fiberVelocityNorm = fiberVelocity/omegaMax;
|
|
double tv = tvCurve.calcValue(fiberVelocityNorm);
|
|
|
|
|
|
double jointTorque = maxActiveIsometricTorque*(
|
|
activation*ta*tv
|
|
+ passiveTorqueScale*tp
|
|
- beta*fiberVelocityNorm);
|
|
if(jointTorque < 0){
|
|
jointTorque = 0;
|
|
}
|
|
|
|
return jointTorque*signOfJointTorque;
|
|
}
|
|
|
|
|
|
|
|
void Millard2016TorqueMuscle::
|
|
calcTorqueMuscleInfo(double jointAngle,
|
|
double jointAngularVelocity,
|
|
double activation,
|
|
TorqueMuscleInfo& tmi) const
|
|
{
|
|
if(muscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
|
|
double fiberAngle = calcFiberAngle(jointAngle);
|
|
double fiberAngularVelocity = calcFiberAngularVelocity(jointAngularVelocity);
|
|
double ta = taCurve.calcValue(fiberAngle);
|
|
double tp = passiveTorqueScale*tpCurve.calcValue(fiberAngle);
|
|
|
|
double omegaNorm = fiberAngularVelocity/omegaMax;
|
|
double D_wn_w = 1.0/omegaMax;
|
|
double tv = tvCurve.calcValue(omegaNorm);
|
|
|
|
double betaUpd = beta;
|
|
double tb = -betaUpd*omegaNorm;
|
|
|
|
//The jointTorque is not allowed to go below 0, as this corresponds
|
|
//to the muscle pushing. If the joint torque is going negative, we update
|
|
//beta so that it will perfectly zero the joint torque.
|
|
if(activation*ta*tv + passiveTorqueScale*tp + tb < 0){
|
|
betaUpd = (activation*ta*tv + passiveTorqueScale*tp)/omegaNorm;
|
|
tb = -betaUpd*omegaNorm;
|
|
}
|
|
|
|
double D_ta_DfiberAngle = taCurve.calcDerivative(fiberAngle,1);
|
|
double D_tp_DfiberAngle = passiveTorqueScale*tpCurve.calcDerivative(fiberAngle,1);
|
|
double D_tv_DfiberAngularVelocity
|
|
= tvCurve.calcDerivative(omegaNorm,1)*D_wn_w;
|
|
double D_tb_DfiberAngularVelocity = -betaUpd*D_wn_w;
|
|
|
|
double D_fiberAngle_D_jointAngle = signOfJointAngle;
|
|
double D_tv_DfiberAngularVelocity_D_jointAngularVelocity =
|
|
signOfConcentricAnglularVelocity;
|
|
|
|
|
|
tmi.jointAngle = jointAngle;
|
|
tmi.jointAngularVelocity = jointAngularVelocity;
|
|
tmi.fiberAngle = fiberAngle;
|
|
//tmi.tendonAngle = 0.;
|
|
tmi.fiberAngularVelocity = fiberAngularVelocity;
|
|
//tmi.tendonAngularVelocity = 0.;
|
|
|
|
tmi.fiberPassiveTorqueAngleMultiplier = tp;
|
|
tmi.fiberActiveTorqueAngleMultiplier = ta;
|
|
tmi.fiberActiveTorqueAngularVelocityMultiplier = tv;
|
|
//tmi.tendonTorqueAngleMultiplier = activation*ta*tv + tp;
|
|
|
|
tmi.activation = activation;
|
|
tmi.fiberActiveTorque = maxActiveIsometricTorque*(activation*ta*tv);
|
|
tmi.fiberPassiveTorque = maxActiveIsometricTorque*(tb+tp);
|
|
tmi.fiberDampingTorque = maxActiveIsometricTorque*(tb);
|
|
tmi.fiberNormDampingTorque = tb;
|
|
tmi.fiberTorque = tmi.fiberActiveTorque
|
|
+ tmi.fiberPassiveTorque;
|
|
//tmi.tendonTorque = tmi.fiberActiveTorque
|
|
// + tmi.fiberPassiveTorque;
|
|
|
|
tmi.jointTorque = signOfJointTorque*tmi.fiberTorque;
|
|
|
|
tmi.fiberStiffness = maxActiveIsometricTorque*(
|
|
activation*D_ta_DfiberAngle*tv
|
|
+ D_tp_DfiberAngle);
|
|
|
|
//tmi.tendonStiffness = std::numeric_limits<double>::infinity();
|
|
tmi.jointStiffness = signOfJointTorque
|
|
*tmi.fiberStiffness
|
|
*D_fiberAngle_D_jointAngle;
|
|
|
|
tmi.fiberActivePower = tmi.fiberActiveTorque
|
|
* tmi.fiberAngularVelocity;
|
|
tmi.fiberPassivePower = tmi.fiberPassiveTorque
|
|
* tmi.fiberAngularVelocity;
|
|
tmi.fiberPower = tmi.fiberActivePower
|
|
+ tmi.fiberPassivePower;
|
|
//tmi.tendonPower = 0.;
|
|
tmi.jointPower = tmi.jointTorque * jointAngularVelocity;
|
|
|
|
|
|
//tau = signTq*tauIso*(a * ta(theta) * tv(thetaDot) + tp(theta) )
|
|
// Dtau_Da = tauMaxIso*(ta(theta) * tv(thetaDot) )
|
|
tmi.DjointTorqueDactivation =
|
|
signOfJointTorque
|
|
*maxActiveIsometricTorque
|
|
*(ta * tv);
|
|
|
|
//Dtau_Domega = signTq*tauIso*(a * ta(theta) * Dtv_DthetaDot(thetaDot)* )
|
|
tmi.DjointTorqueDjointAngularVelocity =
|
|
signOfJointTorque
|
|
* maxActiveIsometricTorque
|
|
* ( activation
|
|
* ta
|
|
* D_tv_DfiberAngularVelocity
|
|
* D_tv_DfiberAngularVelocity_D_jointAngularVelocity
|
|
+ D_tb_DfiberAngularVelocity);
|
|
|
|
tmi.DjointTorqueDjointAngle =
|
|
signOfJointTorque
|
|
* maxActiveIsometricTorque
|
|
* ( activation
|
|
*D_ta_DfiberAngle
|
|
* tv
|
|
+ D_tp_DfiberAngle
|
|
)* D_fiberAngle_D_jointAngle;
|
|
|
|
}
|
|
|
|
|
|
/*************************************************************
|
|
Get / Set Functions
|
|
*************************************************************/
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getJointTorqueSign() const
|
|
{
|
|
return signOfJointTorque;
|
|
}
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getJointAngleSign() const
|
|
{
|
|
return signOfJointAngle;
|
|
}
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getJointAngleOffset() const
|
|
{
|
|
return angleOffset;
|
|
}
|
|
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getNormalizedDampingCoefficient() const
|
|
{
|
|
return beta;
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::
|
|
setNormalizedDampingCoefficient(double betaUpd)
|
|
{
|
|
if(betaUpd < 0){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "setNormalizedDampingCoefficient:"
|
|
<< muscleName
|
|
<< "beta is " << betaUpd
|
|
<< " but beta must be > 0 "
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
|
|
beta = betaUpd;
|
|
}
|
|
|
|
|
|
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getMaximumActiveIsometricTorque() const
|
|
{
|
|
return maxActiveIsometricTorque;
|
|
}
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getMaximumJointAngularVelocity() const
|
|
{
|
|
return calcFiberAngularVelocity(omegaMax);
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::
|
|
setMaximumActiveIsometricTorque(double maxIsoTorque)
|
|
{
|
|
muscleCurvesAreDirty = true;
|
|
maxActiveIsometricTorque = maxIsoTorque;
|
|
}
|
|
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getJointAngleAtMaximumActiveIsometricTorque() const
|
|
{
|
|
return calcJointAngle(angleAtOneNormActiveTorque);
|
|
}
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getJointAngleAtOneNormalizedPassiveIsometricTorque() const
|
|
{
|
|
if(muscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
return calcJointAngle(angleAtOneNormPassiveTorque);
|
|
}
|
|
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getPassiveTorqueScale() const
|
|
{
|
|
return passiveTorqueScale;
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::
|
|
setPassiveTorqueScale(double passiveTorqueScaling)
|
|
{
|
|
muscleCurvesAreDirty = true;
|
|
passiveTorqueScale = passiveTorqueScaling;
|
|
}
|
|
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getPassiveCurveAngleOffset() const
|
|
{
|
|
return passiveCurveAngleOffset;
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::
|
|
setPassiveCurveAngleOffset(double passiveCurveAngleOffsetVal)
|
|
{
|
|
muscleCurvesAreDirty = true;
|
|
passiveCurveAngleOffset = passiveCurveAngleOffsetVal;
|
|
}
|
|
|
|
|
|
void Millard2016TorqueMuscle::
|
|
fitPassiveCurveAngleOffset(double jointAngleTarget,
|
|
double passiveTorqueTarget)
|
|
{
|
|
muscleCurvesAreDirty = true;
|
|
setPassiveCurveAngleOffset(0.0);
|
|
|
|
if(passiveTorqueTarget < SQRTEPSILON){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "fitPassiveTorqueScale:"
|
|
<< muscleName
|
|
<< ": passiveTorque " << passiveTorqueTarget
|
|
<< " but it should be greater than sqrt(eps)"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
//Solve for the fiber angle at which the curve develops
|
|
//the desired passiveTorque
|
|
double normPassiveTorque = passiveTorqueTarget
|
|
/maxActiveIsometricTorque;
|
|
//Ge a good initial guess
|
|
|
|
VectorNd curveDomain = tpCurve.getCurveDomain();
|
|
double angleRange = abs( curveDomain[1]-curveDomain[0]);
|
|
double fiberAngle = 0.5*(curveDomain[0]+curveDomain[1]);
|
|
double jointAngleCurr = calcJointAngle(fiberAngle);
|
|
|
|
TorqueMuscleInfo tqInfo = TorqueMuscleInfo();
|
|
|
|
calcTorqueMuscleInfo(jointAngleCurr,0.,0.,tqInfo);
|
|
double err = tqInfo.fiberPassiveTorqueAngleMultiplier-normPassiveTorque;
|
|
double jointAngleLeft = 0;
|
|
double jointAngleRight = 0;
|
|
double errLeft = 0;
|
|
double errRight = 0;
|
|
|
|
double h = 0.25*angleRange;
|
|
|
|
//Get a good initial guess - necessary because these curves
|
|
//can be *very* nonlinear.
|
|
int iter = 0;
|
|
int iterMax = 10;
|
|
int tol = sqrt(SQRTEPSILON);
|
|
|
|
while(iter < iterMax && abs(err) > tol){
|
|
jointAngleLeft = jointAngleCurr-h;
|
|
jointAngleRight = jointAngleCurr+h;
|
|
|
|
calcTorqueMuscleInfo(jointAngleLeft,0.,0.,tqInfo);
|
|
errLeft = tqInfo.fiberPassiveTorqueAngleMultiplier
|
|
- normPassiveTorque;
|
|
|
|
calcTorqueMuscleInfo(jointAngleRight,0.,0.,tqInfo);
|
|
errRight = tqInfo.fiberPassiveTorqueAngleMultiplier
|
|
- normPassiveTorque;
|
|
|
|
if(abs(errLeft)<abs(err) && abs(errLeft)<abs(errRight)){
|
|
err = errLeft;
|
|
jointAngleCurr = jointAngleLeft;
|
|
}
|
|
if(abs(errRight)<abs(err) && abs(errRight)<abs(errLeft)){
|
|
err = errRight;
|
|
jointAngleCurr = jointAngleRight;
|
|
}
|
|
h = h/2.0;
|
|
++iter;
|
|
}
|
|
|
|
|
|
//Use Newton's method to polish up this initial guess.
|
|
iter = 0;
|
|
err = SQRTEPSILON*2;
|
|
double derr = 0;
|
|
double delta= 0;
|
|
|
|
while(abs(err) > SQRTEPSILON && iter < iterMax){
|
|
calcTorqueMuscleInfo(jointAngleCurr,0.,0.,tqInfo);
|
|
err = tqInfo.fiberPassiveTorqueAngleMultiplier
|
|
-normPassiveTorque;
|
|
derr = signOfJointTorque*tqInfo.DjointTorqueDjointAngle
|
|
/ maxActiveIsometricTorque ;
|
|
delta = -err/derr;
|
|
jointAngleCurr += delta;
|
|
++iter;
|
|
}
|
|
|
|
if(abs(err)>SQRTEPSILON){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "fitPassiveCurveAngleOffset:"
|
|
<< muscleName
|
|
<< ": failed to fit the passive curve offset "
|
|
<< " such that the curve develops the desired "
|
|
<< " passive torque at the given joint angle. This"
|
|
<< " should not be possible - contact the maintainer "
|
|
<< " of this addon."
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
double fiberAngleOffset = calcFiberAngle(jointAngleTarget)
|
|
- calcFiberAngle(jointAngleCurr);
|
|
|
|
setPassiveCurveAngleOffset(fiberAngleOffset);
|
|
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::
|
|
fitPassiveTorqueScale(double jointAngleTarget,
|
|
double passiveTorqueTarget)
|
|
{
|
|
muscleCurvesAreDirty = true;
|
|
setPassiveTorqueScale(1.0);
|
|
|
|
VectorNd curveDomain = tpCurve.getCurveDomain();
|
|
VectorNd curveRange = VectorNd(2);
|
|
curveRange[0] = tpCurve.calcValue(curveDomain[0]);
|
|
curveRange[1] = tpCurve.calcValue(curveDomain[1]);
|
|
double fiberAngle = calcFiberAngle(jointAngleTarget);
|
|
|
|
if( (fiberAngle < curveDomain[0] && (curveRange[0] < curveRange[1]))
|
|
|| (fiberAngle > curveDomain[1] && (curveRange[1] < curveRange[0])) ){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "fitPassiveTorqueScale:"
|
|
<< muscleName
|
|
<< ": joint angle is " << jointAngleTarget
|
|
<< " but it should be between "
|
|
<< calcJointAngle(curveDomain[0]) << " and "
|
|
<< calcJointAngle(curveDomain[1])
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if(passiveTorqueTarget < SQRTEPSILON){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "fitPassiveTorqueScale:"
|
|
<< muscleName
|
|
<< ": passiveTorque " << passiveTorqueTarget
|
|
<< " but it should be greater than sqrt(eps)"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
double normPassiveTorque = passiveTorqueTarget/maxActiveIsometricTorque;
|
|
int iter = 0;
|
|
int iterMax = 10;
|
|
double err = SQRTEPSILON*2;
|
|
double derr = 0;
|
|
double delta= 0;
|
|
double scale = 1.0;
|
|
setPassiveTorqueScale(scale);
|
|
TorqueMuscleInfo tqInfo = TorqueMuscleInfo();
|
|
|
|
while(abs(err) > SQRTEPSILON && iter < iterMax){
|
|
setPassiveTorqueScale(scale);
|
|
calcTorqueMuscleInfo(jointAngleTarget,0,0,tqInfo);
|
|
err = tqInfo.fiberPassiveTorqueAngleMultiplier - normPassiveTorque;
|
|
derr= tqInfo.fiberPassiveTorqueAngleMultiplier/scale;
|
|
delta = -err/derr;
|
|
scale += delta;
|
|
|
|
if(scale < SQRTEPSILON){
|
|
scale = SQRTEPSILON;
|
|
}
|
|
iter++;
|
|
}
|
|
|
|
if(abs(err) > SQRTEPSILON){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "fitPassiveTorqueScale:"
|
|
<< muscleName
|
|
<< ": passiveTorqueScale could not be fit to"
|
|
<< " the data given. See the maintainer of this"
|
|
<< " addon for help."
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
}
|
|
|
|
/*
|
|
const RigidBodyDynamics::Math::VectorNd&
|
|
Millard2016TorqueMuscle::getParametersc1c2c3c4c5c6()
|
|
{
|
|
return c1c2c3c4c5c6Anderson2007;
|
|
}
|
|
|
|
const RigidBodyDynamics::Math::VectorNd&
|
|
Millard2016TorqueMuscle::getParametersb1k1b2k2()
|
|
{
|
|
return b1k1b2k2Anderson2007;
|
|
}
|
|
*/
|
|
|
|
const SmoothSegmentedFunction& Millard2016TorqueMuscle::
|
|
getActiveTorqueAngleCurve() const
|
|
{
|
|
//This must be updated if the parameters have changed
|
|
if(muscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
return taCurve;
|
|
}
|
|
|
|
const SmoothSegmentedFunction& Millard2016TorqueMuscle::
|
|
getPassiveTorqueAngleCurve() const
|
|
{
|
|
//This must be updated if the parameters have changed
|
|
if(muscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
return tpCurve;
|
|
}
|
|
|
|
const SmoothSegmentedFunction& Millard2016TorqueMuscle::
|
|
getTorqueAngularVelocityCurve() const
|
|
{
|
|
//This must be updated if the parameters have changed
|
|
if(muscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
return tvCurve;
|
|
}
|
|
|
|
|
|
string Millard2016TorqueMuscle::getName(){
|
|
return muscleName;
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::setName(string &name){
|
|
muscleCurvesAreDirty = true;
|
|
muscleName = name;
|
|
}
|
|
|
|
|
|
|
|
/*************************************************************
|
|
Utilities
|
|
*************************************************************/
|
|
//fa = signJointAngle*(ja - jaO)
|
|
// ja = signJointAngle*fa + ja0
|
|
//dja = signJointAngle*dfa
|
|
|
|
double Millard2016TorqueMuscle::
|
|
calcFiberAngle(double jointAngle) const
|
|
{
|
|
return signOfJointAngle*(jointAngle-angleOffset);
|
|
}
|
|
|
|
double Millard2016TorqueMuscle::
|
|
calcJointAngle(double fiberAngle) const
|
|
{
|
|
return fiberAngle*signOfJointAngle + angleOffset;
|
|
}
|
|
|
|
|
|
double Millard2016TorqueMuscle::
|
|
calcFiberAngularVelocity(double jointAngularVelocity) const
|
|
{
|
|
return signOfConcentricAnglularVelocity*jointAngularVelocity;
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::updateTorqueMuscleCurves()
|
|
{
|
|
std::string tempName = muscleName;
|
|
|
|
switch(dataSet){
|
|
case DataSet::Anderson2007:
|
|
{
|
|
double c4 = c1c2c3c4c5c6Anderson2007[3];
|
|
double c5 = c1c2c3c4c5c6Anderson2007[4];
|
|
omegaMax = abs( 2.0*c4*c5/(c5-3.0*c4) );
|
|
|
|
scaleFactorAnderson2007 = subjectHeightInMeters
|
|
*subjectMassInKg
|
|
*gravity;
|
|
maxActiveIsometricTorque = scaleFactorAnderson2007
|
|
*c1c2c3c4c5c6Anderson2007[0];
|
|
|
|
angleAtOneNormActiveTorque = c1c2c3c4c5c6Anderson2007[2];
|
|
|
|
TorqueMuscleFunctionFactory::
|
|
createAnderson2007ActiveTorqueAngleCurve(
|
|
c1c2c3c4c5c6Anderson2007[1],
|
|
c1c2c3c4c5c6Anderson2007[2],
|
|
tempName.append("_taCurve"),
|
|
taCurve);
|
|
|
|
tempName = muscleName;
|
|
|
|
TorqueMuscleFunctionFactory::
|
|
createAnderson2007ActiveTorqueVelocityCurve(
|
|
c1c2c3c4c5c6Anderson2007[3],
|
|
c1c2c3c4c5c6Anderson2007[4],
|
|
c1c2c3c4c5c6Anderson2007[5],
|
|
1.1,
|
|
1.4,
|
|
tempName.append("_tvCurve"),
|
|
tvCurve);
|
|
|
|
tempName = muscleName;
|
|
|
|
double normMaxActiveIsometricTorque = maxActiveIsometricTorque
|
|
/scaleFactorAnderson2007;
|
|
|
|
TorqueMuscleFunctionFactory::
|
|
createAnderson2007PassiveTorqueAngleCurve(
|
|
scaleFactorAnderson2007,
|
|
normMaxActiveIsometricTorque,
|
|
b1k1b2k2Anderson2007[0],
|
|
b1k1b2k2Anderson2007[1],
|
|
b1k1b2k2Anderson2007[2],
|
|
b1k1b2k2Anderson2007[3],
|
|
tempName.append("_tpCurve"),
|
|
tpCurve);
|
|
|
|
tpCurve.shift(passiveCurveAngleOffset,0);
|
|
|
|
double k = 0;
|
|
double b = 0;
|
|
|
|
if(b1k1b2k2Anderson2007[0] > 0){
|
|
b = b1k1b2k2Anderson2007[0];
|
|
k = b1k1b2k2Anderson2007[1];
|
|
}else if(b1k1b2k2Anderson2007[2] > 0){
|
|
b = b1k1b2k2Anderson2007[2];
|
|
k = b1k1b2k2Anderson2007[3];
|
|
}
|
|
|
|
if(abs(b) > 0 && passiveTorqueScale > SQRTEPSILON){
|
|
angleAtOneNormPassiveTorque =
|
|
(1/k)*log(abs(maxActiveIsometricTorque/b))
|
|
+ passiveCurveAngleOffset;
|
|
}else{
|
|
angleAtOneNormPassiveTorque =
|
|
std::numeric_limits<double>::signaling_NaN();
|
|
}
|
|
|
|
|
|
//angleAtOneNormPassiveTorque
|
|
//gymnastParams[Gymnast::PassiveAngleAtOneNormTorque]
|
|
|
|
|
|
} break;
|
|
case DataSet::Gymnast:
|
|
{
|
|
omegaMax = gymnastParams[
|
|
Gymnast::OmegaMax];
|
|
maxActiveIsometricTorque = gymnastParams[
|
|
Gymnast::TauMax];
|
|
angleAtOneNormActiveTorque = gymnastParams[
|
|
Gymnast::ActiveAngleAtOneNormTorque];
|
|
|
|
TorqueMuscleFunctionFactory::
|
|
createGaussianShapedActiveTorqueAngleCurve(
|
|
gymnastParams[Gymnast::ActiveAngleAtOneNormTorque],
|
|
gymnastParams[Gymnast::ActiveAngularStandardDeviation],
|
|
tempName.append("_taCurve"),
|
|
taCurve);
|
|
|
|
TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve(
|
|
gymnastParams[Gymnast::PassiveAngleAtZeroTorque],
|
|
gymnastParams[Gymnast::PassiveAngleAtOneNormTorque],
|
|
tempName.append("_tpCurve"),
|
|
tpCurve);
|
|
|
|
tpCurve.shift(passiveCurveAngleOffset,0);
|
|
|
|
TorqueMuscleFunctionFactory::createTorqueVelocityCurve(
|
|
gymnastParams[Gymnast::TvAtMaxEccentricVelocity],
|
|
gymnastParams[Gymnast::TvAtHalfMaxConcentricVelocity],
|
|
tempName.append("_tvCurve"),
|
|
tvCurve);
|
|
|
|
if(passiveTorqueScale > 0){
|
|
angleAtOneNormPassiveTorque =
|
|
gymnastParams[Gymnast::PassiveAngleAtOneNormTorque]
|
|
+ passiveCurveAngleOffset;
|
|
}else{
|
|
angleAtOneNormPassiveTorque =
|
|
std::numeric_limits<double>::signaling_NaN();
|
|
}
|
|
|
|
} break;
|
|
default:
|
|
{
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "Millard2016TorqueMuscle:"
|
|
<< muscleName
|
|
<< "dataSet has a value of " << dataSet
|
|
<< " which is not a valid choice";
|
|
assert(0);
|
|
abort();
|
|
}
|
|
};
|
|
|
|
|
|
|
|
//If the passiveScale is < 1 and > 0, then we must iterate to
|
|
//find the true value of angleAtOneNormPassiveTorque;
|
|
|
|
if(isfinite(angleAtOneNormPassiveTorque)
|
|
&& (passiveTorqueScale > 0 && passiveTorqueScale != 1.0) ){
|
|
int iter = 1;
|
|
int iterMax =100;
|
|
double err = 10*SQRTEPSILON;
|
|
double derr = 0;
|
|
double delta = 0;
|
|
|
|
while(abs(err) > SQRTEPSILON && iter < iterMax){
|
|
|
|
err = passiveTorqueScale*tpCurve.calcValue(angleAtOneNormPassiveTorque)
|
|
- 1.0;
|
|
derr = passiveTorqueScale
|
|
*tpCurve.calcDerivative(angleAtOneNormPassiveTorque,1);
|
|
|
|
delta = -err/derr;
|
|
angleAtOneNormPassiveTorque += delta;
|
|
++iterMax;
|
|
}
|
|
|
|
if(abs(err) > SQRTEPSILON){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "Millard2016TorqueMuscle:"
|
|
<< muscleName
|
|
<< "Internal Error: failed to solve for "
|
|
<<"angleAtOneNormPassiveTorque. Consult the maintainer of this "
|
|
<<"addon";
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
muscleCurvesAreDirty = false;
|
|
}
|
|
|
|
|
|
|
|
void Millard2016TorqueMuscle::printJointTorqueProfileToFile(
|
|
const std::string& path,
|
|
const std::string& fileNameWithoutExtension,
|
|
int numberOfSamplePoints)
|
|
{
|
|
if(muscleCurvesAreDirty){
|
|
updateTorqueMuscleCurves();
|
|
}
|
|
|
|
VectorNd activeDomain = taCurve.getCurveDomain();
|
|
VectorNd passiveDomain = tpCurve.getCurveDomain();
|
|
VectorNd velocityDomain= tvCurve.getCurveDomain();
|
|
|
|
double angleMin = activeDomain[0];
|
|
double angleMax = activeDomain[1];
|
|
|
|
if(tpCurve.calcValue(passiveDomain[0]) >= 0.99){
|
|
angleMin = passiveDomain[0];
|
|
}
|
|
|
|
if(tpCurve.calcValue(passiveDomain[1]) >= 0.99){
|
|
angleMax = passiveDomain[1];
|
|
}
|
|
|
|
double jointMin = signOfJointAngle*angleMin + angleOffset;
|
|
double jointMax = signOfJointAngle*angleMax + angleOffset;
|
|
|
|
if(jointMin > jointMax){
|
|
double tmp = jointMin;
|
|
jointMin=jointMax;
|
|
jointMax=tmp;
|
|
}
|
|
double range = jointMax-jointMin;
|
|
jointMin = jointMin -range*0.1;
|
|
jointMax = jointMax +range*0.1;
|
|
double jointDelta = (jointMax-jointMin)
|
|
/((double)numberOfSamplePoints-1.);
|
|
|
|
double velMin = omegaMax*signOfConcentricAnglularVelocity*velocityDomain[0];
|
|
double velMax = omegaMax*signOfConcentricAnglularVelocity*velocityDomain[1];
|
|
|
|
if(velMin > velMax){
|
|
double tmp = velMin;
|
|
velMin = velMax;
|
|
velMax = tmp;
|
|
}
|
|
double velRange = velMax-velMin;
|
|
velMin = velMin-0.1*velRange;
|
|
velMax = velMax+0.1*velRange;
|
|
double velDelta = (velMax-velMin)/((double)numberOfSamplePoints-1.0);
|
|
|
|
double angleAtMaxIsoTorque = angleAtOneNormActiveTorque;
|
|
|
|
std::vector< std::vector < double > > matrix;
|
|
std::vector < double > row(21);
|
|
std::string header("jointAngle,"
|
|
"jointVelocity,"
|
|
"activation,"
|
|
"fiberAngle,"
|
|
"fiberAngularVelocity,"
|
|
"passiveTorqueAngleMultiplier,"
|
|
"activeTorqueAngleMultiplier,"
|
|
"torqueVelocityMultiplier,"
|
|
"activeTorque,"
|
|
"passiveTorque,"
|
|
"fiberTorque,"
|
|
"jointTorque,"
|
|
"fiberStiffness,"
|
|
"jointStiffness,"
|
|
"fiberActivePower,"
|
|
"fiberPassivePower,"
|
|
"fiberPower,"
|
|
"jointPower,"
|
|
"DjointTorqueDactivation,"
|
|
"DjointTorqueDjointAngularVelocity,"
|
|
"DjointTorqueDjointAngle");
|
|
|
|
double activation =1.0;
|
|
double jointAngle = 0.;
|
|
double jointVelocity = 0.;
|
|
|
|
|
|
for(int i=0; i<numberOfSamplePoints; ++i){
|
|
jointAngle = jointMin + i*jointDelta;
|
|
jointVelocity = 0.;
|
|
|
|
calcTorqueMuscleInfo(jointAngle,
|
|
jointVelocity,
|
|
activation,
|
|
tmInfo);
|
|
|
|
row.at(0) = jointAngle;
|
|
row.at(1) = jointVelocity;
|
|
row.at(2) = activation;
|
|
|
|
row.at(3) = tmInfo.fiberAngle;
|
|
row.at(4) = tmInfo.fiberAngularVelocity;
|
|
row.at(5) = tmInfo.fiberPassiveTorqueAngleMultiplier;
|
|
row.at(6) = tmInfo.fiberActiveTorqueAngleMultiplier;
|
|
row.at(7) = tmInfo.fiberActiveTorqueAngularVelocityMultiplier;
|
|
row.at(8) = tmInfo.fiberActiveTorque;
|
|
row.at(9) = tmInfo.fiberPassiveTorque;
|
|
row.at(10)= tmInfo.fiberTorque;
|
|
row.at(11)= tmInfo.jointTorque;
|
|
row.at(12)= tmInfo.fiberStiffness;
|
|
row.at(13)= tmInfo.jointStiffness;
|
|
row.at(14)= tmInfo.fiberActivePower;
|
|
row.at(15)= tmInfo.fiberPassivePower;
|
|
row.at(16)= tmInfo.fiberPower;
|
|
row.at(17)= tmInfo.jointPower;
|
|
row.at(18)= tmInfo.DjointTorqueDactivation;
|
|
row.at(19)= tmInfo.DjointTorqueDjointAngularVelocity;
|
|
row.at(20)= tmInfo.DjointTorqueDjointAngle;
|
|
|
|
matrix.push_back(row);
|
|
}
|
|
|
|
std::string fullFilePath = path;
|
|
if(!path.empty()){
|
|
fullFilePath.append("/");
|
|
}
|
|
|
|
|
|
fullFilePath.append(fileNameWithoutExtension);
|
|
fullFilePath.append("_variableLengthfixedVelocity");
|
|
fullFilePath.append(".csv");
|
|
printMatrixToFile(matrix,header,fullFilePath);
|
|
|
|
matrix.clear();
|
|
|
|
|
|
for(int i=0; i<numberOfSamplePoints; ++i){
|
|
|
|
jointAngle = angleAtMaxIsoTorque;
|
|
jointVelocity = velMin + i*velDelta;
|
|
|
|
calcTorqueMuscleInfo(jointAngle,
|
|
jointVelocity,
|
|
activation,
|
|
tmInfo);
|
|
|
|
row.at(0) = jointAngle;
|
|
row.at(1) = jointVelocity;
|
|
row.at(2) = activation;
|
|
|
|
row.at(3) = tmInfo.fiberAngle;
|
|
row.at(4) = tmInfo.fiberAngularVelocity;
|
|
row.at(5) = tmInfo.fiberPassiveTorqueAngleMultiplier;
|
|
row.at(6) = tmInfo.fiberActiveTorqueAngleMultiplier;
|
|
row.at(7) = tmInfo.fiberActiveTorqueAngularVelocityMultiplier;
|
|
row.at(8) = tmInfo.fiberActiveTorque;
|
|
row.at(9) = tmInfo.fiberPassiveTorque;
|
|
row.at(10)= tmInfo.fiberTorque;
|
|
row.at(11)= tmInfo.jointTorque;
|
|
row.at(12)= tmInfo.fiberStiffness;
|
|
row.at(13)= tmInfo.jointStiffness;
|
|
row.at(14)= tmInfo.fiberActivePower;
|
|
row.at(15)= tmInfo.fiberPassivePower;
|
|
row.at(16)= tmInfo.fiberPower;
|
|
row.at(17)= tmInfo.jointPower;
|
|
row.at(18)= tmInfo.DjointTorqueDactivation;
|
|
row.at(19)= tmInfo.DjointTorqueDjointAngularVelocity;
|
|
row.at(20)= tmInfo.DjointTorqueDjointAngle;
|
|
|
|
matrix.push_back(row);
|
|
}
|
|
|
|
fullFilePath = path;
|
|
if(!path.empty()){
|
|
fullFilePath.append("/");
|
|
}
|
|
fullFilePath.append(fileNameWithoutExtension);
|
|
fullFilePath.append("_fixedLengthVariableVelocity");
|
|
fullFilePath.append(".csv");
|
|
printMatrixToFile(matrix,header,fullFilePath);
|
|
|
|
matrix.clear();
|
|
|
|
}
|
|
|
|
|
|
|