2478 lines
90 KiB
C++
Executable File
2478 lines
90 KiB
C++
Executable File
//==============================================================================
|
|
/*
|
|
* 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;
|
|
|
|
const double Millard2016TorqueMuscle::mTaLambdaMax = 1.0;
|
|
const double Millard2016TorqueMuscle::mTpLambdaMax = 0.0;
|
|
//static double mTvLambdaMax = 1.0;
|
|
|
|
|
|
/*************************************************************
|
|
Table Access Structure Names
|
|
*************************************************************/
|
|
const double Gravity = 9.81; //Needed for the strength scaling used
|
|
//by Anderson et al. And this value has to
|
|
//be equal to the gravity parameter used by
|
|
//Anderson et al.: it should not be changed if
|
|
//the force of gravity differs in the model!
|
|
|
|
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. mAngleAtOneNormPassiveTorque 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, 7.98695, 1.5506, 1.14543, 1.1, 0.150907, 0.306223, 1.35342 },
|
|
{6,1,0,0,211.65 , 19.2310, 0, 6.28319, 1.1, 0.150907, 0, -0.785398 }};
|
|
|
|
|
|
/*
|
|
Original lumbar parameters
|
|
{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( )
|
|
:mAngleOffset(1.0),
|
|
mSignOfJointAngle(1.0),
|
|
mSignOfJointTorque(1.0),
|
|
mSignOfConcentricAnglularVelocity(1.0),
|
|
mMuscleName("empty")
|
|
{
|
|
mMuscleCurvesAreDirty = true;
|
|
mUseTabularMaxActiveIsometricTorque = true;
|
|
mUseTabularOmegaMax = true;
|
|
mPassiveTorqueScale = 1.0;
|
|
|
|
mTaLambda = 0.0;
|
|
mTpLambda = 0.0;
|
|
mTvLambda = 0.0;
|
|
mTvLambdaMax = 1.0;
|
|
mTaAngleScaling = 1.0;
|
|
}
|
|
|
|
Millard2016TorqueMuscle::Millard2016TorqueMuscle(
|
|
DataSet::item dataSet,
|
|
const SubjectInformation &subjectInfo,
|
|
int jointTorque,
|
|
double jointAngleOffsetRelativeToDoxygenFigures,
|
|
double signOfJointAngleRelativeToDoxygenFigures,
|
|
double mSignOfJointTorque,
|
|
const std::string& name
|
|
):mAngleOffset(jointAngleOffsetRelativeToDoxygenFigures),
|
|
mSignOfJointAngle(signOfJointAngleRelativeToDoxygenFigures),
|
|
mSignOfJointTorque(mSignOfJointTorque),
|
|
mSignOfConcentricAnglularVelocity(mSignOfJointTorque),
|
|
mMuscleName(name),
|
|
mDataSet(dataSet)
|
|
{
|
|
|
|
mSubjectHeightInMeters = subjectInfo.heightInMeters;
|
|
mSubjectMassInKg = subjectInfo.massInKg;
|
|
mPassiveCurveAngleOffset = 0.;
|
|
mPassiveTorqueScale = 1.0;
|
|
mBetaMax = 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:"
|
|
<< mMuscleName
|
|
<< ": A jointTorqueDirection of " << jointTorque
|
|
<< " does not exist.";
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( abs(abs(mSignOfJointAngle)-1) > EPSILON){
|
|
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "Millard2016TorqueMuscle:"
|
|
<< mMuscleName
|
|
<< ": signOfJointAngleRelativeToAnderson2007 must be [-1,1] not "
|
|
<< mSignOfJointAngle;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( abs(abs(mSignOfConcentricAnglularVelocity)-1) > EPSILON){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "Millard2016TorqueMuscle:"
|
|
<< mMuscleName
|
|
<< ": signOfJointAngularVelocityDuringConcentricContraction "
|
|
<< "must be [-1,1] not "
|
|
<< mSignOfConcentricAnglularVelocity;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( abs(abs(mSignOfJointTorque)-1) > EPSILON){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "Millard2016TorqueMuscle:"
|
|
<< mMuscleName
|
|
<< ": mSignOfJointTorque must be [-1,1] not "
|
|
<< mSignOfJointTorque;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
|
|
if(mSubjectHeightInMeters <= 0){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "Millard2016TorqueMuscle:"
|
|
<< mMuscleName
|
|
<< ": mSubjectHeightInMeters > 0, but it's "
|
|
<< mSubjectHeightInMeters;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if(mSubjectMassInKg <= 0){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "Millard2016TorqueMuscle:"
|
|
<< mMuscleName
|
|
<< ": mSubjectMassInKg > 0, but it's "
|
|
<< mSubjectMassInKg;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
|
|
|
|
int idx = -1;
|
|
int jointIdx = 0;
|
|
int dirIdx = 1;
|
|
int genderIdx = 2;
|
|
int ageIdx = 3;
|
|
|
|
switch(mDataSet){
|
|
case DataSet::Anderson2007:
|
|
{
|
|
mAnderson2007c1c2c3c4c5c6.resize(6);
|
|
mAnderson2007b1k1b2k2.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){
|
|
mAnderson2007c1c2c3c4c5c6[i] = Anderson2007Table3Mean[idx][i+4];
|
|
}
|
|
for(int i=0; i<4; ++i){
|
|
mAnderson2007b1k1b2k2[i] = Anderson2007Table3Mean[idx][i+10];
|
|
}
|
|
}
|
|
|
|
} break;
|
|
|
|
case DataSet::Gymnast:
|
|
{
|
|
mGymnastParams.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){
|
|
mGymnastParams[i] = GymnastWholeBody[idx][i+4];
|
|
}
|
|
}
|
|
} break;
|
|
|
|
default:
|
|
{
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "Millard2016TorqueMuscle:"
|
|
<< mMuscleName
|
|
<< "The requested DataSet does not exist.";
|
|
assert(0);
|
|
abort();
|
|
}
|
|
};
|
|
|
|
|
|
|
|
if(idx == -1){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "Millard2016TorqueMuscle:"
|
|
<< mMuscleName
|
|
<< "The combination of data set (" << mDataSet << ")"
|
|
<< " joint (" << joint << ")"
|
|
<< " joint direction (" << jointDirection << ")"
|
|
<< " gender, (" << gender << ")"
|
|
<< "and age " << ageGroup << ")"
|
|
<< "could not be found";
|
|
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
|
|
|
|
mMuscleCurvesAreDirty = true;
|
|
mUseTabularMaxActiveIsometricTorque = true;
|
|
mUseTabularOmegaMax = true;
|
|
mUseTabularTorqueVelocityMultiplierAtHalfOmegaMax = true;
|
|
mTaLambda = 0.0;
|
|
mTpLambda = 0.0;
|
|
mTvLambda = 0.0;
|
|
mTaAngleScaling = 1.0;
|
|
updateTorqueMuscleCurves();
|
|
|
|
|
|
#ifdef RBDL_BUILD_ADDON_MUSCLE_FITTING
|
|
mTmInfo.fittingInfo.resize(3);
|
|
/*
|
|
[0]: D^2 jointTorque / D lambdaA ^2
|
|
[1]: D^2 jointTorque / D lambdaV ^2
|
|
[2]: D^2 jointTorque / D lambdaA D lambdaV
|
|
*/
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
/*************************************************************
|
|
Muscle Model Code
|
|
*************************************************************/
|
|
/*
|
|
To explain the const casting:
|
|
1. calcJointTorque does not modify any member variables which affect
|
|
the output fo the model, and thus it should be const for the user.
|
|
|
|
2. To make the code maintainable the model is evaluated only in 2
|
|
places updTorqueMuscleSummary & updTorqueMuscleInfo.
|
|
|
|
3. To save on memory, these upd functions modify a struct, which is a member
|
|
variable, but is only being used as cache.
|
|
|
|
To achieve a const calcJointTorque function, maintainable code (e.g. model
|
|
evaluated in only 2 spots), and with minimal use of memory/copying I am
|
|
using const_casting.
|
|
|
|
There is perhaps a more elegant way to do this. I dislike
|
|
const_casting, but for now I do not see a better solution.
|
|
*/
|
|
double Millard2016TorqueMuscle::
|
|
calcJointTorque( double jointAngle,
|
|
double jointAngularVelocity,
|
|
double activation) const
|
|
{
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
updTorqueMuscleSummary(activation,
|
|
jointAngle,jointAngularVelocity,
|
|
mTaLambda,mTpLambda,mTvLambda,
|
|
mTaAngleScaling, mAngleAtOneNormActiveTorque,
|
|
mPassiveCurveAngleOffset,
|
|
mOmegaMax,
|
|
mMaxActiveIsometricTorque,
|
|
mutableThis->mTmSummary);
|
|
|
|
return mTmSummary.jointTorque;
|
|
}
|
|
|
|
|
|
|
|
void Millard2016TorqueMuscle::
|
|
calcActivation(double jointAngle,
|
|
double jointAngularVelocity,
|
|
double jointTorque,
|
|
TorqueMuscleSummary &tms) const
|
|
{
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
|
|
updInvertTorqueMuscleSummary(jointTorque,jointAngle,jointAngularVelocity,
|
|
mTaLambda,mTpLambda,mTvLambda,
|
|
mTaAngleScaling,
|
|
mAngleAtOneNormActiveTorque,
|
|
mPassiveCurveAngleOffset,
|
|
mOmegaMax,
|
|
mMaxActiveIsometricTorque,
|
|
tms);
|
|
}
|
|
|
|
double Millard2016TorqueMuscle::
|
|
calcMaximumActiveIsometricTorqueScalingFactor(
|
|
double jointAngle,
|
|
double jointAngularVelocity,
|
|
double activation,
|
|
double jointTorque) const
|
|
{
|
|
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
|
|
double scaleFactor = 0.;
|
|
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
|
|
updTorqueMuscleSummary(activation,
|
|
jointAngle,jointAngularVelocity,
|
|
mTaLambda,mTpLambda,mTvLambda,
|
|
mTaAngleScaling,
|
|
mAngleAtOneNormActiveTorque,
|
|
mPassiveCurveAngleOffset,
|
|
mOmegaMax,
|
|
mMaxActiveIsometricTorque,
|
|
mutableThis->mTmSummary);
|
|
|
|
scaleFactor = jointTorque/mTmSummary.jointTorque;
|
|
|
|
return scaleFactor;
|
|
}
|
|
|
|
|
|
void Millard2016TorqueMuscle::
|
|
calcTorqueMuscleInfo(double jointAngle,
|
|
double jointAngularVelocity,
|
|
double activation,
|
|
TorqueMuscleInfo& tmi) const
|
|
{
|
|
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
|
|
|
|
updTorqueMuscleInfo(activation, jointAngle, jointAngularVelocity,
|
|
mTaLambda,mTpLambda,mTvLambda,
|
|
mTaAngleScaling, mAngleAtOneNormActiveTorque,
|
|
mPassiveCurveAngleOffset,
|
|
mOmegaMax,
|
|
mMaxActiveIsometricTorque,
|
|
tmi);
|
|
}
|
|
|
|
|
|
/*************************************************************
|
|
Get / Set Functions
|
|
*************************************************************/
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getJointTorqueSign() const
|
|
{
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
return mSignOfJointTorque;
|
|
}
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getJointAngleSign() const
|
|
{
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
return mSignOfJointAngle;
|
|
}
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getJointAngleOffset() const
|
|
{
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
return mAngleOffset;
|
|
}
|
|
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getNormalizedDampingCoefficient() const
|
|
{
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
return mBetaMax;
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::
|
|
setNormalizedDampingCoefficient(double betaUpd)
|
|
{
|
|
if(betaUpd < 0){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "setNormalizedDampingCoefficient:"
|
|
<< mMuscleName
|
|
<< "mBetaMax is " << betaUpd
|
|
<< " but mBetaMax must be > 0 "
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
mBetaMax = betaUpd;
|
|
}
|
|
|
|
|
|
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getMaximumActiveIsometricTorque() const
|
|
{
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
|
|
/*
|
|
if(mUseTabularMaxActiveIsometricTorque){
|
|
return mMaxActiveIsometricTorque;
|
|
}else{
|
|
return mMaxActiveIsometricTorque*mMaxActiveIsometricMultiplerProduct;
|
|
}
|
|
*/
|
|
return mMaxActiveIsometricTorque;
|
|
|
|
}
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getMaximumConcentricJointAngularVelocity() const
|
|
{
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
return calcJointAngularVelocity( mOmegaMax );
|
|
}
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getTorqueVelocityMultiplierAtHalfOmegaMax() const
|
|
{
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
return mTorqueVelocityMultiplierAtHalfOmegaMax;
|
|
}
|
|
|
|
|
|
void Millard2016TorqueMuscle::
|
|
setTorqueVelocityMultiplierAtHalfOmegaMax(double tvAtHalfOmegaMax)
|
|
{
|
|
if(mDataSet == DataSet::Anderson2007){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "setTorqueVelocityMultiplierAtHalfOmegaMax:"
|
|
<< mMuscleName
|
|
<< " This function is only compatible with the Gymnast dataset"
|
|
<< " but this muscle is from the Anderson2007 dataset. Switch"
|
|
<< " data sets or stop using this function.";
|
|
assert(0);
|
|
abort();
|
|
}
|
|
mMuscleCurvesAreDirty = true;
|
|
mUseTabularTorqueVelocityMultiplierAtHalfOmegaMax = false;
|
|
mTorqueVelocityMultiplierAtHalfOmegaMax = tvAtHalfOmegaMax;
|
|
}
|
|
|
|
|
|
void Millard2016TorqueMuscle::
|
|
setMaximumActiveIsometricTorque(double maxIsoTorque)
|
|
{
|
|
mMuscleCurvesAreDirty = true;
|
|
mUseTabularMaxActiveIsometricTorque = false;
|
|
//mMaxActiveIsometricTorqueUserDefined = maxIsoTorque;
|
|
mMaxActiveIsometricTorque = maxIsoTorque;
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::
|
|
setMaximumConcentricJointAngularVelocity(double maxAngularVelocity){
|
|
if(fabs(maxAngularVelocity) < SQRTEPSILON){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "setMaximumJointAngularVelocity:"
|
|
<< mMuscleName
|
|
<< " The value of maxJointAngularVelocity needs to be greater "
|
|
" than sqrt(epsilon), but it is "
|
|
<< maxAngularVelocity;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
mMuscleCurvesAreDirty = true;
|
|
mUseTabularOmegaMax = false;
|
|
mOmegaMax = fabs(maxAngularVelocity);
|
|
}
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getJointAngleAtMaximumActiveIsometricTorque() const
|
|
{
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
return calcJointAngle(mAngleAtOneNormActiveTorque);
|
|
}
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getActiveTorqueAngleCurveWidth() const
|
|
{
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
VectorNd domain = mTaCurve.getCurveDomain();
|
|
double activeTorqueAngleAngleScaling
|
|
= getActiveTorqueAngleCurveAngleScaling();
|
|
double width = fabs(domain[1]-domain[0])/activeTorqueAngleAngleScaling;
|
|
|
|
return width;
|
|
|
|
}
|
|
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getJointAngleAtOneNormalizedPassiveIsometricTorque() const
|
|
{
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
return calcJointAngle(mAngleAtOneNormPassiveTorque);
|
|
}
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getJointAngleAtSmallestNormalizedPassiveIsometricTorque() const
|
|
{
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
return calcJointAngle(mAngleAtSmallestNormPassiveTorque);
|
|
}
|
|
|
|
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getPassiveTorqueScale() const
|
|
{
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
return mPassiveTorqueScale;
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::
|
|
setPassiveTorqueScale(double passiveTorqueScaling)
|
|
{
|
|
mMuscleCurvesAreDirty = true;
|
|
mPassiveTorqueScale = passiveTorqueScaling;
|
|
}
|
|
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getPassiveCurveAngleOffset() const
|
|
{
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
return mPassiveCurveAngleOffset;
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::
|
|
setPassiveCurveAngleOffset(double passiveCurveAngleOffsetVal)
|
|
{
|
|
mMuscleCurvesAreDirty = true;
|
|
mPassiveCurveAngleOffset = passiveCurveAngleOffsetVal;
|
|
}
|
|
|
|
|
|
void Millard2016TorqueMuscle::
|
|
fitPassiveCurveAngleOffset(double jointAngleTarget,
|
|
double passiveFiberTorqueTarget)
|
|
{
|
|
mMuscleCurvesAreDirty = true;
|
|
setPassiveCurveAngleOffset(0.0);
|
|
updateTorqueMuscleCurves();
|
|
|
|
if(passiveFiberTorqueTarget < SQRTEPSILON){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "fitPassiveTorqueScale:"
|
|
<< mMuscleName
|
|
<< ": passiveTorque " << passiveFiberTorqueTarget
|
|
<< " 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 normPassiveFiberTorque = passiveFiberTorqueTarget
|
|
/mMaxActiveIsometricTorque;
|
|
double currentFiberAngle = calcFiberAngleGivenNormalizedPassiveTorque(
|
|
normPassiveFiberTorque,
|
|
mTpLambda,
|
|
mPassiveCurveAngleOffset); //0.);
|
|
|
|
//double fiberAngleOffset = mPassiveCurveAngleOffset
|
|
// +calcFiberAngle(jointAngleTarget)
|
|
// -currentFiberAngle;
|
|
double fiberAngleOffset = calcFiberAngle(jointAngleTarget)
|
|
-currentFiberAngle;
|
|
|
|
setPassiveCurveAngleOffset(fiberAngleOffset);
|
|
updateTorqueMuscleCurves();
|
|
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::
|
|
fitPassiveTorqueScale(double jointAngleTarget,
|
|
double passiveFiberTorqueTarget)
|
|
{
|
|
mMuscleCurvesAreDirty = true;
|
|
setPassiveTorqueScale(1.0);
|
|
updateTorqueMuscleCurves();
|
|
|
|
double normPassiveFiberTorqueTarget = passiveFiberTorqueTarget
|
|
/mMaxActiveIsometricTorque;
|
|
double fiberAngle = calcFiberAngle(jointAngleTarget);
|
|
double normPassiveFiberTorqueCurrent=
|
|
calcBlendedCurveDerivative(fiberAngle-mPassiveCurveAngleOffset,
|
|
mTpLambda, mTpLambdaMax,
|
|
0,0,
|
|
mTpCurve);
|
|
|
|
double passiveTorqueScale = normPassiveFiberTorqueTarget
|
|
/normPassiveFiberTorqueCurrent;
|
|
|
|
|
|
setPassiveTorqueScale(passiveTorqueScale);
|
|
updateTorqueMuscleCurves();
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::calcTorqueMuscleDataFeatures(
|
|
RigidBodyDynamics::Math::VectorNd const &jointTorque,
|
|
RigidBodyDynamics::Math::VectorNd const &jointAngle,
|
|
RigidBodyDynamics::Math::VectorNd const &jointAngularVelocity,
|
|
double activeTorqueAngleBlendingVariable,
|
|
double passiveTorqueAngleBlendingVariable,
|
|
double torqueVelocityBlendingVariable,
|
|
double activeTorqueAngleAngleScaling,
|
|
double activeTorqueAngleAtOneNormTorque,
|
|
double passiveTorqueAngleCurveOffset,
|
|
double maxAngularVelocity,
|
|
double maxActiveIsometricTorque,
|
|
TorqueMuscleDataFeatures &tmf) const
|
|
{
|
|
TorqueMuscleSummary tmsCache;
|
|
double activation;
|
|
double tp;
|
|
double minActivation = 1.0;
|
|
double maxActivation = 0.;
|
|
double maxTp = 0.;
|
|
|
|
tmf.isInactive = true;
|
|
|
|
for(unsigned int i=0; i<jointAngle.rows();++i){
|
|
|
|
updInvertTorqueMuscleSummary(jointTorque[i],
|
|
jointAngle[i],
|
|
jointAngularVelocity[i],
|
|
activeTorqueAngleBlendingVariable,
|
|
passiveTorqueAngleBlendingVariable,
|
|
torqueVelocityBlendingVariable,
|
|
activeTorqueAngleAngleScaling,
|
|
activeTorqueAngleAtOneNormTorque,
|
|
passiveTorqueAngleCurveOffset,
|
|
maxAngularVelocity,
|
|
maxActiveIsometricTorque,
|
|
tmsCache);
|
|
activation = tmsCache.activation;
|
|
tp = tmsCache.fiberPassiveTorqueAngleMultiplier;
|
|
if(mSignOfJointTorque*jointTorque[i] > 0){
|
|
tmf.isInactive=false;
|
|
if(activation <= minActivation){
|
|
minActivation = activation;
|
|
tmf.indexOfMinActivation = i;
|
|
tmf.summaryAtMinActivation = tmsCache;
|
|
}
|
|
if(activation >= maxActivation){
|
|
tmf.indexOfMaxActivation = i;
|
|
tmf.summaryAtMaxActivation = tmsCache;
|
|
maxActivation = activation;
|
|
}
|
|
}
|
|
if(tmsCache.fiberPassiveTorqueAngleMultiplier >= maxTp){
|
|
maxTp = tmsCache.fiberPassiveTorqueAngleMultiplier;
|
|
tmf.summaryAtMaxPassiveTorqueAngleMultiplier = tmsCache;
|
|
tmf.indexOfMaxPassiveTorqueAngleMultiplier = i;
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
const SmoothSegmentedFunction& Millard2016TorqueMuscle::
|
|
getActiveTorqueAngleCurve() const
|
|
{
|
|
//This must be updated if the parameters have changed
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
return mTaCurve;
|
|
}
|
|
|
|
const SmoothSegmentedFunction& Millard2016TorqueMuscle::
|
|
getPassiveTorqueAngleCurve() const
|
|
{
|
|
//This must be updated if the parameters have changed
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
return mTpCurve;
|
|
}
|
|
|
|
const SmoothSegmentedFunction& Millard2016TorqueMuscle::
|
|
getTorqueAngularVelocityCurve() const
|
|
{
|
|
//This must be updated if the parameters have changed
|
|
if(mMuscleCurvesAreDirty){
|
|
Millard2016TorqueMuscle* mutableThis =
|
|
const_cast<Millard2016TorqueMuscle* >(this);
|
|
mutableThis->updateTorqueMuscleCurves();
|
|
}
|
|
return mTvCurve;
|
|
}
|
|
|
|
|
|
|
|
string Millard2016TorqueMuscle::getName(){
|
|
return mMuscleName;
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::setName(string &name){
|
|
mMuscleCurvesAreDirty = true;
|
|
mMuscleName = name;
|
|
}
|
|
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getActiveTorqueAngleCurveBlendingVariable() const
|
|
{
|
|
return mTaLambda;
|
|
}
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getPassiveTorqueAngleCurveBlendingVariable() const
|
|
{
|
|
return mTpLambda;
|
|
}
|
|
|
|
double Millard2016TorqueMuscle::
|
|
getTorqueAngularVelocityCurveBlendingVariable() const
|
|
{
|
|
return mTvLambda;
|
|
}
|
|
|
|
double Millard2016TorqueMuscle::getActiveTorqueAngleCurveAngleScaling() const
|
|
{
|
|
return mTaAngleScaling;
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::
|
|
setActiveTorqueAngleCurveAngleScaling(double angleScaling)
|
|
{
|
|
if(angleScaling < SQRTEPSILON){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "setActiveTorqueAngleCurveAngleScaling:"
|
|
<< mMuscleName
|
|
<< ": angleScaling must be > sqrt(eps), this "
|
|
<< angleScaling
|
|
<<" is outside the acceptable range."
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
mMuscleCurvesAreDirty = true;
|
|
mTaAngleScaling = angleScaling;
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::
|
|
setActiveTorqueAngleCurveBlendingVariable(double blendingVariable)
|
|
{
|
|
if(blendingVariable < 0. || blendingVariable > 1.0){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "setActiveTorqueAngleCurveBlendingVariable:"
|
|
<< mMuscleName
|
|
<< ": blending variable must be [0,1] and this "
|
|
<< blendingVariable
|
|
<< " is outside the acceptable range."
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
mMuscleCurvesAreDirty = true;
|
|
mTaLambda = blendingVariable;
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::
|
|
setPassiveTorqueAngleCurveBlendingVariable(double blendingVariable)
|
|
{
|
|
if(blendingVariable < 0. || blendingVariable > 1.0){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "setPassiveTorqueAngleCurveBlendingVariable:"
|
|
<< mMuscleName
|
|
<< ": blending variable must be [0,1] and this "
|
|
<< blendingVariable
|
|
<< " is outside the acceptable range."
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
mMuscleCurvesAreDirty = true;
|
|
mTpLambda = blendingVariable;
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::
|
|
setTorqueAngularVelocityCurveBlendingVariable(double blendingVariable)
|
|
{
|
|
|
|
if(blendingVariable < 0. || blendingVariable > 1.0){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "setTorqueAngularVelocityCurveBlendingVariable:"
|
|
<< mMuscleName
|
|
<< ": blending variable must be [0,1] and this "
|
|
<< blendingVariable
|
|
<< " is outside the acceptable range."
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
mMuscleCurvesAreDirty = true;
|
|
mTvLambda = blendingVariable;
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::setFittedParameters(
|
|
const TorqueMuscleParameterFittingData &fittedParameters)
|
|
{
|
|
|
|
if(!fittedParameters.fittingConverged){
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "setTorqueMuscleParameters:"
|
|
<< mMuscleName
|
|
<< ": The fittingConverged field of fittedParameters is false! "
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
setPassiveTorqueAngleCurveBlendingVariable(
|
|
fittedParameters.passiveTorqueAngleBlendingVariable);
|
|
setActiveTorqueAngleCurveBlendingVariable(
|
|
fittedParameters.activeTorqueAngleBlendingVariable);
|
|
setTorqueAngularVelocityCurveBlendingVariable(
|
|
fittedParameters.torqueVelocityBlendingVariable);
|
|
setPassiveCurveAngleOffset(
|
|
fittedParameters.passiveTorqueAngleCurveOffset);
|
|
setMaximumActiveIsometricTorque(
|
|
fittedParameters.maximumActiveIsometricTorque);
|
|
setMaximumConcentricJointAngularVelocity(
|
|
fittedParameters.maximumAngularVelocity);
|
|
setActiveTorqueAngleCurveAngleScaling(
|
|
fittedParameters.activeTorqueAngleAngleScaling);
|
|
|
|
}
|
|
|
|
/*************************************************************
|
|
Utilities
|
|
*************************************************************/
|
|
//fa = signJointAngle*(ja - jaO)
|
|
// ja = signJointAngle*fa + ja0
|
|
//dja = signJointAngle*dfa
|
|
|
|
double Millard2016TorqueMuscle::
|
|
calcFiberAngle(double jointAngle) const
|
|
{
|
|
return mSignOfJointAngle*(jointAngle-mAngleOffset);
|
|
}
|
|
|
|
double Millard2016TorqueMuscle::
|
|
calcJointAngle(double fiberAngle) const
|
|
{
|
|
return fiberAngle*mSignOfJointAngle + mAngleOffset;
|
|
}
|
|
|
|
|
|
double Millard2016TorqueMuscle::
|
|
calcFiberAngularVelocity(double jointAngularVelocity) const
|
|
{
|
|
return mSignOfConcentricAnglularVelocity*jointAngularVelocity;
|
|
}
|
|
|
|
double Millard2016TorqueMuscle::
|
|
calcJointAngularVelocity(double fiberAngularVelocity) const
|
|
{
|
|
return mSignOfConcentricAnglularVelocity*fiberAngularVelocity;
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::updateTorqueMuscleCurves()
|
|
{
|
|
std::string tempName = mMuscleName;
|
|
|
|
switch(mDataSet){
|
|
case DataSet::Anderson2007:
|
|
{
|
|
double c4 = mAnderson2007c1c2c3c4c5c6[3];
|
|
double c5 = mAnderson2007c1c2c3c4c5c6[4];
|
|
|
|
if(mUseTabularOmegaMax){
|
|
mOmegaMax = abs( 2.0*c4*c5/(c5-3.0*c4) );
|
|
}
|
|
|
|
mScaleFactorAnderson2007 = mSubjectHeightInMeters
|
|
*mSubjectMassInKg
|
|
*Gravity;
|
|
|
|
if(mUseTabularMaxActiveIsometricTorque){
|
|
mMaxActiveIsometricTorque = mScaleFactorAnderson2007
|
|
*mAnderson2007c1c2c3c4c5c6[0];
|
|
}
|
|
|
|
mAngleAtOneNormActiveTorque = mAnderson2007c1c2c3c4c5c6[2];
|
|
|
|
TorqueMuscleFunctionFactory::
|
|
createAnderson2007ActiveTorqueAngleCurve(
|
|
mAnderson2007c1c2c3c4c5c6[1],
|
|
mAnderson2007c1c2c3c4c5c6[2],
|
|
tempName.append("_taCurve"),
|
|
mTaCurve);
|
|
|
|
tempName = mMuscleName;
|
|
|
|
TorqueMuscleFunctionFactory::
|
|
createAnderson2007ActiveTorqueVelocityCurve(
|
|
mAnderson2007c1c2c3c4c5c6[3],
|
|
mAnderson2007c1c2c3c4c5c6[4],
|
|
mAnderson2007c1c2c3c4c5c6[5],
|
|
1.1,
|
|
1.4,
|
|
tempName.append("_tvCurve"),
|
|
mTvCurve);
|
|
|
|
mTorqueVelocityMultiplierAtHalfOmegaMax = mTvCurve.calcValue(0.5);
|
|
|
|
tempName = mMuscleName;
|
|
|
|
double normMaxActiveIsometricTorque = mMaxActiveIsometricTorque
|
|
/mScaleFactorAnderson2007;
|
|
|
|
TorqueMuscleFunctionFactory::
|
|
createAnderson2007PassiveTorqueAngleCurve(
|
|
mScaleFactorAnderson2007,
|
|
normMaxActiveIsometricTorque,
|
|
mAnderson2007b1k1b2k2[0],
|
|
mAnderson2007b1k1b2k2[1],
|
|
mAnderson2007b1k1b2k2[2],
|
|
mAnderson2007b1k1b2k2[3],
|
|
tempName.append("_tpCurve"),
|
|
mTpCurve);
|
|
|
|
//mTpCurve.shift(mPassiveCurveAngleOffset,0);
|
|
mTpCurve.scale(1.0,mPassiveTorqueScale);
|
|
|
|
double k = 0;
|
|
double b = 0;
|
|
|
|
if(mAnderson2007b1k1b2k2[0] > 0){
|
|
b = mAnderson2007b1k1b2k2[0];
|
|
k = mAnderson2007b1k1b2k2[1];
|
|
}else if(mAnderson2007b1k1b2k2[2] > 0){
|
|
b = mAnderson2007b1k1b2k2[2];
|
|
k = mAnderson2007b1k1b2k2[3];
|
|
}
|
|
|
|
|
|
VectorNd xDomain = mTpCurve.getCurveDomain();
|
|
double tpAtX0 = mTpCurve.calcValue(xDomain[0]);
|
|
double tpAtX1 = mTpCurve.calcValue(xDomain[1]);
|
|
|
|
if(fabs(tpAtX0) < SQRTEPSILON){
|
|
mAngleAtSmallestNormPassiveTorque = xDomain[0];
|
|
}else if(fabs(tpAtX1) < SQRTEPSILON){
|
|
mAngleAtSmallestNormPassiveTorque = xDomain[1];
|
|
}else{
|
|
mAngleAtSmallestNormPassiveTorque =
|
|
std::numeric_limits<double>::signaling_NaN();
|
|
}
|
|
|
|
if( fabs(tpAtX0) > SQRTEPSILON
|
|
|| fabs(tpAtX1) > SQRTEPSILON){
|
|
double argGuess = (1/k)*log(abs(mMaxActiveIsometricTorque/b));
|
|
// + mPassiveCurveAngleOffset;
|
|
mAngleAtOneNormPassiveTorque = calcInverseBlendedCurveValue(1.0,
|
|
argGuess,
|
|
mTpLambda,
|
|
mTpLambdaMax,
|
|
mTpCurve);
|
|
mAngleAtOneNormPassiveTorque += mPassiveCurveAngleOffset;
|
|
}else{
|
|
mAngleAtOneNormPassiveTorque =
|
|
std::numeric_limits<double>::signaling_NaN();
|
|
}
|
|
|
|
|
|
//mAngleAtOneNormPassiveTorque
|
|
//mGymnastParams[Gymnast::PassiveAngleAtOneNormTorque]
|
|
|
|
|
|
} break;
|
|
case DataSet::Gymnast:
|
|
{
|
|
if(mUseTabularOmegaMax){
|
|
mOmegaMax = mGymnastParams[
|
|
Gymnast::OmegaMax];
|
|
}
|
|
if(mUseTabularMaxActiveIsometricTorque){
|
|
mMaxActiveIsometricTorque = mGymnastParams[
|
|
Gymnast::TauMax];
|
|
}
|
|
mAngleAtOneNormActiveTorque = mGymnastParams[
|
|
Gymnast::ActiveAngleAtOneNormTorque];
|
|
|
|
TorqueMuscleFunctionFactory::
|
|
createGaussianShapedActiveTorqueAngleCurve(
|
|
mGymnastParams[Gymnast::ActiveAngleAtOneNormTorque],
|
|
mGymnastParams[Gymnast::ActiveAngularStandardDeviation],
|
|
tempName.append("_taCurve"),
|
|
mTaCurve);
|
|
|
|
TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve(
|
|
mGymnastParams[Gymnast::PassiveAngleAtZeroTorque],
|
|
mGymnastParams[Gymnast::PassiveAngleAtOneNormTorque],
|
|
tempName.append("_tpCurve"),
|
|
mTpCurve);
|
|
|
|
//mTpCurve.shift(mPassiveCurveAngleOffset,0);
|
|
mTpCurve.scale(1.0,mPassiveTorqueScale);
|
|
|
|
|
|
|
|
if(mUseTabularTorqueVelocityMultiplierAtHalfOmegaMax){
|
|
mTorqueVelocityMultiplierAtHalfOmegaMax
|
|
= mGymnastParams[Gymnast::TvAtHalfMaxConcentricVelocity];
|
|
}
|
|
|
|
TorqueMuscleFunctionFactory::createTorqueVelocityCurve(
|
|
mGymnastParams[Gymnast::TvAtMaxEccentricVelocity],
|
|
mTorqueVelocityMultiplierAtHalfOmegaMax,
|
|
tempName.append("_tvCurve"),
|
|
mTvCurve);
|
|
|
|
|
|
//If the passive curve is non-zero, recalculate the angle at which
|
|
//it hits one normalized force.
|
|
mAngleAtSmallestNormPassiveTorque =
|
|
mGymnastParams[Gymnast::PassiveAngleAtZeroTorque]
|
|
+mPassiveCurveAngleOffset;
|
|
|
|
//Verify that the curve hits zero at this angle
|
|
if(fabs(mTpCurve.calcValue(mAngleAtSmallestNormPassiveTorque))
|
|
>SQRTEPSILON){
|
|
mAngleAtSmallestNormPassiveTorque =
|
|
numeric_limits<double>::signaling_NaN();
|
|
}
|
|
|
|
VectorNd xDomain = mTpCurve.getCurveDomain();
|
|
double tpAtX0 = mTpCurve.calcValue(xDomain[0]);
|
|
double tpAtX1 = mTpCurve.calcValue(xDomain[1]);
|
|
|
|
if( fabs(tpAtX0) > SQRTEPSILON
|
|
|| fabs(tpAtX1) > SQRTEPSILON){
|
|
double argGuess =mGymnastParams[Gymnast::PassiveAngleAtOneNormTorque];
|
|
//+ mPassiveCurveAngleOffset;
|
|
mAngleAtOneNormPassiveTorque =
|
|
calcInverseBlendedCurveValue(1.0,argGuess,mTpLambda,mTpLambdaMax,
|
|
mTpCurve);
|
|
mAngleAtOneNormPassiveTorque += mPassiveCurveAngleOffset;
|
|
}else{
|
|
mAngleAtOneNormPassiveTorque =
|
|
std::numeric_limits<double>::signaling_NaN();
|
|
}
|
|
} break;
|
|
default:
|
|
{
|
|
cerr << "Millard2016TorqueMuscle::"
|
|
<< "Millard2016TorqueMuscle:"
|
|
<< mMuscleName
|
|
<< "mDataSet has a value of " << mDataSet
|
|
<< " which is not a valid choice";
|
|
assert(0);
|
|
abort();
|
|
}
|
|
};
|
|
|
|
VectorNd tvDomain = mTvCurve.getCurveDomain();
|
|
mTvLambdaMax = mTvCurve.calcValue(tvDomain[0]);
|
|
assert(mTvLambdaMax >= 1.0); //If this fails you've gotten the incorrect
|
|
//domain end.
|
|
|
|
/*
|
|
double taIsoMax = calcBlendedCurveDerivative(mAngleAtOneNormActiveTorque,
|
|
mTaLambda, mTaLambdaMax,
|
|
0,0, mTaCurve);
|
|
double tvIsoMax = calcBlendedCurveDerivative(0,
|
|
mTvLambda, mTvLambdaMax,
|
|
0,0, mTvCurve);
|
|
mMaxActiveIsometricMultiplerProduct = taIsoMax*tvIsoMax;
|
|
|
|
if(mUseTabularMaxActiveIsometricTorque == false){
|
|
mMaxActiveIsometricTorque = mMaxActiveIsometricTorqueUserDefined
|
|
/mMaxActiveIsometricMultiplerProduct;
|
|
}
|
|
*/
|
|
mMuscleCurvesAreDirty = false;
|
|
}
|
|
|
|
|
|
|
|
void Millard2016TorqueMuscle::printJointTorqueProfileToFile(
|
|
const std::string& path,
|
|
const std::string& fileNameWithoutExtension,
|
|
int numberOfSamplePoints)
|
|
{
|
|
if(mMuscleCurvesAreDirty){
|
|
updateTorqueMuscleCurves();
|
|
}
|
|
|
|
VectorNd activeDomain = mTaCurve.getCurveDomain();
|
|
VectorNd passiveDomain = mTpCurve.getCurveDomain();
|
|
VectorNd velocityDomain= mTvCurve.getCurveDomain();
|
|
|
|
double jointMin = calcJointAngle( activeDomain[0] );
|
|
double jointMax = calcJointAngle( activeDomain[1] );
|
|
|
|
if(mTpCurve.calcValue( activeDomain[0] ) >= 0.99){
|
|
jointMin = calcJointAngle( passiveDomain[0] );
|
|
}
|
|
|
|
if(mTpCurve.calcValue(activeDomain[1]) >= 0.99){
|
|
jointMax = calcJointAngle( passiveDomain[1] );
|
|
}
|
|
|
|
|
|
|
|
if(jointMin > jointMax){
|
|
double tmp = jointMin;
|
|
jointMin=jointMax;
|
|
jointMax=tmp;
|
|
}
|
|
double range = jointMax-jointMin;
|
|
jointMin = jointMin -range*0.5;
|
|
jointMax = jointMax +range*0.5;
|
|
double jointDelta = (jointMax-jointMin)
|
|
/((double)numberOfSamplePoints-1.);
|
|
|
|
double velMin = calcJointAngularVelocity( -mOmegaMax );
|
|
double velMax = calcJointAngularVelocity( mOmegaMax );
|
|
|
|
if(velMin > velMax){
|
|
double tmp = velMin;
|
|
velMin = velMax;
|
|
velMax = tmp;
|
|
}
|
|
double velRange = velMax-velMin;
|
|
velMin = velMin-0.5*velRange;
|
|
velMax = velMax+0.5*velRange;
|
|
double velDelta = (velMax-velMin)/((double)numberOfSamplePoints-1.0);
|
|
|
|
double angleAtMaxIsoTorque = mAngleAtOneNormActiveTorque;
|
|
|
|
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,
|
|
mTmInfo);
|
|
|
|
row.at(0) = jointAngle;
|
|
row.at(1) = jointVelocity;
|
|
row.at(2) = activation;
|
|
|
|
row.at(3) = mTmInfo.fiberAngle;
|
|
row.at(4) = mTmInfo.fiberAngularVelocity;
|
|
row.at(5) = mTmInfo.fiberPassiveTorqueAngleMultiplier;
|
|
row.at(6) = mTmInfo.fiberActiveTorqueAngleMultiplier;
|
|
row.at(7) = mTmInfo.fiberTorqueAngularVelocityMultiplier;
|
|
row.at(8) = mTmInfo.fiberActiveTorque;
|
|
row.at(9) = mTmInfo.fiberPassiveTorque;
|
|
row.at(10)= mTmInfo.fiberTorque;
|
|
row.at(11)= mTmInfo.jointTorque;
|
|
row.at(12)= mTmInfo.fiberStiffness;
|
|
row.at(13)= mTmInfo.jointStiffness;
|
|
row.at(14)= mTmInfo.fiberActivePower;
|
|
row.at(15)= mTmInfo.fiberPassivePower;
|
|
row.at(16)= mTmInfo.fiberPower;
|
|
row.at(17)= mTmInfo.jointPower;
|
|
row.at(18)= mTmInfo.DjointTorque_Dactivation;
|
|
row.at(19)= mTmInfo.DjointTorque_DjointAngularVelocity;
|
|
row.at(20)= mTmInfo.DjointTorque_DjointAngle;
|
|
|
|
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 = calcJointAngle( angleAtMaxIsoTorque );
|
|
jointVelocity = calcJointAngularVelocity( velMin + i*velDelta );
|
|
|
|
calcTorqueMuscleInfo(jointAngle,
|
|
jointVelocity,
|
|
activation,
|
|
mTmInfo);
|
|
|
|
row.at(0) = jointAngle;
|
|
row.at(1) = jointVelocity;
|
|
row.at(2) = activation;
|
|
|
|
row.at(3) = mTmInfo.fiberAngle;
|
|
row.at(4) = mTmInfo.fiberAngularVelocity;
|
|
row.at(5) = mTmInfo.fiberPassiveTorqueAngleMultiplier;
|
|
row.at(6) = mTmInfo.fiberActiveTorqueAngleMultiplier;
|
|
row.at(7) = mTmInfo.fiberTorqueAngularVelocityMultiplier;
|
|
row.at(8) = mTmInfo.fiberActiveTorque;
|
|
row.at(9) = mTmInfo.fiberPassiveTorque;
|
|
row.at(10)= mTmInfo.fiberTorque;
|
|
row.at(11)= mTmInfo.jointTorque;
|
|
row.at(12)= mTmInfo.fiberStiffness;
|
|
row.at(13)= mTmInfo.jointStiffness;
|
|
row.at(14)= mTmInfo.fiberActivePower;
|
|
row.at(15)= mTmInfo.fiberPassivePower;
|
|
row.at(16)= mTmInfo.fiberPower;
|
|
row.at(17)= mTmInfo.jointPower;
|
|
row.at(18)= mTmInfo.DjointTorque_Dactivation;
|
|
row.at(19)= mTmInfo.DjointTorque_DjointAngularVelocity;
|
|
row.at(20)= mTmInfo.DjointTorque_DjointAngle;
|
|
|
|
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();
|
|
|
|
}
|
|
|
|
//=============================================================================
|
|
//=============================================================================
|
|
double Millard2016TorqueMuscle::
|
|
calcInverseBlendedCurveValue(
|
|
double blendedCurveValue,
|
|
double argGuess,
|
|
double blendingVariable,
|
|
double maximumBlendingValue,
|
|
RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction
|
|
const &curve) const
|
|
{
|
|
double arg = 0;
|
|
double curveValue =
|
|
(blendedCurveValue-(blendingVariable*maximumBlendingValue))
|
|
/(1-blendingVariable);
|
|
arg = curve.calcInverseValue(curveValue,argGuess);
|
|
return arg;
|
|
}
|
|
double Millard2016TorqueMuscle::
|
|
calcBlendedCurveDerivative(
|
|
double curveArgument,
|
|
double blendingVariable,
|
|
double maximumBlendingValue,
|
|
unsigned int derivativeOrderArgument,
|
|
unsigned int derivativeOrderBlendingVariable,
|
|
RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction
|
|
const &curve) const
|
|
{
|
|
double output = 0;
|
|
|
|
switch(derivativeOrderArgument) {
|
|
case 0: {
|
|
switch(derivativeOrderBlendingVariable) {
|
|
case 0: {
|
|
output = (1.-blendingVariable)*curve.calcValue(curveArgument)
|
|
+ blendingVariable*maximumBlendingValue;
|
|
} break;
|
|
case 1: {
|
|
output = maximumBlendingValue - curve.calcValue(curveArgument);
|
|
} break;
|
|
case 2: {
|
|
output = 0.;
|
|
} break;
|
|
default: {
|
|
output =0.; //Higher order derivatives are zero.
|
|
}
|
|
}
|
|
} break;
|
|
case 1: {
|
|
switch(derivativeOrderBlendingVariable) {
|
|
case 0: {
|
|
output = (1.-blendingVariable)*curve.calcDerivative(curveArgument,1);
|
|
} break;
|
|
case 1: {
|
|
output = -1.0*curve.calcDerivative(curveArgument,1);
|
|
} break;
|
|
case 2: {
|
|
output = 0.;
|
|
} break;
|
|
default: {
|
|
output = 0.; //Higher order derivatives are zero.
|
|
}
|
|
}
|
|
} break;
|
|
case 2: {
|
|
switch(derivativeOrderBlendingVariable) {
|
|
case 0: {
|
|
output = (1.-blendingVariable)*curve.calcDerivative(curveArgument,2);
|
|
} break;
|
|
case 1: {
|
|
output = -1.0*curve.calcDerivative(curveArgument,2);
|
|
} break;
|
|
case 2: {
|
|
output = 0.;
|
|
} break;
|
|
default: {
|
|
output = 0.; //Higher order derivatives are zero.
|
|
}
|
|
}
|
|
} break;
|
|
|
|
default: {
|
|
cerr << "3rd order derivatives not yet implemented";
|
|
assert(0);
|
|
abort();
|
|
}
|
|
};
|
|
return output;
|
|
}
|
|
|
|
|
|
|
|
//=============================================================================
|
|
//=============================================================================
|
|
|
|
double Millard2016TorqueMuscle::
|
|
calcFiberAngleGivenNormalizedPassiveTorque(
|
|
double normPassiveFiberTorque,
|
|
double blendingVariable,
|
|
double passiveTorqueAngleCurveOffset) const
|
|
{
|
|
|
|
double angle = numeric_limits<double>::signaling_NaN();
|
|
VectorNd domain = mTpCurve.getCurveDomain();
|
|
double y0 = mTpCurve.calcValue(domain[0]);
|
|
double y1 = mTpCurve.calcValue(domain[1]);
|
|
|
|
if( (blendingVariable < 1) && (y0 > SQRTEPSILON || y1 > SQRTEPSILON) ){
|
|
double tp = normPassiveFiberTorque/(1-blendingVariable);
|
|
angle = mTpCurve.calcInverseValue(tp,0.)-passiveTorqueAngleCurveOffset;
|
|
}
|
|
|
|
return angle;
|
|
}
|
|
|
|
|
|
//=============================================================================
|
|
//=============================================================================
|
|
|
|
void Millard2016TorqueMuscle::
|
|
updTorqueMuscleSummaryCurveValues(double fiberAngle,
|
|
double normFiberAngularVelocity,
|
|
double activeTorqueAngleBlendingVariable,
|
|
double passiveTorqueAngleBlendingVariable,
|
|
double torqueAngularVelocityBlendingVariable,
|
|
double activeTorqueAngleAngleScaling,
|
|
double activeTorqueAngleAtOneNormTorque,
|
|
double passiveTorqueAngleCurveOffset,
|
|
TorqueMuscleSummary &updTms) const
|
|
{
|
|
|
|
double taAngle = (fiberAngle-activeTorqueAngleAtOneNormTorque)
|
|
/activeTorqueAngleAngleScaling
|
|
+ activeTorqueAngleAtOneNormTorque;
|
|
|
|
updTms.fiberActiveTorqueAngleMultiplier =
|
|
calcBlendedCurveDerivative(taAngle,
|
|
activeTorqueAngleBlendingVariable,
|
|
mTaLambdaMax,
|
|
0,0,
|
|
mTaCurve);
|
|
|
|
updTms.fiberPassiveTorqueAngleMultiplier =
|
|
calcBlendedCurveDerivative(fiberAngle-passiveTorqueAngleCurveOffset,
|
|
passiveTorqueAngleBlendingVariable,
|
|
mTpLambdaMax,
|
|
0,0,
|
|
mTpCurve);
|
|
|
|
updTms.fiberTorqueAngularVelocityMultiplier =
|
|
calcBlendedCurveDerivative(
|
|
normFiberAngularVelocity,
|
|
torqueAngularVelocityBlendingVariable,
|
|
mTvLambdaMax,
|
|
0,0,
|
|
mTvCurve);
|
|
|
|
updTms.fiberNormalizedDampingTorque = updTms.fiberPassiveTorqueAngleMultiplier
|
|
* (-normFiberAngularVelocity*mBetaMax);
|
|
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::
|
|
updTorqueMuscleInfo(
|
|
double activation,
|
|
double jointAngle,
|
|
double jointAngularVelocity,
|
|
double activeTorqueAngleBlendingVariable,
|
|
double passiveTorqueAngleBlendingVariable,
|
|
double torqueAngularVelocityBlendingVariable,
|
|
double activeTorqueAngleAngleScaling,
|
|
double activeTorqueAngleAtOneNormTorque,
|
|
double passiveTorqueAngleCurveOffset,
|
|
double maxAngularVelocity,
|
|
double maxActIsoTorque,
|
|
TorqueMuscleInfo &updTmi) const
|
|
{
|
|
|
|
//Update state quantities
|
|
updTmi.activation = activation;
|
|
updTmi.jointAngle = jointAngle;
|
|
updTmi.jointAngularVelocity = jointAngularVelocity;
|
|
|
|
double fiberAngle = calcFiberAngle(jointAngle);
|
|
double fiberAngularVelocity = calcFiberAngularVelocity(jointAngularVelocity);
|
|
double omegaNorm = fiberAngularVelocity/maxAngularVelocity;
|
|
double D_wn_D_w = 1.0/maxAngularVelocity;
|
|
double D_wn_D_wmax = -fiberAngularVelocity
|
|
/(maxAngularVelocity*maxAngularVelocity);
|
|
|
|
double D2_wn_D_wmax2 = -2.0*D_wn_D_wmax/maxAngularVelocity;
|
|
|
|
double taAngle = (fiberAngle-activeTorqueAngleAtOneNormTorque)
|
|
/activeTorqueAngleAngleScaling
|
|
+ activeTorqueAngleAtOneNormTorque;
|
|
|
|
double D_taAngle_D_fiberAngle = 1.0/activeTorqueAngleAngleScaling;
|
|
|
|
double D_taAngle_D_angleScaling =
|
|
-(fiberAngle-activeTorqueAngleAtOneNormTorque)
|
|
/(activeTorqueAngleAngleScaling*activeTorqueAngleAngleScaling);
|
|
|
|
double D2_taAngle_D_angleScaling2 = -2.0*D_taAngle_D_angleScaling
|
|
/activeTorqueAngleAngleScaling;
|
|
|
|
//Update force component values
|
|
double ta =
|
|
calcBlendedCurveDerivative(taAngle,
|
|
activeTorqueAngleBlendingVariable,
|
|
mTaLambdaMax,
|
|
0,0,
|
|
mTaCurve);
|
|
|
|
double tp =
|
|
calcBlendedCurveDerivative(fiberAngle-passiveTorqueAngleCurveOffset,
|
|
passiveTorqueAngleBlendingVariable,
|
|
mTpLambdaMax,
|
|
0,0,
|
|
mTpCurve);
|
|
|
|
double tv = calcBlendedCurveDerivative(
|
|
omegaNorm,
|
|
torqueAngularVelocityBlendingVariable,
|
|
mTvLambdaMax,
|
|
0,0,
|
|
mTvCurve);
|
|
|
|
double tb = tp * (-omegaNorm*mBetaMax);
|
|
|
|
double D_tb_D_omegaMax = tp*(-D_wn_D_wmax*mBetaMax);
|
|
|
|
|
|
//Update force component derivative values;
|
|
//1st derivatives w.r.t fiber angle/velocity
|
|
double D_ta_D_taAngle = calcBlendedCurveDerivative(
|
|
taAngle,
|
|
activeTorqueAngleBlendingVariable,
|
|
mTaLambdaMax,
|
|
1,0,
|
|
mTaCurve);
|
|
|
|
double D_ta_D_angleScaling = D_ta_D_taAngle*D_taAngle_D_angleScaling;
|
|
|
|
double D_ta_D_fiberAngle = D_ta_D_taAngle*D_taAngle_D_fiberAngle;
|
|
|
|
double D_tp_D_fiberAngle = calcBlendedCurveDerivative(
|
|
fiberAngle-passiveTorqueAngleCurveOffset,
|
|
passiveTorqueAngleBlendingVariable,
|
|
mTpLambdaMax,
|
|
1,0,
|
|
mTpCurve);
|
|
|
|
double D_tp_D_fiberAngleOffset= -D_tp_D_fiberAngle;
|
|
|
|
double D_tv_D_wn = calcBlendedCurveDerivative(
|
|
omegaNorm,
|
|
torqueAngularVelocityBlendingVariable,
|
|
mTvLambdaMax,
|
|
1,0,
|
|
mTvCurve);
|
|
|
|
|
|
|
|
double D_tv_D_fiberAngularVelocity = D_tv_D_wn*D_wn_D_w;
|
|
double D_tv_D_omegaMax = D_tv_D_wn*D_wn_D_wmax;
|
|
|
|
|
|
//1st derivatives w.r.t fitting-related variables
|
|
double D_ta_D_taLambda = calcBlendedCurveDerivative(
|
|
taAngle,
|
|
activeTorqueAngleBlendingVariable,
|
|
mTaLambdaMax,
|
|
0,1,
|
|
mTaCurve);
|
|
|
|
double D_tp_D_tpLambda = calcBlendedCurveDerivative(
|
|
fiberAngle-passiveTorqueAngleCurveOffset,
|
|
passiveTorqueAngleBlendingVariable,
|
|
mTpLambdaMax,
|
|
0,1,
|
|
mTpCurve);
|
|
double D_tv_D_tvLambda = calcBlendedCurveDerivative(
|
|
omegaNorm,
|
|
torqueAngularVelocityBlendingVariable,
|
|
mTvLambdaMax,
|
|
0,1,
|
|
mTvCurve);
|
|
|
|
//2nd derivatives w.r.t fitting-related variables.
|
|
double D2_ta_D_taLambda2 = calcBlendedCurveDerivative(
|
|
taAngle,
|
|
activeTorqueAngleBlendingVariable,
|
|
mTaLambdaMax,
|
|
0,2,
|
|
mTaCurve);
|
|
|
|
double D2_tv_D_tvLambda2 = calcBlendedCurveDerivative(
|
|
omegaNorm,
|
|
torqueAngularVelocityBlendingVariable,
|
|
mTvLambdaMax,
|
|
0,2,
|
|
mTvCurve);
|
|
double D2_tp_D_tpLambda2 = calcBlendedCurveDerivative(
|
|
fiberAngle-passiveTorqueAngleCurveOffset,
|
|
passiveTorqueAngleBlendingVariable,
|
|
mTpLambdaMax,
|
|
0,2,
|
|
mTpCurve);
|
|
|
|
|
|
double D2_tp_D_fiberAngle2 =calcBlendedCurveDerivative(
|
|
fiberAngle-passiveTorqueAngleCurveOffset,
|
|
passiveTorqueAngleBlendingVariable,
|
|
mTpLambdaMax,
|
|
2,0,
|
|
mTpCurve);
|
|
|
|
double D2_ta_D_taAngle2 = calcBlendedCurveDerivative(
|
|
taAngle,
|
|
activeTorqueAngleBlendingVariable,
|
|
mTaLambdaMax,
|
|
2,0,
|
|
mTaCurve);
|
|
|
|
double D2_ta_D_angleScaling2 =
|
|
D2_ta_D_taAngle2*D_taAngle_D_angleScaling*D_taAngle_D_angleScaling
|
|
+ D_ta_D_taAngle*D2_taAngle_D_angleScaling2;
|
|
|
|
double D2_tv_D_wn2 = calcBlendedCurveDerivative(
|
|
omegaNorm,
|
|
torqueAngularVelocityBlendingVariable,
|
|
mTvLambdaMax,
|
|
2,0,
|
|
mTvCurve);
|
|
|
|
double D2_tb_D_omegaMax2 = tp*(-D2_wn_D_wmax2*mBetaMax);
|
|
double D2_tb_D_omegaMax_D_tpLambda = D_tp_D_tpLambda*(-D_wn_D_wmax*mBetaMax);
|
|
double D2_tb_D_omegaMax_D_tpOffset = D_tp_D_fiberAngleOffset
|
|
*(-D_wn_D_wmax*mBetaMax);
|
|
double D2_tv_D_omegaMax2 = D2_tv_D_wn2*D_wn_D_wmax*D_wn_D_wmax
|
|
+ D_tv_D_wn*D2_wn_D_wmax2;
|
|
|
|
|
|
|
|
//Note that d/d_tpOffsetAngle kicks out another -1, so this 2nd derivative has
|
|
//a positive sign.
|
|
double D2_tp_D_fiberAngleOffset2= D2_tp_D_fiberAngle2;
|
|
|
|
double D2_tp_D_tpLambda_D_fiberAngle =
|
|
calcBlendedCurveDerivative( fiberAngle-passiveTorqueAngleCurveOffset,
|
|
passiveTorqueAngleBlendingVariable,
|
|
mTpLambdaMax,
|
|
1,1,
|
|
mTpCurve);
|
|
double D2_tp_D_tpLambda_D_fiberAngleOffset = -D2_tp_D_tpLambda_D_fiberAngle;
|
|
|
|
//Damping derivatives
|
|
double D_tb_D_wn = tp * (-1.0*mBetaMax);
|
|
|
|
double D_tb_D_tpLambda = -D_tp_D_tpLambda*mBetaMax*omegaNorm;
|
|
|
|
|
|
double D_tb_D_fiberAngularVelocity = D_tb_D_wn*D_wn_D_w;
|
|
|
|
double D_tb_D_fiberAngle = -D_tp_D_fiberAngle*mBetaMax*omegaNorm;
|
|
|
|
double D_tb_D_fiberAngleOffset= -D_tp_D_fiberAngleOffset*mBetaMax*omegaNorm;
|
|
|
|
//Damping second derivatives
|
|
double D2_tb_D_tpLambda_D_fiberAngleOffset =
|
|
-D2_tp_D_tpLambda_D_fiberAngleOffset*mBetaMax*omegaNorm;
|
|
double D2_tb_D_tpLambda2 =
|
|
-D2_tp_D_tpLambda2*mBetaMax*omegaNorm;
|
|
double D2_tb_D_fiberAngleOffset2 =
|
|
-D2_tp_D_fiberAngleOffset2*mBetaMax*omegaNorm;
|
|
|
|
//Sign conventions
|
|
double D_fiberAngle_D_jointAngle = mSignOfJointAngle;
|
|
double D_fiberAngularVelocity_D_jointAngularVelocity =
|
|
mSignOfConcentricAnglularVelocity;
|
|
|
|
|
|
updTmi.jointAngle = jointAngle;
|
|
updTmi.jointAngularVelocity = jointAngularVelocity;
|
|
updTmi.fiberAngle = fiberAngle;
|
|
updTmi.fiberAngularVelocity = fiberAngularVelocity;
|
|
|
|
updTmi.fiberPassiveTorqueAngleMultiplier = tp;
|
|
updTmi.fiberActiveTorqueAngleMultiplier = ta;
|
|
updTmi.fiberTorqueAngularVelocityMultiplier = tv;
|
|
|
|
updTmi.activation = activation;
|
|
updTmi.fiberActiveTorque = maxActIsoTorque*(activation*ta*tv);
|
|
updTmi.fiberPassiveTorque = maxActIsoTorque*(tp+tb);
|
|
updTmi.fiberPassiveElasticTorque = maxActIsoTorque*tp;
|
|
updTmi.fiberDampingTorque = maxActIsoTorque*tb;
|
|
updTmi.fiberNormDampingTorque = tb;
|
|
|
|
updTmi.fiberTorque = updTmi.fiberActiveTorque + updTmi.fiberPassiveTorque;
|
|
updTmi.jointTorque = mSignOfJointTorque*updTmi.fiberTorque;
|
|
|
|
updTmi.fiberStiffness = maxActIsoTorque*(
|
|
activation*D_ta_D_fiberAngle*tv
|
|
+ D_tp_D_fiberAngle
|
|
+ D_tb_D_fiberAngle);
|
|
|
|
updTmi.jointStiffness = mSignOfJointTorque
|
|
*updTmi.fiberStiffness
|
|
*D_fiberAngle_D_jointAngle;
|
|
|
|
updTmi.fiberActivePower = updTmi.fiberActiveTorque
|
|
* updTmi.fiberAngularVelocity;
|
|
|
|
updTmi.fiberPassivePower = updTmi.fiberPassiveTorque
|
|
* updTmi.fiberAngularVelocity;
|
|
|
|
updTmi.fiberPower = updTmi.fiberActivePower
|
|
+ updTmi.fiberPassivePower;
|
|
|
|
updTmi.jointPower = updTmi.jointTorque * jointAngularVelocity;
|
|
|
|
|
|
updTmi.DfiberPassiveTorqueAngleMultiplier_DblendingVariable
|
|
= D_tp_D_tpLambda;
|
|
updTmi.DfiberActiveTorqueAngleMultiplier_DblendingVariable
|
|
= D_ta_D_taLambda;
|
|
updTmi.DfiberTorqueAngularVelocityMultiplier_DblendingVariable
|
|
= D_tv_D_tvLambda;
|
|
updTmi.DfiberPassiveTorqueAngleMultiplier_DangleOffset
|
|
= D_tp_D_fiberAngleOffset;
|
|
|
|
|
|
//tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)*(1-beta*omegaNorm)
|
|
// Dtau_Da = tauMaxIso*(ta(theta) * tv(thetaDot) )
|
|
updTmi.DjointTorque_Dactivation =
|
|
mSignOfJointTorque
|
|
*maxActIsoTorque
|
|
*(ta * tv);
|
|
|
|
//tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)*(1-beta*omegaNorm)
|
|
//Dtau_Dtheta = signTq*tauIso*(a*Dta_Dtheta(theta)*tv(thetaDot)
|
|
// + Dtp_Dtheta(theta)*(1-beta*omegaNorm)
|
|
updTmi.DjointTorque_DjointAngle =
|
|
mSignOfJointTorque
|
|
* maxActIsoTorque
|
|
* ( activation
|
|
*D_ta_D_fiberAngle
|
|
* tv
|
|
+ ( D_tp_D_fiberAngle
|
|
+ D_tb_D_fiberAngle)
|
|
)* D_fiberAngle_D_jointAngle;
|
|
|
|
|
|
//tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)*(1-beta*omegaNorm)
|
|
//Dtau_Domega = signTq*tauIso*(a * ta(theta) * Dtv_DthetaDot(thetaDot)
|
|
// - tp(theta)*beta*DomegaNorm_thetaDot
|
|
updTmi.DjointTorque_DjointAngularVelocity =
|
|
mSignOfJointTorque
|
|
* maxActIsoTorque
|
|
* ( activation
|
|
* ta
|
|
* ( D_tv_D_fiberAngularVelocity
|
|
*D_fiberAngularVelocity_D_jointAngularVelocity)
|
|
+ ( D_tb_D_fiberAngularVelocity
|
|
*D_fiberAngularVelocity_D_jointAngularVelocity)
|
|
);
|
|
|
|
//tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)*(1-beta*omegaNorm)
|
|
//Dtau_DtaLambda = signTq*tauIso*(a * Dta(theta)_Dlambda * tv(thetaDot)
|
|
updTmi.DjointTorque_DactiveTorqueAngleBlendingVariable =
|
|
mSignOfJointTorque
|
|
* maxActIsoTorque
|
|
*(activation*D_ta_D_taLambda*tv);
|
|
|
|
//tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)*(1-beta*omegaNorm)
|
|
//Dtau_DtpLambda = signTq*tauIso*( D_tp(theta)_D_lambda(1-beta*omegaNorm)
|
|
updTmi.DjointTorque_DpassiveTorqueAngleBlendingVariable =
|
|
mSignOfJointTorque
|
|
* maxActIsoTorque
|
|
*(D_tp_D_tpLambda + D_tb_D_tpLambda);
|
|
|
|
//tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)*(1-beta*omegaNorm)
|
|
//Dtau_tvLambda = signTq*tauIso*(a*ta(theta)*Dtv_Dlambda(thetaDot)
|
|
updTmi.DjointTorque_DtorqueAngularVelocityBlendingVariable =
|
|
mSignOfJointTorque
|
|
* maxActIsoTorque
|
|
*(activation*ta*D_tv_D_tvLambda);
|
|
|
|
// mSignOfJointTorque*( (maxActIsoTorque*(activation*ta*tv)
|
|
// + maxActIsoTorque*(tp+tb)))
|
|
updTmi.DjointTorque_DmaximumIsometricTorque =
|
|
mSignOfJointTorque
|
|
*((activation*ta*tv) + (tp+tb));
|
|
|
|
updTmi.DjointTorque_DpassiveTorqueAngleCurveAngleOffset =
|
|
mSignOfJointTorque
|
|
*maxActIsoTorque
|
|
*(D_tp_D_fiberAngleOffset + D_tb_D_fiberAngleOffset);
|
|
//* D_fiberAngle_D_jointAngle;
|
|
|
|
//tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)+tb(theta,omega)
|
|
//Dtau_DomegaMax = signTq*tauIso*(a*ta(theta)*Dtv_DomegaMax(thetaDot)
|
|
// + tp(theta)*(-beta*D_omegaNorm_DomegaMax)
|
|
updTmi.DjointTorque_DmaximumAngularVelocity =
|
|
mSignOfJointTorque
|
|
*maxActIsoTorque
|
|
*(activation*ta*D_tv_D_omegaMax + D_tb_D_omegaMax);
|
|
|
|
//tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)+tb(theta,omega)
|
|
//Dtau_DtaAngleScaling = signTq*tauIso*(a*Dta(theta)_DangleScaling*tv(thetaDot)
|
|
|
|
updTmi.DjointTorque_DactiveTorqueAngleAngleScaling =
|
|
mSignOfJointTorque
|
|
*maxActIsoTorque
|
|
*(activation*D_ta_D_angleScaling*tv);
|
|
|
|
|
|
/*
|
|
Second derivatives of the exposed variables, x, for fitting:
|
|
|
|
Option 1:
|
|
x = [taLambda,tvLambda,tpLambda,tpOffset]
|
|
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.
|
|
|
|
Option 2:
|
|
x = [taAngleScaling, tvOmegaMax, tpLambda,tpOffset]
|
|
for brevity
|
|
x = [s, m, p, o]
|
|
|
|
For reference:
|
|
t(s,m,p,o) = mSignOfJointTorque*(
|
|
(maxActIsoTorque*(
|
|
activation*ta(s)*tv(m)) + (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(s,m,p,o) w.r.t. x is
|
|
stored, row-wise, in the vector fittingInfo. Thus we have
|
|
|
|
s m p o
|
|
H(tau(s,m,p,o),x) = s [ 0: d2t/ds2
|
|
m [ 1: d2t/dsdm 2: d2t/dm2
|
|
p [ 3: d2t/dsdp 4: d2t/dmdp 5: d2t/dp2
|
|
o [ 6: d2t/dsdo 7: d2t/dmdo 8: d2t/dpdo 9: d2t/do2
|
|
The numbers indicate the index of the quantity in fittingInfo.
|
|
|
|
So that both fitting options are possible, the extra entries for the
|
|
above Hessian will just be concatentated to the existing vector. Three
|
|
of the entries are duplicates, but for now they are being included
|
|
just to make this slightly easier to use for now. Thus the vector of
|
|
2nd derivatives will look like:
|
|
|
|
For joint-torque related constraints that use x = [a,v,p,o]
|
|
0: d2t/da2
|
|
1: d2t/dadv
|
|
2: d2t/dv2
|
|
3: d2t/dadp
|
|
4: d2t/dvdp
|
|
5: d2t/dp2
|
|
6: d2t/dado
|
|
7: d2t/dvdo
|
|
8: d2t/dpdo
|
|
9: d2t/do2
|
|
For constraints on the value of the passive element
|
|
10: d2p/dp2
|
|
11: d2p/dpdo
|
|
12: d2p/do2
|
|
For joint-torque related constraints that use x = [s,m,p,o]
|
|
13: d2t/ds2
|
|
14: d2t/dsdm
|
|
15: d2t/dm2
|
|
16: d2t/dsdp
|
|
17: d2t/dmdp
|
|
18: d2t/dp2
|
|
19: d2t/dsdo
|
|
20: d2t/dmdo
|
|
21: d2t/dpdo
|
|
22: d2t/do2
|
|
|
|
*/
|
|
|
|
#ifdef RBDL_BUILD_ADDON_MUSCLE_FITTING
|
|
updTmi.fittingInfo.conservativeResize(23);
|
|
|
|
//0: d2t/da2
|
|
updTmi.fittingInfo[0] = mSignOfJointTorque*(maxActIsoTorque*(
|
|
activation*D2_ta_D_taLambda2*tv));
|
|
|
|
//1: d2t/dadv
|
|
updTmi.fittingInfo[1] = mSignOfJointTorque*(maxActIsoTorque*
|
|
(activation*D_ta_D_taLambda*D_tv_D_tvLambda));
|
|
|
|
//2: d2t/dv2
|
|
updTmi.fittingInfo[2] = mSignOfJointTorque*(maxActIsoTorque*
|
|
(activation*ta*D2_tv_D_tvLambda2));
|
|
|
|
//3: d2t/dadp
|
|
updTmi.fittingInfo[3] = 0.;
|
|
|
|
//4: d2t/dvdp
|
|
updTmi.fittingInfo[4] = 0.;
|
|
|
|
//5: d2t/dp2
|
|
updTmi.fittingInfo[5] = mSignOfJointTorque*(maxActIsoTorque*(
|
|
D2_tp_D_tpLambda2+D2_tb_D_tpLambda2));
|
|
//6: d2t/dado
|
|
updTmi.fittingInfo[6] = 0.;
|
|
//7: d2t/dvdo
|
|
updTmi.fittingInfo[7] = 0.;
|
|
//8: d2t/dpdo
|
|
updTmi.fittingInfo[8] = mSignOfJointTorque*(maxActIsoTorque*(
|
|
D2_tp_D_tpLambda_D_fiberAngleOffset
|
|
+D2_tb_D_tpLambda_D_fiberAngleOffset
|
|
));//*D_fiberAngle_D_jointAngle);
|
|
//9: d2t/do2
|
|
updTmi.fittingInfo[9] = mSignOfJointTorque*(maxActIsoTorque*(
|
|
D2_tp_D_fiberAngleOffset2
|
|
+D2_tb_D_fiberAngleOffset2
|
|
));//*D_fiberAngle_D_jointAngle);
|
|
//10: d2tp/dp2
|
|
updTmi.fittingInfo[10] = D2_tp_D_tpLambda2;
|
|
//11: d2tp/dpdo
|
|
updTmi.fittingInfo[11] = D2_tp_D_tpLambda_D_fiberAngleOffset;
|
|
//12: d2tp/do2
|
|
updTmi.fittingInfo[12] = D2_tp_D_fiberAngleOffset2;
|
|
|
|
//13: d2t/ds2
|
|
updTmi.fittingInfo[13] = mSignOfJointTorque
|
|
*maxActIsoTorque
|
|
*(activation*D2_ta_D_angleScaling2*tv);
|
|
//14: d2t/dsdm
|
|
updTmi.fittingInfo[14] = mSignOfJointTorque
|
|
*maxActIsoTorque
|
|
*(activation*D_ta_D_angleScaling*D_tv_D_omegaMax);
|
|
|
|
//15: d2t/dm2
|
|
updTmi.fittingInfo[15] = mSignOfJointTorque
|
|
*maxActIsoTorque
|
|
*(activation*ta*D2_tv_D_omegaMax2 + D2_tb_D_omegaMax2);
|
|
//16: d2t/dsdp
|
|
updTmi.fittingInfo[16] = 0.;
|
|
|
|
//17: d2t/dmdp
|
|
updTmi.fittingInfo[17] = mSignOfJointTorque
|
|
*maxActIsoTorque
|
|
*(D2_tb_D_omegaMax_D_tpLambda);
|
|
//18: d2t/dp2
|
|
updTmi.fittingInfo[18] = mSignOfJointTorque*(
|
|
maxActIsoTorque*(
|
|
D2_tp_D_tpLambda2+D2_tb_D_tpLambda2));
|
|
//19: d2t/dsdo
|
|
updTmi.fittingInfo[19] = 0.;
|
|
|
|
//20: d2t/dmdo
|
|
updTmi.fittingInfo[20] = mSignOfJointTorque
|
|
*maxActIsoTorque
|
|
*(D2_tb_D_omegaMax_D_tpOffset);
|
|
|
|
//21: d2t/dpdo
|
|
updTmi.fittingInfo[21] = mSignOfJointTorque
|
|
*(maxActIsoTorque
|
|
*( D2_tp_D_tpLambda_D_fiberAngleOffset
|
|
+D2_tb_D_tpLambda_D_fiberAngleOffset));
|
|
|
|
//22: d2t/do2
|
|
updTmi.fittingInfo[22] = mSignOfJointTorque
|
|
*(maxActIsoTorque
|
|
*( D2_tp_D_fiberAngleOffset2
|
|
+D2_tb_D_fiberAngleOffset2));
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
//=============================================================================
|
|
void Millard2016TorqueMuscle::
|
|
updTorqueMuscleSummary( double activation,
|
|
double jointAngle,
|
|
double jointAngularVelocity,
|
|
double activeTorqueAngleBlendingVariable,
|
|
double passiveTorqueAngleBlendingVariable,
|
|
double torqueAngularVelocityBlendingVariable,
|
|
double activeTorqueAngleAngleScaling,
|
|
double activeTorqueAngleAtOneNormTorque,
|
|
double passiveTorqueAngleCurveOffset,
|
|
double maxAngularVelocity,
|
|
double maxActIsoTorque,
|
|
TorqueMuscleSummary &updTms) const
|
|
{
|
|
double fiberAngle = calcFiberAngle(jointAngle);
|
|
double fiberVelocity = calcFiberAngularVelocity(jointAngularVelocity);
|
|
double fiberVelocityNorm = fiberVelocity/maxAngularVelocity;
|
|
|
|
|
|
updTorqueMuscleSummaryCurveValues(fiberAngle,
|
|
fiberVelocityNorm,
|
|
activeTorqueAngleBlendingVariable,
|
|
passiveTorqueAngleBlendingVariable,
|
|
torqueAngularVelocityBlendingVariable,
|
|
activeTorqueAngleAngleScaling,
|
|
activeTorqueAngleAtOneNormTorque,
|
|
passiveTorqueAngleCurveOffset,
|
|
updTms);
|
|
updTms.fiberAngle = fiberAngle;
|
|
updTms.fiberAngularVelocity = fiberVelocity;
|
|
updTms.activation = activation;
|
|
updTms.fiberTorque =
|
|
maxActIsoTorque
|
|
*( activation*( updTms.fiberActiveTorqueAngleMultiplier
|
|
*updTms.fiberTorqueAngularVelocityMultiplier
|
|
)
|
|
+ ( updTms.fiberPassiveTorqueAngleMultiplier
|
|
+ updTms.fiberNormalizedDampingTorque
|
|
)
|
|
);
|
|
|
|
updTms.jointTorque = updTms.fiberTorque*mSignOfJointTorque;
|
|
}
|
|
|
|
void Millard2016TorqueMuscle::
|
|
updInvertTorqueMuscleSummary(double jointTorque,
|
|
double jointAngle,
|
|
double jointAngularVelocity,
|
|
double activeTorqueAngleBlendingVariable,
|
|
double passiveTorqueAngleBlendingVariable,
|
|
double torqueAngularVelocityBlendingVariable,
|
|
double activeTorqueAngleAngleScaling,
|
|
double activeTorqueAngleAtOneNormTorque,
|
|
double passiveTorqueAngleCurveOffset,
|
|
double maxAngularVelocity,
|
|
double maxActIsoTorque,
|
|
TorqueMuscleSummary &updTms) const
|
|
{
|
|
|
|
double fiberAngle = calcFiberAngle(jointAngle);
|
|
double fiberVelocity = calcFiberAngularVelocity(jointAngularVelocity);
|
|
double fiberVelocityNorm = fiberVelocity/maxAngularVelocity;
|
|
|
|
updTorqueMuscleSummaryCurveValues(fiberAngle,
|
|
fiberVelocityNorm,
|
|
activeTorqueAngleBlendingVariable,
|
|
passiveTorqueAngleBlendingVariable,
|
|
torqueAngularVelocityBlendingVariable,
|
|
activeTorqueAngleAngleScaling,
|
|
activeTorqueAngleAtOneNormTorque,
|
|
passiveTorqueAngleCurveOffset,
|
|
updTms);
|
|
|
|
updTms.fiberAngle = fiberAngle;
|
|
updTms.fiberAngularVelocity = fiberVelocity;
|
|
|
|
updTms.jointTorque = jointTorque;
|
|
updTms.fiberTorque = jointTorque*mSignOfJointTorque;
|
|
|
|
updTms.activation =
|
|
( (updTms.fiberTorque/maxActIsoTorque)
|
|
- (updTms.fiberPassiveTorqueAngleMultiplier
|
|
+ updTms.fiberNormalizedDampingTorque)
|
|
)/( updTms.fiberActiveTorqueAngleMultiplier
|
|
*updTms.fiberTorqueAngularVelocityMultiplier);
|
|
}
|
|
|
|
//==============================================================================
|