1435 lines
43 KiB
C++
1435 lines
43 KiB
C++
/*-------------------------------------------------------------------------
|
|
OpenSim: SmoothSegmentedFunctionFactory.cpp
|
|
--------------------------------------------------------------------------
|
|
The OpenSim API is a toolkit for musculoskeletal modeling and simulation.
|
|
See http:%opensim.stanford.edu and the NOTICE file for more information.
|
|
OpenSim is developed at Stanford University and supported by the US
|
|
National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA
|
|
through the Warrior Web program.
|
|
|
|
Copyright (c) 2005-2012 Stanford University and the Authors
|
|
Author(s): Matthew Millard
|
|
|
|
Licensed under the Apache License, Version 2.0 (the 'License'); you may
|
|
not use this file except in compliance with the License. You may obtain a
|
|
copy of the License at http:%www.apache.org/licenses/LICENSE-2.0.
|
|
|
|
Unless required by applicable law or agreed to in writing, software
|
|
distributed under the License is distributed on an 'AS IS' BASIS,
|
|
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
See the License for the specific language governing permissions and
|
|
limitations under the License.
|
|
--------------------------------------------------------------------------
|
|
|
|
Derivative work
|
|
Date : September 2016
|
|
Authors(s): Millard
|
|
Updates : Made active torque-angle, passive-torque-angle, torque-velocity
|
|
and tendon-torque-angle curves based on the equivalent line-type
|
|
curves in OpenSim.
|
|
*/
|
|
|
|
#include "TorqueMuscleFunctionFactory.h"
|
|
#include <cmath>
|
|
#include <cstdio>
|
|
#include <iostream>
|
|
#include <fstream>
|
|
#include <sstream>
|
|
|
|
using namespace std;
|
|
using namespace RigidBodyDynamics::Math;
|
|
using namespace RigidBodyDynamics::Addons::Muscle;
|
|
using namespace RigidBodyDynamics::Addons::Geometry;
|
|
|
|
static double SQRTEPS = sqrt( (double)std::numeric_limits<double>::epsilon());
|
|
|
|
//=============================================================================
|
|
// Anderson 2007 Active Torque Angle Curve
|
|
//=============================================================================
|
|
|
|
void TorqueMuscleFunctionFactory::
|
|
createAnderson2007ActiveTorqueAngleCurve(
|
|
double c2,
|
|
double c3,
|
|
const std::string& curveName,
|
|
SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate)
|
|
{
|
|
//Check the input arguments
|
|
if( !(c2 > 0) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createAnderson2007ActiveTorqueAngleCurve "
|
|
<< curveName
|
|
<< ": c2 must be greater than 0"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
|
|
std::string name=curveName;
|
|
name.append(".createAnderson2007ActiveTorqueAngleCurve");
|
|
|
|
//For now these advanced paramters are hidden. They will only be
|
|
//uncovered if absolutely necessary.
|
|
double minValueAtShoulders = 0;
|
|
double minShoulderSlopeMagnitude = 0;
|
|
|
|
double curviness = 0.5;
|
|
double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness);
|
|
|
|
//Translate the user parameters to quintic Bezier points
|
|
double x0 = c3 - 1.05*(0.5*(M_PI/c2));
|
|
double x1 = c3 - 0.95*(0.5*(M_PI/c2));
|
|
double x2 = c3;
|
|
double x3 = c3 + 0.95*(0.5*(M_PI/c2));
|
|
double x4 = c3 + 1.05*(0.5*(M_PI/c2));
|
|
|
|
double y0 = minValueAtShoulders;
|
|
double y1 = cos(c2*(x1-c3));
|
|
double y2 = cos(c2*(x2-c3));
|
|
double y3 = cos(c2*(x3-c3));
|
|
double y4 = minValueAtShoulders;
|
|
|
|
double dydx0 = minShoulderSlopeMagnitude;
|
|
double dydx1 = -sin(c2*(x1-c3))*c2;
|
|
double dydx2 = -sin(c2*(x2-c3))*c2;
|
|
double dydx3 = -sin(c2*(x3-c3))*c2;
|
|
double dydx4 = -minShoulderSlopeMagnitude;
|
|
|
|
|
|
//Compute the Quintic Bezier control points
|
|
RigidBodyDynamics::Math::MatrixNd p0 =
|
|
SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(x0,y0,dydx0,
|
|
x1,y1,dydx1,c);
|
|
|
|
RigidBodyDynamics::Math::MatrixNd p1 =
|
|
SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(x1,y1,dydx1,
|
|
x2,y2,dydx2,c);
|
|
|
|
RigidBodyDynamics::Math::MatrixNd p2 =
|
|
SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(x2,y2,dydx2,
|
|
x3,y3,dydx3,c);
|
|
|
|
RigidBodyDynamics::Math::MatrixNd p3 =
|
|
SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(x3,y3,dydx3,
|
|
x4,y4,dydx4,c);
|
|
|
|
RigidBodyDynamics::Math::MatrixNd mX(6,4);
|
|
RigidBodyDynamics::Math::MatrixNd mY(6,4);
|
|
|
|
mX.col(0) = p0.col(0);
|
|
mY.col(0) = p0.col(1);
|
|
mX.col(1) = p1.col(0);
|
|
mY.col(1) = p1.col(1);
|
|
mX.col(2) = p2.col(0);
|
|
mY.col(2) = p2.col(1);
|
|
mX.col(3) = p3.col(0);
|
|
mY.col(3) = p3.col(1);
|
|
|
|
|
|
smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction(
|
|
mX, mY, x0, x4, y0, y4, dydx0, dydx4, curveName);
|
|
}
|
|
|
|
//=============================================================================
|
|
// ANDERSON 2007 Active Torque Angular Velocity Curve
|
|
//=============================================================================
|
|
void TorqueMuscleFunctionFactory::
|
|
createAnderson2007ActiveTorqueVelocityCurve(
|
|
double c4,
|
|
double c5,
|
|
double c6,
|
|
double minEccentricMultiplier,
|
|
double maxEccentricMultiplier,
|
|
const std::string& curveName,
|
|
SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate)
|
|
{
|
|
//Check the input arguments
|
|
if( !(c4 < c5) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createAndersonActiveTorqueVelocityCurve "
|
|
<< curveName
|
|
<< ": c4 must be greater than c5"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( !((c4 > 0)) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createAndersonActiveTorqueVelocityCurve "
|
|
<< curveName
|
|
<< ": c4 must be greater than 0"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( !(c6 > 0.0) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createAndersonActiveTorqueVelocityCurve "
|
|
<< curveName
|
|
<< ": c6 must be greater than 1.0"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( !(minEccentricMultiplier > 1.0) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createAndersonActiveTorqueVelocityCurve "
|
|
<< curveName
|
|
<< ": minEccentricMultiplier must be greater than 1.0"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( !(maxEccentricMultiplier > minEccentricMultiplier) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createAndersonActiveTorqueVelocityCurve "
|
|
<< curveName
|
|
<< ": maxEccentricMultiplier must be greater than "
|
|
<< " minEccentricMultiplier"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
//Advanced settings that we'll hide for now
|
|
double minShoulderSlopeMagnitude = 0;
|
|
double curviness = 0.75;
|
|
double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness);
|
|
|
|
//Go and get the value of the curve that is closest to
|
|
//the maximum contraction velocity by setting rhs of Eqn. 9
|
|
//to 0 and solving
|
|
double dthMaxConc = abs( 2.0*c4*c5/(c5-3.0*c4) );
|
|
|
|
//Go and evaluate the concentric side of the Anderson curve
|
|
//at 1/2 of omega max - we need this to use the updated
|
|
//torque-velocity curve.
|
|
double wMid = dthMaxConc*0.50;
|
|
double tvMidDen = (2*c4*c5 + wMid*(2*c5-4*c4));
|
|
double tvMid = (2*c4*c5 + wMid*(c5-3*c4))/tvMidDen;
|
|
|
|
tvMid = min(tvMid,0.45);
|
|
double tvMaxEcc = 1.1 + c6*0.2;
|
|
|
|
createTorqueVelocityCurve(tvMaxEcc,
|
|
tvMid,
|
|
curveName,
|
|
smoothSegmentedFunctionToUpdate);
|
|
|
|
|
|
}
|
|
//=============================================================================
|
|
// ANDERSON 2007 Passive Torque Angle Curve
|
|
//=============================================================================
|
|
void TorqueMuscleFunctionFactory::
|
|
createAnderson2007PassiveTorqueAngleCurve(
|
|
double scale,
|
|
double c1,
|
|
double b1,
|
|
double k1,
|
|
double b2,
|
|
double k2,
|
|
const std::string& curveName,
|
|
SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate)
|
|
{
|
|
|
|
if( !(scale > 0) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createAnderson2007PassiveTorqueAngleCurve "
|
|
<< curveName
|
|
<< ": scale must be greater than 0"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( !(c1 > 0) ) {
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createAnderson2007PassiveTorqueAngleCurve "
|
|
<< curveName
|
|
<< ": c1 must be greater than 0"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
//Advanced settings that we'll hide for now
|
|
double curviness = 0.75;
|
|
double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness);
|
|
double minShoulderSlopeMagnitude = 0;
|
|
|
|
//Zero out the coefficients associated with a
|
|
//the side of the curve that goes negative.
|
|
bool flag_oneSided = true;
|
|
|
|
if(flag_oneSided){
|
|
if(fabs(b1) > 0){
|
|
if(b1 > 0){
|
|
b2 = 0;
|
|
k2 = 0;
|
|
}else{
|
|
b1 = 0;
|
|
k1 = 0;
|
|
}
|
|
|
|
}else if(fabs(b2) > 0){
|
|
if(b2 > 0){
|
|
b1 = 0;
|
|
k1 = 0;
|
|
}else{
|
|
b2 = 0;
|
|
k2 = 0;
|
|
}
|
|
}
|
|
}
|
|
//Divide up the curve into a left half
|
|
//and a right half, rather than 1 and 2.
|
|
//Why? These two different halves require different
|
|
// Bezier curves.
|
|
|
|
double c1Scale = c1*scale;
|
|
double thL = 0.; //left
|
|
double thR = 0.; //right
|
|
double DtauDthL = 0.;
|
|
double DtauDthR = 0.;
|
|
double bL = 0.;
|
|
double kL = 0.;
|
|
double bR = 0.;
|
|
double kR = 0.;
|
|
|
|
int curveType = 0; //flat curve
|
|
int flag_thL = 0;
|
|
int flag_thR = 0;
|
|
|
|
if(fabs(k1)>0 && fabs(b1)>0){
|
|
//The value of theta where the passive force generated by the
|
|
//muscle is equal to 1 maximum isometric contraction.
|
|
thL = (1/k1)*log(fabs( c1Scale/b1 ));
|
|
DtauDthL = b1*k1*exp(thL*k1);
|
|
bL = b1;
|
|
kL = k1;
|
|
flag_thL = 1;
|
|
}
|
|
|
|
if(fabs(k2)>0 && fabs(b2)>0){
|
|
//The value of theta where the passive force generated by the
|
|
//muscle is equal to 1 maximum isometric contraction.
|
|
thR = (1/k2)*log(fabs( c1Scale/b2 ));
|
|
DtauDthR = b2*k2*exp(thR*k2);
|
|
bR = b2;
|
|
kR = k2;
|
|
flag_thR = 1;
|
|
}
|
|
|
|
//A 'left' curve has a negative slope,
|
|
//A 'right' curve has a positive slope.
|
|
if(DtauDthL > DtauDthR){
|
|
double tmpD = thL;
|
|
thL = thR;
|
|
thR = tmpD;
|
|
|
|
tmpD = bR;
|
|
bR = bL;
|
|
bL = tmpD;
|
|
|
|
tmpD = kR;
|
|
kR = kL;
|
|
kL = tmpD;
|
|
|
|
tmpD = DtauDthL;
|
|
DtauDthL = DtauDthR;
|
|
DtauDthR = tmpD;
|
|
|
|
int tmpI = flag_thL;
|
|
flag_thL = flag_thR;
|
|
flag_thR = tmpI;
|
|
}
|
|
|
|
|
|
if(flag_thL){
|
|
curveType = curveType + 1;
|
|
}
|
|
if(flag_thR){
|
|
curveType = curveType + 2;
|
|
}
|
|
|
|
RigidBodyDynamics::Math::MatrixNd mX(6,1);
|
|
RigidBodyDynamics::Math::MatrixNd mY(6,1);
|
|
|
|
double xStart = 0;
|
|
double xEnd = 0;
|
|
double yStart = 0;
|
|
double yEnd = 0;
|
|
double dydxStart = 0;
|
|
double dydxEnd = 0;
|
|
|
|
|
|
switch(curveType){
|
|
|
|
case 0:
|
|
{//No curve - it's a flat line
|
|
RigidBodyDynamics::Math::MatrixNd p0 =
|
|
SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(0.,0.,0.,
|
|
1.,0.,0.,c);
|
|
|
|
mX.col(0) = p0.col(0);
|
|
mY.col(0) = p0.col(1);
|
|
|
|
}break;
|
|
case 1:
|
|
{
|
|
//Get a point on the curve that is close to 0.
|
|
double x1 = (1/kL)*log(fabs(0.01*c1Scale/bL) );
|
|
double y1 = bL*exp(kL*x1);
|
|
double dydx1 = bL*kL*exp(kL*x1);
|
|
|
|
//Get a point that is at 1 maximum isometric torque
|
|
double x3 = thL;
|
|
double y3 = bL*exp(kL*x3);
|
|
double dydx3 = bL*kL*exp(kL*x3);
|
|
|
|
//Get a mid-point
|
|
double x2 = 0.5*(x1+x3);
|
|
double y2 = bL*exp(kL*x2);
|
|
double dydx2 = bL*kL*exp(kL*x2);
|
|
|
|
//Past the crossing point of the linear extrapolation
|
|
double x0 = x1 - 2*y1/dydx1;
|
|
double y0 = 0;
|
|
double dydx0 = minShoulderSlopeMagnitude*copysign(1.0,dydx1);
|
|
|
|
xStart = x3;
|
|
xEnd = x0;
|
|
yStart = y3;
|
|
yEnd = y0;
|
|
dydxStart = dydx3;
|
|
dydxEnd = dydx0;
|
|
|
|
RigidBodyDynamics::Math::MatrixNd p0 =
|
|
SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(x3,y3,dydx3,
|
|
x2,y2,dydx2,c);
|
|
RigidBodyDynamics::Math::MatrixNd p1 =
|
|
SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(x2,y2,dydx2,
|
|
x1,y1,dydx1,c);
|
|
RigidBodyDynamics::Math::MatrixNd p2 =
|
|
SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(x1,y1,dydx1,
|
|
x0,y0,dydx0,c);
|
|
|
|
mX.resize(6,3);
|
|
mY.resize(6,3);
|
|
|
|
mX.col(0) = p0.col(0);
|
|
mY.col(0) = p0.col(1);
|
|
mX.col(1) = p1.col(0);
|
|
mY.col(1) = p1.col(1);
|
|
mX.col(2) = p2.col(0);
|
|
mY.col(2) = p2.col(1);
|
|
|
|
}break;
|
|
case 2:
|
|
{
|
|
//Get a point on the curve that is close to 0.
|
|
double x1 = (1/kR)*log(fabs(0.01*c1Scale/bR) );
|
|
double y1 = bR*exp(kR*x1);
|
|
double dydx1 = bR*kR*exp(kR*x1);
|
|
|
|
//Go just past the crossing point of the linear extrapolation
|
|
double x0 = x1 - 2*y1/dydx1;
|
|
double y0 = 0;
|
|
double dydx0 = minShoulderSlopeMagnitude*copysign(1.0,dydx1);
|
|
|
|
//Get a point close to 1 maximum isometric torque
|
|
double x3 = thR;
|
|
double y3 = bR*exp(kR*x3);
|
|
double dydx3 = bR*kR*exp(kR*x3);
|
|
|
|
//Get a mid point.
|
|
double x2 = 0.5*(x1+x3);
|
|
double y2 = bR*exp(kR*x2);
|
|
double dydx2 = bR*kR*exp(kR*x2);
|
|
|
|
xStart = x0;
|
|
xEnd = x3;
|
|
yStart = y0;
|
|
yEnd = y3;
|
|
dydxStart = dydx0;
|
|
dydxEnd = dydx3;
|
|
|
|
RigidBodyDynamics::Math::MatrixNd p0 =
|
|
SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(x0,y0,dydx0,
|
|
x1,y1,dydx1,c);
|
|
RigidBodyDynamics::Math::MatrixNd p1 =
|
|
SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(x1,y1,dydx1,
|
|
x2,y2,dydx2,c);
|
|
RigidBodyDynamics::Math::MatrixNd p2 =
|
|
SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(x2,y2,dydx2,
|
|
x3,y3,dydx3,c);
|
|
mX.resize(6,3);
|
|
mY.resize(6,3);
|
|
|
|
mX.col(0) = p0.col(0);
|
|
mY.col(0) = p0.col(1);
|
|
mX.col(1) = p1.col(0);
|
|
mY.col(1) = p1.col(1);
|
|
mX.col(2) = p2.col(0);
|
|
mY.col(2) = p2.col(1);
|
|
|
|
}break;
|
|
case 3:
|
|
{
|
|
//Only when flag_oneSided = false;
|
|
double x0 = thL;
|
|
double x4 = thR;
|
|
|
|
double x2 = 0.5*(x0 + x4);
|
|
double x1 = 0.5*(x0 + x2);
|
|
double x3 = 0.5*(x2 + x4);
|
|
|
|
double y0 = b1*exp(k1*x0)
|
|
+ b2*exp(k2*x0);
|
|
double y1 = b1*exp(k1*x1)
|
|
+ b2*exp(k2*x1);
|
|
double y2 = b1*exp(k1*x2)
|
|
+ b2*exp(k2*x2);
|
|
double y3 = b1*exp(k1*x3)
|
|
+ b2*exp(k2*x3);
|
|
double y4 = b1*exp(k1*x4)
|
|
+ b2*exp(k2*x4);
|
|
|
|
double dydx0 = b1*k1*exp(k1*x0)
|
|
+ b2*k2*exp(k2*x0);
|
|
double dydx1 = b1*k1*exp(k1*x1)
|
|
+ b2*k2*exp(k2*x1);
|
|
double dydx2 = b1*k1*exp(k1*x2)
|
|
+ b2*k2*exp(k2*x2);
|
|
double dydx3 = b1*k1*exp(k1*x3)
|
|
+ b2*k2*exp(k2*x3);
|
|
double dydx4 = b1*k1*exp(k1*x4)
|
|
+ b2*k2*exp(k2*x4);
|
|
|
|
xStart = x0;
|
|
xEnd = x4;
|
|
yStart = y0;
|
|
yEnd = y4;
|
|
dydxStart = dydx0;
|
|
dydxEnd = dydx4;
|
|
|
|
RigidBodyDynamics::Math::MatrixNd p0 =
|
|
SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(x0,y0,dydx0,
|
|
x1,y1,dydx1,c);
|
|
RigidBodyDynamics::Math::MatrixNd p1 =
|
|
SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(x1,y1,dydx1,
|
|
x2,y2,dydx2,c);
|
|
RigidBodyDynamics::Math::MatrixNd p2 =
|
|
SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(x2,y2,dydx2,
|
|
x3,y3,dydx3,c);
|
|
RigidBodyDynamics::Math::MatrixNd p3 =
|
|
SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(x3,y3,dydx3,
|
|
x4,y4,dydx4,c);
|
|
|
|
mX.resize(6,4);
|
|
mY.resize(6,4);
|
|
|
|
mX.col(0) = p0.col(0);
|
|
mY.col(0) = p0.col(1);
|
|
mX.col(1) = p1.col(0);
|
|
mY.col(1) = p1.col(1);
|
|
mX.col(2) = p2.col(0);
|
|
mY.col(2) = p2.col(1);
|
|
mX.col(3) = p3.col(0);
|
|
mY.col(3) = p3.col(1);
|
|
|
|
}break;
|
|
default:
|
|
{
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createAnderson2007PassiveTorqueAngleCurve "
|
|
<< curveName
|
|
<< ": undefined curveType"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
};
|
|
|
|
//Normalize the y values.
|
|
mY = mY*(1/c1Scale);
|
|
|
|
smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction(
|
|
mX, mY,
|
|
xStart, xEnd,
|
|
yStart/c1Scale, yEnd/c1Scale,
|
|
dydxStart/c1Scale, dydxEnd/c1Scale,
|
|
curveName);
|
|
}
|
|
|
|
//=============================================================================
|
|
//
|
|
// New torque-muscle characteristic curves
|
|
//
|
|
//=============================================================================
|
|
|
|
|
|
//=============================================================================
|
|
// torque-velocity curves
|
|
//=============================================================================
|
|
|
|
void TorqueMuscleFunctionFactory::createTorqueVelocityCurve(
|
|
double tvAtEccentricOmegaMax,
|
|
double tvAtHalfConcentricOmegaMax,
|
|
const std::string& curveName,
|
|
RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction&
|
|
smoothSegmentedFunctionToUpdate )
|
|
{
|
|
|
|
double slopeAtConcentricOmegaMax = 0.0;
|
|
double slopeNearEccentricOmegaMax = -0.025;
|
|
double slopeAtEccentricOmegaMax = 0.0;
|
|
double eccentricCurviness = 0.75;
|
|
|
|
createTorqueVelocityCurve(
|
|
tvAtEccentricOmegaMax,
|
|
tvAtHalfConcentricOmegaMax,
|
|
slopeAtConcentricOmegaMax,
|
|
slopeNearEccentricOmegaMax,
|
|
slopeAtEccentricOmegaMax,
|
|
eccentricCurviness,
|
|
curveName,
|
|
smoothSegmentedFunctionToUpdate);
|
|
|
|
}
|
|
|
|
void TorqueMuscleFunctionFactory::createTorqueVelocityCurve(
|
|
double tvAtEccentricOmegaMax,
|
|
double tvAtHalfConcentricOmegaMax,
|
|
double slopeAtConcentricOmegaMax,
|
|
double slopeNearEccentricOmegaMax,
|
|
double slopeAtEccentricOmegaMax,
|
|
double eccentricCurviness,
|
|
const std::string& curveName,
|
|
RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction&
|
|
smoothSegmentedFunctionToUpdate )
|
|
{
|
|
|
|
if( (tvAtEccentricOmegaMax < 1.05) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createTorqueVelocityCurve "
|
|
<< curveName
|
|
<< ": tvAtEccentricOmegaMax must be greater than 1.05"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( (tvAtHalfConcentricOmegaMax < 0.05
|
|
|| tvAtHalfConcentricOmegaMax > 0.45) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createTorqueVelocityCurve "
|
|
<< curveName
|
|
<< ": tvAtHalfOmegaMax must be in the interval [0.05,0.45]"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( (slopeAtConcentricOmegaMax > 0) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createTorqueVelocityCurve "
|
|
<< curveName
|
|
<< ": slopeAtConcentricOmegaMax cannot be less than 0"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( (slopeNearEccentricOmegaMax > 0) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createTorqueVelocityCurve "
|
|
<< curveName
|
|
<< ": slopeNearEccentricOmegaMax cannot be less than 0"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( (slopeAtEccentricOmegaMax > 0) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createTorqueVelocityCurve "
|
|
<< curveName
|
|
<< ": slopeAtEccentricOmegaMax cannot be less than 0"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( (eccentricCurviness < 0 || eccentricCurviness > 1.0) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createTorqueVelocityCurve "
|
|
<< curveName
|
|
<< ": eccentricCurviness must be in the interval [0,1]"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
|
|
double omegaMax = 1.0;
|
|
double wmaxC = omegaMax; //In biomechanics the concentric side gets a
|
|
//a +'ve signed velocity
|
|
double wmaxE = -omegaMax;
|
|
|
|
//-----------------------------------------------------------------------
|
|
// 1. Fit concentric Bezier curves to a Hill-type hyperbolic curve
|
|
//-----------------------------------------------------------------------
|
|
|
|
/*
|
|
First we compute the terms that are consistent with a Hill-type concentric
|
|
contraction. Starting from Hill's hyperbolic equation
|
|
|
|
f(w) = (fiso*b - a*w) / (b+w)
|
|
|
|
|
|
Taking a derivative w.r.t omega yields
|
|
|
|
df(w)/dw = [ (fiso*b - a*w + a*(b+w))] / (b+w)^2
|
|
|
|
at w = wmaxC the numerator goes to 0
|
|
|
|
(fiso*b - a*wmaxC) / (b + wmaxC) = 0
|
|
(fiso*b - a*wmaxC) = 0;
|
|
b = a*wmaxC/fiso;
|
|
|
|
Subtituting this expression for b into the expression when
|
|
f(wHalf) = tvAtHalfOmegaMax yields this expression for parameter a
|
|
|
|
a = tvAtHalfOmegaMax*w*fiso ...
|
|
/ (wmaxC*tvAtHalfOmegaMax + fiso*wmaxC - fiso*w);
|
|
|
|
*/
|
|
|
|
double fiso = 1.0;
|
|
double w = 0.5*wmaxC;
|
|
double a = -tvAtHalfConcentricOmegaMax*w*fiso
|
|
/ (wmaxC*tvAtHalfConcentricOmegaMax - fiso*wmaxC + fiso*w);
|
|
|
|
double b = a*wmaxC/fiso;
|
|
double yCheck = (b*fiso-a*w)/(b+w);
|
|
|
|
if( abs(yCheck-tvAtHalfConcentricOmegaMax) > SQRTEPS ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createTorqueVelocityCurve "
|
|
<< curveName
|
|
<< ": Internal error fitting the concentric curve to Hill's "
|
|
<< "hyperbolic curve. This error condition was true: "
|
|
<< "abs(yCheck-tvAtHalfOmegaMax) > sqrt(eps)"
|
|
<< "Consult the maintainers of this addon."
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
|
|
w = 0*wmaxC;
|
|
double dydxIso = (-(a)*(b+w) - (b*fiso-a*w)) / ((b+w)*(b+w));
|
|
|
|
|
|
w = 0.9*wmaxC;
|
|
double dydxNearC = (-(a)*(b+w) - (b*fiso-a*w)) / ((b+w)*(b+w));
|
|
|
|
|
|
if( dydxNearC > slopeAtConcentricOmegaMax || abs(dydxNearC) > abs(1/wmaxC) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createTorqueVelocityCurve "
|
|
<< curveName
|
|
<< ": Internal error fitting the concentric curve to Hill's "
|
|
<< "hyperbolic curve. This error condition was true: "
|
|
<< " dydxNearC < dydxC || dydxNearC > abs(1/wmaxC)"
|
|
<< "Consult the maintainers of this addon."
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Iterate over the curviness value to get a good match between Hill's
|
|
// hyperbolic curve and the Bezier curve
|
|
//----------------------------------------------------------------------------
|
|
|
|
double xNearC = 0.9*wmaxC;
|
|
double yNearC = (b*fiso-a*w)/(b+w);
|
|
double xIso = 0;
|
|
double yIso = 1.0;
|
|
|
|
double cC = 0.5;
|
|
|
|
MatrixNd pts = SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(xIso, yIso, dydxIso,
|
|
xNearC,yNearC, dydxNearC,
|
|
cC);
|
|
MatrixNd xpts(6,1);
|
|
xpts.col(0) = pts.col(0);
|
|
MatrixNd ypts(6,1);
|
|
ypts.col(0) = pts.col(1);
|
|
|
|
SmoothSegmentedFunction fvCFcn = SmoothSegmentedFunction(
|
|
xpts,ypts,
|
|
xIso,xNearC,
|
|
yIso,yNearC,
|
|
dydxIso,dydxNearC,
|
|
"tvFcn");
|
|
SmoothSegmentedFunction fvCFcnLeft = SmoothSegmentedFunction();
|
|
SmoothSegmentedFunction fvCFcnRight = SmoothSegmentedFunction();
|
|
|
|
int nSample = 10;
|
|
double f = 0;
|
|
double yHill = 0;
|
|
|
|
//Calculate the error of the Bezier curve with the starting curviness value
|
|
//of 0.5
|
|
for(int j=0; j<nSample;++j){
|
|
w = ((double)(j-1))*wmaxC/((double)nSample-1.0);
|
|
yHill = (b*fiso-a*w)/(b+w);
|
|
f += abs( fvCFcn.calcValue(w)-yHill);
|
|
}
|
|
|
|
double fBest = f;
|
|
double fLeft = 0.;
|
|
double fRight = 0.;
|
|
double cCBest = cC;
|
|
|
|
double cCLeft = 0;
|
|
double cCRight = 0;
|
|
double h = 0.25;
|
|
|
|
std::string fvLeftName("fvLeft");
|
|
std::string fvRightName("fvRight");
|
|
|
|
//Use the bisection method to solve for the curviness value that
|
|
//results in the best fit to Hill's hyperbola. Here we are using
|
|
//the bisection method rather than Newton's method because I had
|
|
//convergence problems with Newton's method in the prototype code.
|
|
for(int i=0; i<10;++i){
|
|
|
|
//Form the left-approaching curve and evaluate its error
|
|
cCLeft = cCBest - h;
|
|
|
|
pts = SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(
|
|
xIso, yIso, dydxIso,
|
|
xNearC,yNearC, dydxNearC,
|
|
cCLeft);
|
|
xpts.col(0) = pts.col(0);
|
|
ypts.col(0) = pts.col(1);
|
|
fvCFcnLeft.updSmoothSegmentedFunction(xpts, ypts,
|
|
xIso, xNearC,
|
|
yIso, yNearC,
|
|
dydxIso, dydxNearC,
|
|
fvLeftName);
|
|
fLeft = 0;
|
|
for(int j=0; j<nSample;++j){
|
|
w = ((double)(j-1))*wmaxC/((double)nSample-1.0);
|
|
yHill = (b*fiso-a*w)/(b+w);
|
|
fLeft += abs( fvCFcnLeft.calcValue(w)-yHill);
|
|
}
|
|
|
|
//Form the right-approaching curve and evaluate its error
|
|
cCRight = cCBest + h;
|
|
pts = SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(
|
|
xIso, yIso, dydxIso,
|
|
xNearC,yNearC, dydxNearC, cCRight);
|
|
xpts.col(0) = pts.col(0);
|
|
ypts.col(0) = pts.col(1);
|
|
fvCFcnRight.updSmoothSegmentedFunction(xpts, ypts,
|
|
xIso, xNearC,
|
|
yIso, yNearC,
|
|
dydxIso, dydxNearC,
|
|
fvRightName);
|
|
fRight = 0;
|
|
for(int j=0; j<nSample;++j){
|
|
w = ((double)(j-1))*wmaxC/((double)nSample-1.0);
|
|
yHill = (b*fiso-a*w)/(b+w);
|
|
fRight += abs( fvCFcnRight.calcValue(w)-yHill);
|
|
}
|
|
|
|
if(abs(fLeft)<abs(fBest)){
|
|
fBest = fLeft;
|
|
cCBest = cCLeft;
|
|
}
|
|
if(abs(fRight)<abs(fBest)){
|
|
fBest = fRight;
|
|
cCBest = cCRight;
|
|
}
|
|
|
|
h = h/2.0;
|
|
}
|
|
|
|
cC = cCBest;
|
|
double cE = SegmentedQuinticBezierToolkit::scaleCurviness(eccentricCurviness);
|
|
|
|
MatrixNd xM(6,4);
|
|
MatrixNd yM(6,4);
|
|
|
|
double xC = wmaxC;
|
|
double yC = 0.0;
|
|
|
|
double xE = wmaxE;
|
|
double yE = tvAtEccentricOmegaMax;
|
|
|
|
double xNearE = 0.9*wmaxE;
|
|
double dydxNearE = slopeAtEccentricOmegaMax;
|
|
double yNearE = yE + 0.5*dydxNearE*(xNearE-xE)
|
|
+ 0.5*slopeAtEccentricOmegaMax*(xNearE-xE);
|
|
|
|
|
|
pts = SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(
|
|
xE,yE, slopeAtEccentricOmegaMax,
|
|
xNearE,yNearE, dydxNearE,
|
|
cE);
|
|
|
|
xM.col(0) = pts.col(0);
|
|
yM.col(0) = pts.col(1);
|
|
|
|
pts = SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(
|
|
xNearE,yNearE, dydxNearE,
|
|
xIso,yIso,dydxIso,
|
|
cE);
|
|
|
|
xM.col(1) = pts.col(0);
|
|
yM.col(1) = pts.col(1);
|
|
|
|
|
|
pts = SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(
|
|
xIso,yIso,dydxIso,
|
|
xNearC,yNearC, dydxNearC,
|
|
cC);
|
|
|
|
xM.col(2) = pts.col(0);
|
|
yM.col(2) = pts.col(1);
|
|
|
|
|
|
pts = SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(
|
|
xNearC,yNearC, dydxNearC,
|
|
xC,yC, slopeAtConcentricOmegaMax,
|
|
cC);
|
|
|
|
xM.col(3) = pts.col(0);
|
|
yM.col(3) = pts.col(1);
|
|
|
|
|
|
smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction(
|
|
xM,yM,
|
|
xE,xC,
|
|
yE,yC,
|
|
slopeAtEccentricOmegaMax,
|
|
slopeAtConcentricOmegaMax,
|
|
curveName);
|
|
|
|
}
|
|
|
|
|
|
//=============================================================================
|
|
// passive-torque angle curve
|
|
//=============================================================================
|
|
void TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve(
|
|
double angleAtZeroTorque,
|
|
double angleAtOneNormTorque,
|
|
const std::string& curveName,
|
|
RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction&
|
|
smoothSegmentedFunctionToUpdate )
|
|
{
|
|
|
|
double stiffnessAtOneNormTorque =
|
|
4.6/(angleAtOneNormTorque-angleAtZeroTorque);
|
|
double stiffnessAtLowTorque = 0.01*stiffnessAtOneNormTorque;
|
|
double curviness = 0.5;
|
|
|
|
createPassiveTorqueAngleCurve(
|
|
angleAtZeroTorque,
|
|
angleAtOneNormTorque,
|
|
stiffnessAtLowTorque,
|
|
stiffnessAtOneNormTorque,
|
|
curviness,
|
|
curveName,
|
|
smoothSegmentedFunctionToUpdate);
|
|
|
|
}
|
|
|
|
void TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve(
|
|
double angleAtZeroTorque,
|
|
double angleAtOneNormTorque,
|
|
double stiffnessAtLowTorque,
|
|
double stiffnessAtOneNormTorque,
|
|
double curviness,
|
|
const std::string& curveName,
|
|
RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction&
|
|
smoothSegmentedFunctionToUpdate )
|
|
{
|
|
|
|
if( abs(angleAtOneNormTorque - angleAtZeroTorque) <= SQRTEPS){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createPassiveTorqueAngleCurve "
|
|
<< curveName
|
|
<< ": abs(angleAtOneNormTorque - angleAtZeroTorque) must be "
|
|
<< "greater than sqrt(eps)"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( abs(stiffnessAtLowTorque)
|
|
> (0.9/abs(angleAtOneNormTorque-angleAtZeroTorque)) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createPassiveTorqueAngleCurve "
|
|
<< curveName
|
|
<< ": stiffnessAtLowTorque has a magnitude that is too "
|
|
<< "large, it exceeds 0.9/abs(angleAtOneNormTorque-angleAtZeroTorque)"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( abs(stiffnessAtOneNormTorque)
|
|
< (1.1/abs(angleAtOneNormTorque-angleAtZeroTorque)) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createPassiveTorqueAngleCurve "
|
|
<< curveName
|
|
<< ": stiffnessAtOneNormTorque has a magnitude that is too "
|
|
<< "small, it is less than 1.1/abs(angleAtOneNormTorque-angleAtZeroTorque)"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( stiffnessAtOneNormTorque*stiffnessAtLowTorque < 0.0 ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createPassiveTorqueAngleCurve "
|
|
<< curveName
|
|
<< ": stiffnessAtLowTorque and stiffnessAtOneNormTorque must have the"
|
|
<< " same sign."
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( stiffnessAtOneNormTorque
|
|
*(angleAtOneNormTorque-angleAtZeroTorque) < 0.0){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createPassiveTorqueAngleCurve "
|
|
<< curveName
|
|
<< ": stiffnessAtOneNormTorque must have the same sign as "
|
|
<< "(angleAtOneNormTorque-angleAtZeroTorque)"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( (curviness < 0 || curviness > 1.0) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createPassiveTorqueAngleCurve "
|
|
<< curveName
|
|
<< ": curviness must be in the interval [0,1]"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
MatrixNd xM(6,2);
|
|
MatrixNd yM(6,2);
|
|
MatrixNd pts(6,2);
|
|
|
|
double x0,x1,y0,y1,dydx0,dydx1;
|
|
if(angleAtZeroTorque < angleAtOneNormTorque){
|
|
x0 = angleAtZeroTorque;
|
|
x1 = angleAtOneNormTorque;
|
|
y0 = 0.;
|
|
y1 = 1.;
|
|
dydx0 = 0.0;
|
|
dydx1 = stiffnessAtOneNormTorque;
|
|
}else{
|
|
x0 = angleAtOneNormTorque;
|
|
x1 = angleAtZeroTorque;
|
|
y0 = 1.0;
|
|
y1 = 0.0;
|
|
dydx0 = stiffnessAtOneNormTorque;
|
|
dydx1 = 0.0;
|
|
}
|
|
|
|
double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness);
|
|
|
|
if(abs(stiffnessAtOneNormTorque) > SQRTEPS){
|
|
|
|
double delta = min( 0.1*(1.0-abs(1.0/stiffnessAtOneNormTorque)),
|
|
0.05*abs(x1-x0));
|
|
if(stiffnessAtOneNormTorque < 0.){
|
|
delta *= -1.0;
|
|
}
|
|
|
|
double xLow = angleAtZeroTorque + delta;
|
|
double xFoot = angleAtZeroTorque + 0.5*(xLow-angleAtZeroTorque);
|
|
double yFoot = 0.0;
|
|
double yLow = yFoot + stiffnessAtLowTorque*(xLow-xFoot);
|
|
|
|
pts = SegmentedQuinticBezierToolkit::calcQuinticBezierCornerControlPoints(
|
|
x0, y0, dydx0,
|
|
xLow,yLow,stiffnessAtLowTorque, c);
|
|
xM.col(0) = pts.col(0);
|
|
yM.col(0) = pts.col(1);
|
|
|
|
pts = SegmentedQuinticBezierToolkit::calcQuinticBezierCornerControlPoints(
|
|
xLow,yLow,stiffnessAtLowTorque,
|
|
x1, y1, dydx1, c);
|
|
xM.col(1) = pts.col(0);
|
|
yM.col(1) = pts.col(1);
|
|
|
|
}else{
|
|
pts = SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(
|
|
angleAtZeroTorque,0, 0,
|
|
angleAtOneNormTorque,0, 0,
|
|
c);
|
|
xM.col(0) = pts.col(0);
|
|
yM.col(1) = pts.col(1);
|
|
}
|
|
|
|
smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction(
|
|
xM, yM,
|
|
x0, x1,
|
|
y0, y1,
|
|
dydx0, dydx1,
|
|
curveName);
|
|
|
|
|
|
}
|
|
|
|
//=============================================================================
|
|
// active-torque angle curve
|
|
//=============================================================================
|
|
|
|
|
|
void TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve(
|
|
double angleAtOneNormTorque,
|
|
double angularStandardDeviation,
|
|
const std::string& curveName,
|
|
RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction&
|
|
smoothSegmentedFunctionToUpdate )
|
|
{
|
|
|
|
createGaussianShapedActiveTorqueAngleCurve(
|
|
angleAtOneNormTorque,
|
|
angularStandardDeviation,
|
|
0.0,
|
|
0.0,
|
|
0.5,
|
|
curveName,
|
|
smoothSegmentedFunctionToUpdate );
|
|
|
|
}
|
|
|
|
|
|
void TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve(
|
|
double angleAtOneNormTorque,
|
|
double angularStandardDeviation,
|
|
double minSlopeAtShoulders,
|
|
double minValueAtShoulders,
|
|
double curviness,
|
|
const std::string& curveName,
|
|
RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction&
|
|
smoothSegmentedFunctionToUpdate )
|
|
{
|
|
|
|
if( (angularStandardDeviation < SQRTEPS) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createGaussianShapedActiveTorqueAngleCurve "
|
|
<< curveName
|
|
<< ": angularStandardDeviation is less than sqrt(eps)"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( (minValueAtShoulders < 0) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createGaussianShapedActiveTorqueAngleCurve "
|
|
<< curveName
|
|
<< ": minValueAtShoulders is less than 0"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
|
|
if( (minSlopeAtShoulders < 0) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createGaussianShapedActiveTorqueAngleCurve "
|
|
<< curveName
|
|
<< ": minSlopeAtShoulders is less than 0"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( (curviness < 0 || curviness > 1.0) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createGaussianShapedActiveTorqueAngleCurve "
|
|
<< curveName
|
|
<< ": curviness must be in the interval [0,1]"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
double taCutoff = 1e-3;
|
|
if(taCutoff < minValueAtShoulders ){
|
|
taCutoff = minValueAtShoulders;
|
|
}
|
|
double angularStandardDeviationSq =
|
|
angularStandardDeviation*angularStandardDeviation;
|
|
double thetaWidth = sqrt(-log(taCutoff)*2*angularStandardDeviationSq);
|
|
double thetaMin = -thetaWidth + angleAtOneNormTorque;
|
|
double thetaMax = thetaWidth + angleAtOneNormTorque;
|
|
|
|
double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness);
|
|
|
|
double x0 = thetaMin;
|
|
|
|
double x1 = angleAtOneNormTorque - angularStandardDeviation;
|
|
double x2 = angleAtOneNormTorque;
|
|
double x3 = angleAtOneNormTorque + angularStandardDeviation;
|
|
|
|
if( (angleAtOneNormTorque-thetaMin) <
|
|
(angleAtOneNormTorque-angularStandardDeviation)){
|
|
x1 = angleAtOneNormTorque - 0.5*thetaMin;
|
|
x3 = angleAtOneNormTorque + 0.5*thetaMin;
|
|
}
|
|
|
|
double x4 = thetaMax;
|
|
|
|
double y0 = minValueAtShoulders;
|
|
double y1 = exp(-(x1-angleAtOneNormTorque)*(x1-angleAtOneNormTorque)
|
|
/ (2*angularStandardDeviationSq) );
|
|
double y2 = exp(-(x2-angleAtOneNormTorque)*(x2-angleAtOneNormTorque)
|
|
/ (2*angularStandardDeviationSq) );
|
|
double y3 = exp(-(x3-angleAtOneNormTorque)*(x3-angleAtOneNormTorque)
|
|
/ (2*angularStandardDeviationSq) );
|
|
double y4 = minValueAtShoulders;
|
|
|
|
|
|
double dydx1 = -(2*(x1-angleAtOneNormTorque)
|
|
/ (2*angularStandardDeviationSq)) * y1;
|
|
double dydx2 = -(2*(x2-angleAtOneNormTorque)
|
|
/ (2*angularStandardDeviationSq)) * y2;
|
|
double dydx3 = -(2*(x3-angleAtOneNormTorque)
|
|
/ (2*angularStandardDeviationSq)) * y3;
|
|
|
|
double dydx0 = minSlopeAtShoulders;
|
|
double dydx4 = minSlopeAtShoulders;
|
|
|
|
if(dydx1 < 0){
|
|
dydx0 *= -1.0;
|
|
}
|
|
if(dydx3 < 0){
|
|
dydx4 *= -1.0;
|
|
}
|
|
|
|
|
|
MatrixNd xM(6,4);
|
|
MatrixNd yM(6,4);
|
|
MatrixNd pts(6,2);
|
|
|
|
pts = SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(
|
|
x0,y0, dydx0,
|
|
x1,y1, dydx1, c);
|
|
|
|
xM.col(0) = pts.col(0);
|
|
yM.col(0) = pts.col(1);
|
|
|
|
pts = SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(
|
|
x1,y1, dydx1,
|
|
x2,y2, dydx2, c);
|
|
|
|
xM.col(1) = pts.col(0);
|
|
yM.col(1) = pts.col(1);
|
|
|
|
pts = SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(
|
|
x2,y2, dydx2,
|
|
x3,y3, dydx3, c);
|
|
|
|
xM.col(2) = pts.col(0);
|
|
yM.col(2) = pts.col(1);
|
|
|
|
pts = SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(
|
|
x3,y3, dydx3,
|
|
x4,y4, dydx4, c);
|
|
|
|
xM.col(3) = pts.col(0);
|
|
yM.col(3) = pts.col(1);
|
|
|
|
smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction(
|
|
xM, yM,
|
|
x0, x4,
|
|
y0, y4,
|
|
dydx0,dydx4,
|
|
curveName);
|
|
|
|
}
|
|
|
|
|
|
//=============================================================================
|
|
// tendon-torque angle curve
|
|
//=============================================================================
|
|
|
|
void TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve(
|
|
double angularStretchAtOneNormTorque,
|
|
const std::string& curveName,
|
|
RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction&
|
|
smoothSegmentedFunctionToUpdate )
|
|
{
|
|
|
|
createTendonTorqueAngleCurve(
|
|
angularStretchAtOneNormTorque,
|
|
1.5/angularStretchAtOneNormTorque,
|
|
1.0/3.0,
|
|
0.5,
|
|
curveName,
|
|
smoothSegmentedFunctionToUpdate );
|
|
|
|
}
|
|
|
|
|
|
void TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve(
|
|
double angularStretchAtOneNormTorque,
|
|
double stiffnessAtOneNormTorque,
|
|
double normTorqueAtToeEnd,
|
|
double curviness,
|
|
const std::string& curveName,
|
|
RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction&
|
|
smoothSegmentedFunctionToUpdate )
|
|
{
|
|
|
|
|
|
if( angularStretchAtOneNormTorque < SQRTEPS ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createTendonTorqueAngleCurve "
|
|
<< curveName
|
|
<< ": angularStretchAtOneNormTorque should be greater than sqrt(eps)"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( stiffnessAtOneNormTorque < 1.1/angularStretchAtOneNormTorque){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createTendonTorqueAngleCurve "
|
|
<< curveName
|
|
<< ": stiffnessAtOneNormTorque should be greater "
|
|
<< " than 1.1/angularStretchAtOneNormTorque"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( normTorqueAtToeEnd < SQRTEPS || normTorqueAtToeEnd > 0.99){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createTendonTorqueAngleCurve "
|
|
<< curveName
|
|
<< ": normTorqueAtToeEnd must be in the inteval [sqrt(eps), 0.99]"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( (curviness < 0 || curviness > 1.0) ){
|
|
cerr << "TorqueMuscleFunctionFactory::"
|
|
<< "createTendonTorqueAngleCurve "
|
|
<< curveName
|
|
<< ": curviness must be in the interval [0,1]"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness);
|
|
double x0 = 0;
|
|
double y0 = 0;
|
|
double dydx0 = 0;
|
|
|
|
double xIso = angularStretchAtOneNormTorque;
|
|
double yIso = 1;
|
|
double dydxIso = stiffnessAtOneNormTorque;
|
|
|
|
//Location where the curved section becomes linear
|
|
double yToe = normTorqueAtToeEnd;
|
|
double xToe = (yToe-1)/stiffnessAtOneNormTorque + xIso;
|
|
|
|
|
|
//To limit the 2nd derivative of the toe region the line it tends to
|
|
//has to intersect the x axis to the right of the origin
|
|
double xFoot = (xToe)/10.0;
|
|
double yFoot = 0;
|
|
|
|
|
|
//Compute the location of the corner formed by the average slope of the
|
|
//toe and the slope of the linear section
|
|
double yToeMid = yToe*0.5;
|
|
double xToeMid = (yToeMid-yIso)/stiffnessAtOneNormTorque + xIso;
|
|
double dydxToeMid = (yToeMid-yFoot)/(xToeMid-xFoot);
|
|
|
|
//Compute the location of the control point to the left of the corner
|
|
double xToeCtrl = xFoot + 0.5*(xToeMid-xFoot);
|
|
double yToeCtrl = yFoot + dydxToeMid*(xToeCtrl-xFoot);
|
|
|
|
MatrixNd xM(6,2);
|
|
MatrixNd yM(6,2);
|
|
MatrixNd pts(6,2);
|
|
|
|
//Compute the Quintic Bezier control points
|
|
pts = SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(x0, y0, dydx0,
|
|
xToeCtrl,yToeCtrl,dydxToeMid, c);
|
|
xM.col(0) = pts.col(0);
|
|
yM.col(0) = pts.col(1);
|
|
|
|
pts = SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(xToeCtrl, yToeCtrl, dydxToeMid,
|
|
xToe, yToe, dydxIso, c);
|
|
xM.col(1) = pts.col(0);
|
|
yM.col(1) = pts.col(1);
|
|
|
|
smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction(xM,yM,
|
|
x0,xToe,
|
|
y0,yToe,
|
|
dydx0,dydxIso,
|
|
curveName);
|
|
|
|
}
|