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

993 lines
32 KiB
C++
Raw Normal View History

Initial commit
2020-10-03 22:55:14 +02:00
#include "TorqueMuscleFittingToolkit.h"
#include "coin/IpIpoptApplication.hpp"
using namespace Ipopt;
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;
void TorqueMuscleFittingToolkit::
fitTorqueMuscleParameters(
Millard2016TorqueMuscle const &tqMcl,
VectorNd const &jointAngle,
VectorNd const &jointAngularVelocity,
VectorNd const &jointTorque,
double activationUpperBound,
double passiveTorqueAngleMultiplierUpperBound,
TorqueMuscleParameterFittingData &parametersOfBestFit,
bool verbose)
{
if(tqMcl.mMuscleCurvesAreDirty){
Millard2016TorqueMuscle* mutableTqMcl =
const_cast<Millard2016TorqueMuscle* >(&tqMcl);
mutableTqMcl->updateTorqueMuscleCurves();
}
assert(jointAngle.rows() > 1);
assert(jointAngularVelocity.rows() > 1);
assert(jointTorque.rows() > 1);
unsigned int nrows = jointAngle.rows();
double activationLowerBound = 0.0;
assert(jointAngularVelocity.rows() == nrows);
assert(jointTorque.rows() == nrows);
assert(activationUpperBound > 0 && activationUpperBound <= 1.0);
assert(activationLowerBound >= 0 && activationLowerBound<activationUpperBound);
VectorNd jointTorqueSingleSided;
jointTorqueSingleSided.resize(jointTorque.size());
for(unsigned int i=0; i<jointTorque.size();++i){
if(tqMcl.getJointTorqueSign()*jointTorque[i] >= 0){
jointTorqueSingleSided[i] = jointTorque[i];
}else{
jointTorqueSingleSided[i] = 0.0;
}
}
double tiso = tqMcl.getMaximumActiveIsometricTorque();
double omegaMax = tqMcl.getMaximumConcentricJointAngularVelocity();
double maxConcentricAngularVelocityData = 0;
for(unsigned int i=0; i<jointAngularVelocity.rows();++i){
if(fabs(jointAngularVelocity[i])
> fabs(maxConcentricAngularVelocityData)){
maxConcentricAngularVelocityData = fabs(jointAngularVelocity[i]);
}
}
//Set ta and tv blending variables to a small finite value so that
//it is possible to solve for an activation of finite value when
//calcTorqueMuscleDataFeatures is called
double taLambda = sqrt(SQRTEPSILON);
double tvLambda = sqrt(SQRTEPSILON);
if(tqMcl.getActiveTorqueAngleCurveBlendingVariable() > taLambda){
taLambda = tqMcl.getActiveTorqueAngleCurveBlendingVariable();
}
if(tqMcl.getTorqueAngularVelocityCurveBlendingVariable() > tvLambda){
tvLambda = tqMcl.getTorqueAngularVelocityCurveBlendingVariable();
}
double tpLambda = 0.0;
double taAngleScaling = tqMcl.mTaAngleScaling;
double taAngleAtOneNormTorque = tqMcl.mAngleAtOneNormActiveTorque;
double tvOmegaMaxScale = 1.0;
double tpOffset = 0.0;
double objectiveValue = 0.0;
double constraintError = 0.0;
TorqueMuscleDataFeatures tmf;
omegaMax = fabs(tqMcl.getMaximumConcentricJointAngularVelocity()
*tvOmegaMaxScale);
//If the optimization run fails, the values from tmf are passed out to
//the user to help them debug the problem.
tqMcl.calcTorqueMuscleDataFeatures(
jointTorqueSingleSided,jointAngle,jointAngularVelocity,
taLambda,tpLambda,tvLambda,
taAngleScaling,taAngleAtOneNormTorque,tpOffset,omegaMax,
tiso,tmf);
Millard2016TorqueMuscle* mutableTqMcl =
const_cast<Millard2016TorqueMuscle* >(&tqMcl);
SmartPtr<FitTorqueMuscleParameters> fittingProblem
= new FitTorqueMuscleParameters(
jointAngle,
jointAngularVelocity,
jointTorqueSingleSided,
activationUpperBound,
passiveTorqueAngleMultiplierUpperBound,
taLambda,
tvLambda,
*mutableTqMcl);
SmartPtr<IpoptApplication> app = IpoptApplicationFactory();
app->RethrowNonIpoptException(true);
double solnTol = SQRTEPSILON;
app->Options()->SetNumericValue("tol",solnTol);
app->Options()->SetStringValue("mu_strategy","adaptive");
if(!verbose){
app->Options()->SetIntegerValue("print_level",0);
}
ApplicationReturnStatus status;
objectiveValue = std::numeric_limits<double>::infinity();
constraintError= std::numeric_limits<double>::infinity();
bool converged = false;
double taAngleScaleStart = 1.0;
double tvOmegaMaxScaleStart =fabs(1.1*maxConcentricAngularVelocityData
/tqMcl.getMaximumConcentricJointAngularVelocity());
double tisoScale = 1.0;
bool updateStart = false;
status = app->Initialize();
if(status == Solve_Succeeded){
status = app->OptimizeTNLP(fittingProblem);
if(status == Solve_Succeeded){
converged = true;
updateStart = true;
taAngleScaling = fittingProblem
->getSolutionActiveTorqueAngleAngleScaling();
tvOmegaMaxScale = fittingProblem
->getSolutionTorqueAngularVelocityOmegaMaxScale();
tpLambda = fittingProblem
->getSolutionPassiveTorqueAngleBlendingParameter();
tpOffset = fittingProblem
->getSolutionPassiveTorqueAngleCurveOffset();
tisoScale = fittingProblem
->getSolutionMaximumActiveIsometricTorqueScale();
tiso = tisoScale * tiso;
omegaMax = fabs(tvOmegaMaxScale
*tqMcl.getMaximumConcentricJointAngularVelocity());
objectiveValue = fittingProblem->getObjectiveValue();
constraintError= fittingProblem->getConstraintError().norm();
tqMcl.calcTorqueMuscleDataFeatures(
jointTorqueSingleSided,jointAngle,jointAngularVelocity,
taLambda,tpLambda,tvLambda,
taAngleScaling,taAngleAtOneNormTorque,tpOffset,omegaMax,
tiso,tmf);
}
}
if(converged){
parametersOfBestFit.fittingConverged = true;
parametersOfBestFit.isTorqueMuscleActive = !(tmf.isInactive);
parametersOfBestFit.indexAtMaximumActivation=tmf.indexOfMaxActivation;
parametersOfBestFit.indexAtMinimumActivation=tmf.indexOfMinActivation;
parametersOfBestFit.indexAtMaxPassiveTorqueAngleMultiplier
=tmf.indexOfMaxPassiveTorqueAngleMultiplier;
parametersOfBestFit.activeTorqueAngleBlendingVariable = taLambda;
parametersOfBestFit.torqueVelocityBlendingVariable = tvLambda;
parametersOfBestFit.passiveTorqueAngleBlendingVariable = tpLambda;
parametersOfBestFit.passiveTorqueAngleCurveOffset = tpOffset;
parametersOfBestFit.maximumActiveIsometricTorque = tiso;
parametersOfBestFit.activeTorqueAngleAngleScaling = taAngleScaling;
parametersOfBestFit.maximumAngularVelocity = fabs(omegaMax);
parametersOfBestFit.summaryDataAtMaximumActivation
=tmf.summaryAtMaxActivation;
parametersOfBestFit.summaryDataAtMinimumActivation
=tmf.summaryAtMinActivation;
parametersOfBestFit.summaryDataAtMaximumPassiveTorqueAngleMultiplier
=tmf.summaryAtMaxPassiveTorqueAngleMultiplier;
}else{
parametersOfBestFit.fittingConverged = false;
parametersOfBestFit.isTorqueMuscleActive = !(tmf.isInactive);
parametersOfBestFit.indexAtMaximumActivation
=numeric_limits<unsigned int>::signaling_NaN();
parametersOfBestFit.indexAtMinimumActivation
=numeric_limits<unsigned int>::signaling_NaN();
parametersOfBestFit.indexAtMaxPassiveTorqueAngleMultiplier
=numeric_limits<unsigned int>::signaling_NaN();
parametersOfBestFit.activeTorqueAngleBlendingVariable
= numeric_limits<double>::signaling_NaN();
parametersOfBestFit.passiveTorqueAngleBlendingVariable
= numeric_limits<double>::signaling_NaN();
parametersOfBestFit.passiveTorqueAngleCurveOffset
= numeric_limits<double>::signaling_NaN();
parametersOfBestFit.torqueVelocityBlendingVariable
= numeric_limits<double>::signaling_NaN();
parametersOfBestFit.maximumActiveIsometricTorque
= numeric_limits<double>::signaling_NaN();
parametersOfBestFit.activeTorqueAngleAngleScaling
= numeric_limits<double>::signaling_NaN();
parametersOfBestFit.maximumAngularVelocity
= numeric_limits<double>::signaling_NaN();
parametersOfBestFit.summaryDataAtMaximumActivation
=tmf.summaryAtMaxActivation;
parametersOfBestFit.summaryDataAtMinimumActivation
=tmf.summaryAtMinActivation;
parametersOfBestFit.summaryDataAtMaximumPassiveTorqueAngleMultiplier
=tmf.summaryAtMaxPassiveTorqueAngleMultiplier;
}
}
FitTorqueMuscleParameters::
FitTorqueMuscleParameters(
const RigidBodyDynamics::Math::VectorNd &jointAngle,
const RigidBodyDynamics::Math::VectorNd &jointAngularVelocity,
const RigidBodyDynamics::Math::VectorNd &jointTorque,
double maxActivation,
double maxPassiveTorqueAngleMultiplier,
double taLambda,
double tvLambda,
Millard2016TorqueMuscle &tqMcl):
mJointAngle(jointAngle),
mJointAngularVelocity(jointAngularVelocity),
mJointTorque(jointTorque),
mTqMcl(tqMcl)
{
assert(jointTorque.rows() == jointAngle.rows());
assert(jointTorque.rows() == jointAngularVelocity.rows());
//Initialize all member variables
mN = 5;
mM = 3*jointTorque.rows();
double angleMin = std::numeric_limits< double >::max();
double omegaMin = std::numeric_limits< double >::max();
double angleMax = -std::numeric_limits< double >::max();
double omegaMax = -std::numeric_limits< double >::max();
double tauMax = 0;
for(unsigned int i=0; i<mJointAngle.rows();++i){
if(mJointAngle[i] < angleMin){
angleMin = mJointAngle[i];
}
if(mJointAngle[i] > angleMax){
angleMax = mJointAngle[i];
}
if(mJointAngularVelocity[i] < omegaMin){
omegaMin = mJointAngularVelocity[i];
}
if(mJointAngularVelocity[i] > omegaMax){
omegaMax = mJointAngularVelocity[i];
}
if(mJointTorque[i]*mTqMcl.getJointTorqueSign() > tauMax){
tauMax = mJointTorque[i]*mTqMcl.getJointTorqueSign();
}
}
double tvScale = max(fabs(omegaMin),fabs(omegaMax))
/fabs(mTqMcl.getMaximumConcentricJointAngularVelocity());
double taScale = fabs(angleMax-angleMin)
/mTqMcl.getActiveTorqueAngleCurveWidth();
double tauScale = tauMax/fabs(mTqMcl.getMaximumActiveIsometricTorque());
mConIdxTauActMaxStart = 0;
mConIdxTauActMaxEnd = jointTorque.rows()-1;
mConIdxTauActMinStart = jointTorque.rows();
mConIdxTauActMinEnd = 2*jointTorque.rows()-1;
mConIdxTauPassiveStart = 2*jointTorque.rows();
mConIdxTauPassiveEnd = 3*jointTorque.rows()-1;
mMaxActivation = maxActivation;
mMinActivation = 0.0;
mMaxTp = maxPassiveTorqueAngleMultiplier;
mTauIso = mTqMcl.getMaximumActiveIsometricTorque();
mTaLambda = taLambda;
mTvLambda = tvLambda;
mTaAngleAtOneNormTorque = mTqMcl.mAngleAtOneNormActiveTorque;
mOmegaMax = mTqMcl.mOmegaMax;
mTaAngleScaleStart = max(taScale,
mTqMcl.getActiveTorqueAngleCurveAngleScaling());
mTvOmegaMaxScaleStart = max(tvScale, 1.);
mTpLambdaStart = mTqMcl.getPassiveTorqueAngleCurveBlendingVariable();
mTpAngleOffsetStart = mTqMcl.getPassiveCurveAngleOffset();
mTauScalingStart = max(tauScale, 1.);
mTaAngleScaleLB = 1.;
mTvOmegaMaxScaleLB = 1.;
mTpLambdaLB = 0.;
mTpAngleOffsetLB = -M_PI*0.5;
mTauScalingLB = 1.0;
mTaAngleScaleUB = 2.0e19;
mTvOmegaMaxScaleUB = 2.0e19;
mTpLambdaUB = 1.0;
mTpAngleOffsetUB = M_PI*0.5;
mTauScalingUB = 2.0e19;
mIndexTaAngleScale = 0;
mIndexTvOmegaMaxScale = 1;
mIndexTpLambda = 2;
mIndexTpOffset = 3;
mIndexTauScaling = 4;
//Initialize vectors
mConstraintErrors.resize(mM);
mXOffset.resize(mN);
mWeights.resize(mN);
for(unsigned int i=0; i<mWeights.rows(); ++i){
mWeights[i] = 1.0;
}
mXOffset.resize(mN);
mXOffset[0] = mTqMcl.getActiveTorqueAngleCurveAngleScaling();
mXOffset[1] = 1.0;
mXOffset[2] = mTqMcl.getPassiveTorqueAngleCurveBlendingVariable();
mXOffset[3] = mTqMcl.getPassiveCurveAngleOffset();
mXOffset[4] = 1.0;
for(unsigned int i=0; i< mM; ++i){
mConstraintErrors[i] = 0.;
}
mObjValue = 0.;
}
//Manditory functions of the TNLP interface
bool FitTorqueMuscleParameters::
get_nlp_info(Ipopt::Index &n,
Ipopt::Index &m,
Ipopt::Index &nnz_jac_g,
Ipopt::Index &nnz_h_lag,
Ipopt::TNLP::IndexStyleEnum &index_style)
{
//Parameters
// lambda_A
// lambda_V
// tisoScaling
n = mN;
//Constraints
// a_max*(tisoScaled)*fal*fv+fpe - tau_measured >= 0
m = mM;
//Constraint Jacobian
//This is a dense Jacobian
nnz_jac_g = mM*mN;
//Hessian
//This is a symmetric matrix 5x5 matrix, but its densley populated so
//we must fill in the bottom left triangle.
nnz_h_lag = 15;
index_style = Ipopt::TNLP::C_STYLE;
return true;
}
bool FitTorqueMuscleParameters::
get_bounds_info(Ipopt::Index n,
Ipopt::Number *x_l,
Ipopt::Number *x_u,
Ipopt::Index m,
Ipopt::Number *g_l,
Ipopt::Number *g_u)
{
assert(n==mN);
assert(m==mM);
x_l[0] = mTaAngleScaleLB;
x_l[1] = mTvOmegaMaxScaleLB;
x_l[2] = mTpLambdaLB;
x_l[3] = mTpAngleOffsetLB;
x_l[4] = mTauScalingLB;
x_u[0] = mTaAngleScaleUB;
x_u[1] = mTvOmegaMaxScaleUB;
x_u[2] = mTpLambdaUB;
x_u[3] = mTpAngleOffsetUB;
x_u[4] = mTauScalingUB;
//The optimization routine will only strengthen
//the muscle to satisfy the constraint - but it
//will not weaken it.
double tauActMaxLB = 0;
double tauActMaxUB = 0;
double tauActMinLB = 0;
double tauActMinUB = 0;
double tauPassiveLB = 0;
double tauPassiveUB = 0;
if(mTqMcl.getJointTorqueSign() > 0){
tauActMaxLB = 0;
tauActMaxUB = 2e19;
tauActMinLB = -2e19;
tauActMinUB = 0;
tauPassiveLB = -2e19;
tauPassiveUB = 0;
}else{
tauActMaxLB = -2.e19;
tauActMaxUB = 0.;
tauActMinLB = 0;
tauActMinUB = 2e19;
tauPassiveLB = -2e19;
tauPassiveUB = 0;
}
for(unsigned int i = mConIdxTauActMaxStart; i <= mConIdxTauActMaxEnd; ++i){
g_l[i] = tauActMaxLB;
g_u[i] = tauActMaxUB;
}
for(unsigned int i = mConIdxTauActMinStart; i <= mConIdxTauActMinEnd; ++i){
g_l[i] = tauActMinLB;
g_u[i] = tauActMinUB;
}
for(unsigned int i = mConIdxTauPassiveStart; i <= mConIdxTauPassiveEnd; ++i){
g_l[i] = tauPassiveLB;
g_u[i] = tauPassiveUB;
}
return true;
}
bool FitTorqueMuscleParameters::
get_starting_point(Ipopt::Index n,
bool init_x,
Ipopt::Number *x,
bool init_z,
Ipopt::Number *z_L,
Ipopt::Number *z_U,
Ipopt::Index m,
bool init_lambda,
Ipopt::Number *lambda)
{
assert(n==mN);
x[0] = mTaAngleScaleStart;
x[1] = mTvOmegaMaxScaleStart;
x[2] = mTpLambdaStart;
x[3] = mTpAngleOffsetStart;
x[4] = mTauScalingStart;
init_x = true;
init_z = false;
init_lambda = false;
mDtp_Dx.resize(mN);
return true;
}
bool FitTorqueMuscleParameters::
eval_f(Ipopt::Index n,
const Ipopt::Number *x,
bool new_x,
Ipopt::Number &obj_value)
{
assert(n==mN);
obj_value = 0.;
double xUpd = 0.;
for(unsigned i=0; i<mN; ++i){
xUpd = x[i]-mXOffset[i];
obj_value += mWeights[i]*xUpd*xUpd;
}
new_x = false;
return true;
}
bool FitTorqueMuscleParameters::
eval_grad_f(Ipopt::Index n,
const Ipopt::Number *x,
bool new_x,
Ipopt::Number *grad_f)
{
assert(n==mN);
for(unsigned int i=0; i<(unsigned int)n;++i){
grad_f[i] = 0.;
}
double xUpd = 0;
for(unsigned int i=0; i<(unsigned int)n;++i){
xUpd = x[i]-mXOffset[i];
grad_f[i] = 2.0*mWeights[i]*xUpd;
}
return true;
}
bool FitTorqueMuscleParameters::
eval_g(Ipopt::Index n,
const Ipopt::Number *x,
bool new_x,
Ipopt::Index m,
Ipopt::Number *g)
{
assert(n==mN);
assert(m==mM);
updOptimizationVariables(x);
double tauMax = mTauScaling*mTauIso;
double omegaMax = mTvOmegaMaxScale*mOmegaMax;
unsigned int j = 0;
for(unsigned int i = mConIdxTauActMaxStart; i <= mConIdxTauActMaxEnd; ++i){
mTqMcl.updTorqueMuscleSummary(mMaxActivation,
mJointAngle[j],
mJointAngularVelocity[j],
mTaLambda,
mTpLambda,
mTvLambda,
mTaAngleScale,
mTaAngleAtOneNormTorque,
mTpAngleOffset,
omegaMax,
tauMax,
mTms);
g[i] = mTms.jointTorque - mJointTorque[j];
++j;
}
j=0;
unsigned int k = mConIdxTauPassiveStart;
for(unsigned int i = mConIdxTauActMinStart; i <= mConIdxTauActMinEnd; ++i){
mTqMcl.updTorqueMuscleSummary(mMinActivation,
mJointAngle[j],
mJointAngularVelocity[j],
mTaLambda,
mTpLambda,
mTvLambda,
mTaAngleScale,
mTaAngleAtOneNormTorque,
mTpAngleOffset,
omegaMax,
tauMax,
mTms);
g[i] = mTms.jointTorque - mJointTorque[j];
g[k] = mTms.fiberPassiveTorqueAngleMultiplier - mMaxTp;
++j;
++k;
}
return true;
}
bool FitTorqueMuscleParameters::
eval_jac_g(Ipopt::Index n,
const Ipopt::Number *x,
bool new_x,
Ipopt::Index m,
Ipopt::Index nele_jac,
Ipopt::Index *iRow,
Ipopt::Index *jCol,
Ipopt::Number *values)
{
assert(n==mN);
assert(m==mM);
if(values == NULL){
unsigned int idx = 0;
for(unsigned int rows = 0; rows < mM; ++rows){
for(unsigned int cols = 0; cols < mN; ++cols){
iRow[idx] = rows;
jCol[idx] = cols;
++idx;
}
}
}else{
updOptimizationVariables(x);
double tauMax = mTauScaling*mTauIso;
double omegaMax = mTvOmegaMaxScale*mOmegaMax;
unsigned int idx = 0;
unsigned int j = 0;
for(unsigned int i = mConIdxTauActMaxStart; i <= mConIdxTauActMaxEnd; ++i){
mTqMcl.updTorqueMuscleInfo( mMaxActivation,
mJointAngle[j],
mJointAngularVelocity[j],
mTaLambda,
mTpLambda,
mTvLambda,
mTaAngleScale,
mTaAngleAtOneNormTorque,
mTpAngleOffset,
omegaMax,
tauMax,
mTmi);
values[idx] = mTmi.DjointTorque_DactiveTorqueAngleAngleScaling;
++idx;
values[idx] = mTmi.DjointTorque_DmaximumAngularVelocity*mOmegaMax;
++idx;
values[idx] = mTmi.DjointTorque_DpassiveTorqueAngleBlendingVariable;
++idx;
values[idx] = mTmi.DjointTorque_DpassiveTorqueAngleCurveAngleOffset;
++idx;
values[idx] = mTmi.jointTorque/mTauScaling;
++idx;
++j;
}
j=0;
for(unsigned int i = mConIdxTauActMinStart; i <= mConIdxTauActMinEnd; ++i){
mTqMcl.updTorqueMuscleInfo( mMinActivation,
mJointAngle[j],
mJointAngularVelocity[j],
mTaLambda,
mTpLambda,
mTvLambda,
mTaAngleScale,
mTaAngleAtOneNormTorque,
mTpAngleOffset,
omegaMax,
tauMax,
mTmi);
values[idx] = mTmi.DjointTorque_DactiveTorqueAngleAngleScaling;
++idx;
values[idx] = mTmi.DjointTorque_DmaximumAngularVelocity*mOmegaMax;
++idx;
values[idx] = mTmi.DjointTorque_DpassiveTorqueAngleBlendingVariable;
++idx;
values[idx] = mTmi.DjointTorque_DpassiveTorqueAngleCurveAngleOffset;
++idx;
values[idx] = mTmi.jointTorque/mTauScaling;
++idx;
++j;
}
j=0;
for(unsigned int i= mConIdxTauPassiveStart; i <= mConIdxTauPassiveEnd; ++i){
mTqMcl.updTorqueMuscleInfo(0.,
mJointAngle[j],
mJointAngularVelocity[j],
mTaLambda,
mTpLambda,
mTvLambda,
mTaAngleScale,
mTaAngleAtOneNormTorque,
mTpAngleOffset,
omegaMax,
tauMax,
mTmi);
values[idx] = 0.;
++idx;
values[idx] = 0.;
++idx;
values[idx] = mTmi.DfiberPassiveTorqueAngleMultiplier_DblendingVariable;
++idx;
values[idx] = mTmi.DfiberPassiveTorqueAngleMultiplier_DangleOffset;
++idx;
values[idx] = 0.;
++idx;
++j;
}
}
return true;
}
void FitTorqueMuscleParameters::
finalize_solution(Ipopt::SolverReturn status,
Ipopt::Index n,
const Ipopt::Number *x,
const Ipopt::Number *z_L,
const Ipopt::Number *z_U,
Ipopt::Index m,
const Ipopt::Number *g,
const Ipopt::Number *lambda,
Ipopt:: Number obj_value,
const Ipopt::IpoptData *ip_data,
Ipopt::IpoptCalculatedQuantities *ip_cq)
{
assert(n==mN);
assert(m==mM);
updOptimizationVariables(x);
mObjValue = obj_value;
for(unsigned int i=0; i<mM; ++i){
mConstraintErrors[i] = g[i];
}
}
/*
This is incorrect - the constraint has a non-zero Hessian.
*/
bool FitTorqueMuscleParameters::
eval_h(Ipopt::Index n,
const Ipopt::Number *x,
bool new_x,
Ipopt::Number obj_factor,
Ipopt::Index m,
const Ipopt::Number *lambda,
bool new_lambda,
Ipopt::Index nele_hess,
Ipopt::Index *iRow,
Ipopt::Index *jCol,
Ipopt::Number *values)
{
assert(n==mN);
assert(m==mM);
if(values==NULL){
unsigned int ele = 0;
for(unsigned int row = 0; row < mN; ++row){
for(unsigned int col = 0; col <= row; ++col){
iRow[ele] = row;
jCol[ele] = col;
++ele;
}
}
assert(ele == (unsigned int)nele_hess);
}else{
/*
[K ] [D2tau_DlambdaA2 ]
[ K ] + lambda_i [D2tau_DlambdaADlambdaV D2tau_DlambdaV2 ]
[ K] [D2tau_DlambdaADtauS D2tau_DlambdaVDtauS D2tau_DtauS2]
*/
updOptimizationVariables(x);
double tauMax = mTauScaling*mTauIso;
double omegaMax = mTvOmegaMaxScale*mOmegaMax;
for(unsigned int i = 0; i < nele_hess; ++i){
values[i] = 0.;
}
values[0] = obj_factor*2.0*mWeights[0];
values[2] = obj_factor*2.0*mWeights[1];
values[5] = obj_factor*2.0*mWeights[2];
values[9] = obj_factor*2.0*mWeights[3];
values[14]= obj_factor*2.0*mWeights[4];
//Add in the components of the Hessian due to the constraints.
double d2t_ds2 = 0.;
double d2t_dsdm = 0.;
double d2t_dm2 = 0.;
double d2t_dsdp = 0.;
double d2t_dmdp = 0.;
double d2t_dp2 = 0.;
double d2t_dsdo = 0.;
double d2t_dmdo = 0.;
double d2t_dpdo = 0.;
double d2t_do2 = 0.;
double dt_ds = 0.;
double dt_dm = 0.;
double dt_dp = 0.;
double dt_do = 0.;
double d2tp_dp2 = 0.;
double d2tp_dpdo= 0.;
double d2tp_do2 = 0.;
unsigned int j = 0;
for(unsigned int i = mConIdxTauActMaxStart; i <= mConIdxTauActMaxEnd; ++i){
mTqMcl.updTorqueMuscleInfo( mMaxActivation,
mJointAngle[j],
mJointAngularVelocity[j],
mTaLambda,
mTpLambda,
mTvLambda,
mTaAngleScale,
mTaAngleAtOneNormTorque,
mTpAngleOffset,
omegaMax,
tauMax,
mTmi);
d2t_ds2 = mTmi.fittingInfo[13];
d2t_dsdm = mTmi.fittingInfo[14]*mOmegaMax;
d2t_dm2 = mTmi.fittingInfo[15]*mOmegaMax*mOmegaMax;
d2t_dsdp = mTmi.fittingInfo[16];
d2t_dmdp = mTmi.fittingInfo[17]*mOmegaMax;
d2t_dp2 = mTmi.fittingInfo[18];
d2t_dsdo = mTmi.fittingInfo[19];
d2t_dmdo = mTmi.fittingInfo[20]*mOmegaMax;
d2t_dpdo = mTmi.fittingInfo[21];
d2t_do2 = mTmi.fittingInfo[22];
dt_ds = mTmi.DjointTorque_DactiveTorqueAngleAngleScaling;
dt_dm = mTmi.DjointTorque_DmaximumAngularVelocity*mOmegaMax;
dt_dp = mTmi.DjointTorque_DpassiveTorqueAngleBlendingVariable;
dt_do = mTmi.DjointTorque_DpassiveTorqueAngleCurveAngleOffset;
values[0] += lambda[i]*d2t_ds2;
values[1] += lambda[i]*d2t_dsdm;
values[2] += lambda[i]*d2t_dm2;
values[3] += lambda[i]*d2t_dsdp;
values[4] += lambda[i]*d2t_dmdp;
values[5] += lambda[i]*d2t_dp2;
values[6] += lambda[i]*d2t_dsdo;
values[7] += lambda[i]*d2t_dmdo;
values[8] += lambda[i]*d2t_dpdo;
values[9] += lambda[i]*d2t_do2;
values[10] += lambda[i]*dt_ds/mTauScaling;
values[11] += lambda[i]*dt_dm/mTauScaling;
values[12] += lambda[i]*dt_dp/mTauScaling;
values[13] += lambda[i]*dt_do/mTauScaling;
values[14] += lambda[i]*0.;
++j;
}
j=0;
for(unsigned int i = mConIdxTauActMinStart; i <= mConIdxTauActMinEnd; ++i){
mTqMcl.updTorqueMuscleInfo( mMinActivation,
mJointAngle[j],
mJointAngularVelocity[j],
mTaLambda,
mTpLambda,
mTvLambda,
mTaAngleScale,
mTaAngleAtOneNormTorque,
mTpAngleOffset,
omegaMax,
tauMax,
mTmi);
d2t_ds2 = mTmi.fittingInfo[13];
d2t_dsdm = mTmi.fittingInfo[14]*mOmegaMax;
d2t_dm2 = mTmi.fittingInfo[15]*mOmegaMax*mOmegaMax;
d2t_dsdp = mTmi.fittingInfo[16];
d2t_dmdp = mTmi.fittingInfo[17]*mOmegaMax;
d2t_dp2 = mTmi.fittingInfo[18];
d2t_dsdo = mTmi.fittingInfo[19];
d2t_dmdo = mTmi.fittingInfo[20]*mOmegaMax;
d2t_dpdo = mTmi.fittingInfo[21];
d2t_do2 = mTmi.fittingInfo[22];
dt_ds = mTmi.DjointTorque_DactiveTorqueAngleAngleScaling;
dt_dm = mTmi.DjointTorque_DmaximumAngularVelocity*mOmegaMax;
dt_dp = mTmi.DjointTorque_DpassiveTorqueAngleBlendingVariable;
dt_do = mTmi.DjointTorque_DpassiveTorqueAngleCurveAngleOffset;
values[0] += lambda[i]*d2t_ds2;
values[1] += lambda[i]*d2t_dsdm;
values[2] += lambda[i]*d2t_dm2;
values[3] += lambda[i]*d2t_dsdp;
values[4] += lambda[i]*d2t_dmdp;
values[5] += lambda[i]*d2t_dp2;
values[6] += lambda[i]*d2t_dsdo;
values[7] += lambda[i]*d2t_dmdo;
values[8] += lambda[i]*d2t_dpdo;
values[9] += lambda[i]*d2t_do2;
values[10] += lambda[i]*dt_ds/mTauScaling;
values[11] += lambda[i]*dt_dm/mTauScaling;
values[12] += lambda[i]*dt_dp/mTauScaling;
values[13] += lambda[i]*dt_do/mTauScaling;
values[14] += lambda[i]*0.;
++j;
}
//Room for improvement: combine this loop with the previous one.
//But, be sure not to screw up idx.
j=0;
for(unsigned int i= mConIdxTauPassiveStart; i <= mConIdxTauPassiveEnd; ++i){
mTqMcl.updTorqueMuscleInfo(0.,
mJointAngle[j],
mJointAngularVelocity[j],
mTaLambda,
mTpLambda,
mTvLambda,
mTaAngleScale,
mTaAngleAtOneNormTorque,
mTpAngleOffset,
omegaMax,
tauMax,
mTmi);
d2tp_dp2 = mTmi.fittingInfo[10];
d2tp_dpdo= mTmi.fittingInfo[11];
d2tp_do2 = mTmi.fittingInfo[12];
values[5] += lambda[i]*d2tp_dp2; //
values[8] += lambda[i]*d2tp_dpdo; //
values[9] += lambda[i]*d2tp_do2; //
++j;
}
}
return true;
}
double FitTorqueMuscleParameters::
getSolutionActiveTorqueAngleAngleScaling()
{
return mTaAngleScale;
}
double FitTorqueMuscleParameters::
getSolutionTorqueAngularVelocityOmegaMaxScale()
{
return mTvOmegaMaxScale;
}
double FitTorqueMuscleParameters::
getSolutionPassiveTorqueAngleBlendingParameter()
{
return mTpLambda;
}
double FitTorqueMuscleParameters::
getSolutionPassiveTorqueAngleCurveOffset()
{
return mTpAngleOffset;
}
double FitTorqueMuscleParameters::
getSolutionMaximumActiveIsometricTorqueScale()
{
return mTauScaling;
}
double FitTorqueMuscleParameters::getObjectiveValue()
{
return mObjValue;
}
RigidBodyDynamics::Math::VectorNd&
FitTorqueMuscleParameters::getConstraintError()
{
return mConstraintErrors;
}
void FitTorqueMuscleParameters::
updOptimizationVariables(const Ipopt::Number *x){
mTaAngleScale = x[0];
mTvOmegaMaxScale = x[1];
mTpLambda = x[2];
mTpAngleOffset = x[3];
mTauScaling = x[4];
}