356 lines
13 KiB
C
356 lines
13 KiB
C
|
#ifndef TORQUEMUSCLEFITTINGTOOLKIT_H_
|
||
|
|
#define TORQUEMUSCLEFITTINGTOOLKIT_H_
|
||
|
|
|
||
|
|
|
||
|
|
#include <vector>
|
||
|
|
#include <rbdl/rbdl_math.h>
|
||
|
|
#include "coin/IpTNLP.hpp"
|
||
|
|
#include "Millard2016TorqueMuscle.h"
|
||
|
|
|
||
|
|
namespace RigidBodyDynamics {
|
||
|
|
namespace Addons {
|
||
|
|
namespace Muscle{
|
||
|
|
|
||
|
|
|
||
|
|
class TorqueMuscleFittingToolkit {
|
||
|
|
public:
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
\brief This function will adjust the parameters of the
|
||
|
|
muscle-torque-generator (MTG) so that the MTG is strong enough and
|
||
|
|
flexible enough that it can produce the measured output torque
|
||
|
|
across the vector of measured joint angles and angular
|
||
|
|
velocities without exceeding the desired activation upper
|
||
|
|
bound, nor the passive forces exceeding the prescribed upper bound.
|
||
|
|
This function requires that IPOPT is installed
|
||
|
|
and that cmake has been correctly configured to point at
|
||
|
|
IPOPT's install directory. For a detailed description of the method please
|
||
|
|
see : M.Millard, A.L.Kleesattel, M.Harant, & K.Mombaur. A reduced muscle
|
||
|
|
model and planar musculoskeletal model fit for the synthesis of whole body
|
||
|
|
movements. Journal of Biomechanics. (under review as of August 2018)
|
||
|
|
|
||
|
|
The method adjusts five parameters:
|
||
|
|
|
||
|
|
-# \f$s^A\f$: the scale of the domain (angle) of the
|
||
|
|
active-torque-angle-curve. This scaling is applied so that
|
||
|
|
the angle which corresponds to the peak value of the
|
||
|
|
active-torque-angle-curve remains unchanged.
|
||
|
|
-# \f$s^V\f$: the scaling applied to the maximum angular velocity at
|
||
|
|
which the concentric side of the torque-angular-velocity
|
||
|
|
curve goes to zero.
|
||
|
|
-# \f$\lambda^P\f$: the passive-torque-angle-blending-variable
|
||
|
|
-# \f$\Delta^P\f$: the passive-torque-angle-offset
|
||
|
|
-# \f$s^\tau\f$: the scaling applied to \f$\tau_{o}\f$ the
|
||
|
|
maximum active isometric torque
|
||
|
|
|
||
|
|
These 5 parameters are a subset of the fitting variables that are available,
|
||
|
|
and are illustrated below:
|
||
|
|
|
||
|
|
\image html fig_MuscleAddon_BlendableTorqueMuscle.png "Examples of the adjustable characteristic curves"
|
||
|
|
|
||
|
|
|
||
|
|
To peform the fitting, the user must provide vectors of the
|
||
|
|
joint angle, joint angular velocity, and the joint torque
|
||
|
|
for the joint that the muscle-torque-generator (MTG) actuates. The sign of
|
||
|
|
all of these quantities must be consistent with the multibody
|
||
|
|
and the torque muscle. IPOPT solves for the fitting parameters
|
||
|
|
\f$\mathbf{x}\f$ such that the final result is as close as possible to the
|
||
|
|
values the MTG has before the fitting is performed \f$\mathbf{x}_o\f$
|
||
|
|
\f[
|
||
|
|
(\mathbf{x}-\mathbf{x}_o)^T \mathbf{K} (\mathbf{x}-\mathbf{x}_o)
|
||
|
|
\f]
|
||
|
|
such that at every point \f$i\f$ in the vector of joint angles, velocities,
|
||
|
|
and torques the following constraints are satisfied
|
||
|
|
\f[
|
||
|
|
\tau^M(a^{max},\theta_i,\dot{\theta}_i,\mathbf{x})-\tau_i \ge 0
|
||
|
|
\f]
|
||
|
|
\f[
|
||
|
|
\tau^M(0,\theta_i,\dot{\theta}_i,\mathbf{x})-\tau_i \le 0
|
||
|
|
\f]
|
||
|
|
\f[
|
||
|
|
\mathbf{t}_{P}(\theta_i) - \mathbf{t}_{P}^{max} \le 0
|
||
|
|
\f]
|
||
|
|
\f[
|
||
|
|
s^A, s^V, s^{\tau} \ge 1
|
||
|
|
\f]
|
||
|
|
\f[
|
||
|
|
1 \ge \lambda^P \ge 0
|
||
|
|
\f]
|
||
|
|
\f[
|
||
|
|
\pi/2 \ge \Delta^P \ge -\pi/2.
|
||
|
|
\f]
|
||
|
|
These constraints ensure that the muscle can deliver the
|
||
|
|
requested torque with an activation of \f$a^{max}\f$ or less, such that
|
||
|
|
the active forces of the muscle are never fighting the passive forces,
|
||
|
|
and that passive forces that are \f$\mathbf{t}_{P}^{max}\f$ or less.
|
||
|
|
The bound limits on the various optimization variables ensure that the
|
||
|
|
muscle is not made to be weaker or slower than the default settings for
|
||
|
|
the muscle. Thus if your subject is weaker than the default settings
|
||
|
|
for the muscle, you should first weaken the MTG and then run this function.
|
||
|
|
|
||
|
|
<b>Notes</b>
|
||
|
|
-# This routine will return the parameters that
|
||
|
|
fit the data, even if they are unreasonable: it is your
|
||
|
|
responsibility to have a look at this data to sanity check it
|
||
|
|
before using it.
|
||
|
|
-# This function will <b>not</b> update the internal
|
||
|
|
parameters of the model - it just calculates what they need
|
||
|
|
to be and returns them in the
|
||
|
|
TorqueMuscleParametersFittingData structure. If you decide
|
||
|
|
that these parameters are parameters are suitable, then they
|
||
|
|
can be assigned to the model using the function
|
||
|
|
Millard2016TorqueMuscle.setFittedParameters. The solve and set functions
|
||
|
|
have been deliberately split up to reflect the inherentely
|
||
|
|
iterative, and interactive nature of fitting the strength
|
||
|
|
of a musculoskeletal model to data.
|
||
|
|
-# Before attempting to fit the strength of the muscles
|
||
|
|
to experimental data we recommend that you pre-process the
|
||
|
|
data first. Normally this involves filtering the joint angles
|
||
|
|
using a zero-phase 2nd order low-pass Butterworth filter with
|
||
|
|
a cutoff frequency of 8-10 Hz (for human data). This can be
|
||
|
|
achieved in Matlab using the functions 'butter' and
|
||
|
|
'filtfilt'. Following this filtering step joint angular
|
||
|
|
velocities can be safely calculated numerically using a
|
||
|
|
central-difference method. If you fail to filter measured
|
||
|
|
joint angles it is likely that the joint angular velocities
|
||
|
|
you calculate will be noisy and will result in a terrible
|
||
|
|
fit. This function is not magic, and so the rule
|
||
|
|
garbage in, garbage out applies.
|
||
|
|
|
||
|
|
@param tqMcl:
|
||
|
|
The Millard2016TorqueMuscle object that is to be fitted.
|
||
|
|
@param jointAngle (radians)
|
||
|
|
A vector of measured joint angles. The sign of this vector
|
||
|
|
must be consistent with the multibody model and this
|
||
|
|
torque muscle.
|
||
|
|
@param jointAngularVelocity (radians/s)
|
||
|
|
A vector of joint angular velocities.
|
||
|
|
The sign of this vector must be consistent with the
|
||
|
|
multibody model and this torque muscle.
|
||
|
|
@param jointTorque (Nm)
|
||
|
|
A vector of joint torques. Only the
|
||
|
|
values in this vector that have the same torque sign as
|
||
|
|
the muscle are considered during the fitting process. This
|
||
|
|
means, inherently, that the muscle fitting is done assuming that
|
||
|
|
there is no co-contraction. If you want co-contraction to be included
|
||
|
|
in the fitting process, you must preprocess this vector so that it just
|
||
|
|
contains the (estimated) single-signed torque of the muscle in question.
|
||
|
|
The sign of this vector must be consistent with the
|
||
|
|
multibody model and this torque muscle.
|
||
|
|
@param activationUpperBound (0,1]
|
||
|
|
The maximum activation that is permitted for the given
|
||
|
|
set of joint angles, joint angular velocities, and
|
||
|
|
joint torques.
|
||
|
|
@param passiveTorqueAngleMultiplierUpperBound (0, \f$\infty\f$]
|
||
|
|
The maximum passive torque angle multipler that you expect
|
||
|
|
to see when the muscle moves through the values in
|
||
|
|
jointAngle. Note that a value of 1 is quite large: this
|
||
|
|
corresponds to 1 maximum active isometric torque.
|
||
|
|
@param parametersOfBestFit
|
||
|
|
A structure that contains the parameters of best fit along
|
||
|
|
with a summary of the internal variables of the muscle
|
||
|
|
model at the points in the data where the peak passive
|
||
|
|
forces are developed and where the maximum activation is
|
||
|
|
developed.
|
||
|
|
@param verbose
|
||
|
|
Setting this to will print information from IPOPT to the screen
|
||
|
|
during the solution process.
|
||
|
|
|
||
|
|
*/
|
||
|
|
static void fitTorqueMuscleParameters(
|
||
|
|
Millard2016TorqueMuscle const &tqMcl,
|
||
|
|
RigidBodyDynamics::Math::VectorNd const &jointAngle,
|
||
|
|
RigidBodyDynamics::Math::VectorNd const &jointAngularVelocity,
|
||
|
|
RigidBodyDynamics::Math::VectorNd const &jointTorque,
|
||
|
|
double activationUpperBound,
|
||
|
|
double passiveTorqueAngleMultiplierUpperBound,
|
||
|
|
TorqueMuscleParameterFittingData ¶metersOfBestFit,
|
||
|
|
bool verbose=false);
|
||
|
|
|
||
|
|
|
||
|
|
};
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
//A class that defines the virtual functions needed
|
||
|
|
//to use Ipopt to solve the problem
|
||
|
|
class FitTorqueMuscleParameters : public Ipopt::TNLP{
|
||
|
|
public:
|
||
|
|
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);
|
||
|
|
|
||
|
|
|
||
|
|
//Manditory functions of the TNLP interface
|
||
|
|
virtual bool get_nlp_info(Ipopt::Index &n,
|
||
|
|
Ipopt::Index &m,
|
||
|
|
Ipopt::Index &nnz_jac_g,
|
||
|
|
Ipopt::Index &nnz_h_lag,
|
||
|
|
Ipopt::TNLP::IndexStyleEnum &index_style);
|
||
|
|
|
||
|
|
|
||
|
|
virtual bool 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);
|
||
|
|
|
||
|
|
|
||
|
|
virtual bool 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);
|
||
|
|
|
||
|
|
virtual bool eval_f(Ipopt::Index n,
|
||
|
|
const Ipopt::Number *x,
|
||
|
|
bool new_x,
|
||
|
|
Ipopt::Number &obj_value);
|
||
|
|
|
||
|
|
virtual bool eval_grad_f(Ipopt::Index n,
|
||
|
|
const Ipopt::Number *x,
|
||
|
|
bool new_x,
|
||
|
|
Ipopt::Number *grad_f);
|
||
|
|
|
||
|
|
virtual bool eval_g(Ipopt::Index n,
|
||
|
|
const Ipopt::Number *x,
|
||
|
|
bool new_x,
|
||
|
|
Ipopt::Index m,
|
||
|
|
Ipopt::Number *g);
|
||
|
|
|
||
|
|
virtual bool 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);
|
||
|
|
|
||
|
|
virtual void 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);
|
||
|
|
|
||
|
|
//Optional functions
|
||
|
|
virtual bool 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);
|
||
|
|
|
||
|
|
double getSolutionActiveTorqueAngleAngleScaling();
|
||
|
|
double getSolutionPassiveTorqueAngleBlendingParameter();
|
||
|
|
double getSolutionTorqueAngularVelocityOmegaMaxScale();
|
||
|
|
double getSolutionPassiveTorqueAngleCurveOffset();
|
||
|
|
double getSolutionMaximumActiveIsometricTorqueScale();
|
||
|
|
double getObjectiveValue();
|
||
|
|
RigidBodyDynamics::Math::VectorNd& getConstraintError();
|
||
|
|
|
||
|
|
void updOptimizationVariables(const Ipopt::Number *x);
|
||
|
|
private:
|
||
|
|
unsigned int mN, mM;
|
||
|
|
|
||
|
|
double mMaxActivation;
|
||
|
|
double mMinActivation;
|
||
|
|
double mMaxTp;
|
||
|
|
double mTauIso;
|
||
|
|
|
||
|
|
double mTaAngleAtOneNormTorque;
|
||
|
|
double mOmegaMax;
|
||
|
|
double mTaLambda;
|
||
|
|
double mTvLambda;
|
||
|
|
|
||
|
|
|
||
|
|
double mTaAngleScaleStart;
|
||
|
|
double mTvOmegaMaxScaleStart;
|
||
|
|
double mTpLambdaStart;
|
||
|
|
double mTpAngleOffsetStart;
|
||
|
|
double mTauScalingStart;
|
||
|
|
|
||
|
|
|
||
|
|
double mTaAngleScaleLB;
|
||
|
|
double mTvOmegaMaxScaleLB;
|
||
|
|
double mTpLambdaLB;
|
||
|
|
double mTpAngleOffsetLB;
|
||
|
|
double mTauScalingLB;
|
||
|
|
|
||
|
|
|
||
|
|
double mTaAngleScaleUB;
|
||
|
|
double mTvOmegaMaxScaleUB;
|
||
|
|
double mTpLambdaUB;
|
||
|
|
double mTpAngleOffsetUB;
|
||
|
|
double mTauScalingUB;
|
||
|
|
|
||
|
|
|
||
|
|
double mTaAngleScale;
|
||
|
|
double mTvOmegaMaxScale;
|
||
|
|
double mTpLambda;
|
||
|
|
double mTpAngleOffset;
|
||
|
|
double mTauScaling;
|
||
|
|
|
||
|
|
unsigned int mIndexTaAngleScale;
|
||
|
|
unsigned int mIndexTvOmegaMaxScale;
|
||
|
|
unsigned int mIndexTpLambda;
|
||
|
|
unsigned int mIndexTpOffset;
|
||
|
|
unsigned int mIndexTauScaling;
|
||
|
|
|
||
|
|
unsigned int mConIdxTauActMaxStart;
|
||
|
|
unsigned int mConIdxTauActMaxEnd;
|
||
|
|
unsigned int mConIdxTauActMinStart;
|
||
|
|
unsigned int mConIdxTauActMinEnd;
|
||
|
|
unsigned int mConIdxTauPassiveStart;
|
||
|
|
unsigned int mConIdxTauPassiveEnd;
|
||
|
|
|
||
|
|
|
||
|
|
const RigidBodyDynamics::Math::VectorNd &mJointAngle;
|
||
|
|
const RigidBodyDynamics::Math::VectorNd &mJointAngularVelocity;
|
||
|
|
const RigidBodyDynamics::Math::VectorNd &mJointTorque;
|
||
|
|
|
||
|
|
RigidBodyDynamics::Math::VectorNd mXOffset;
|
||
|
|
RigidBodyDynamics::Math::VectorNd mDtp_Dx;
|
||
|
|
RigidBodyDynamics::Math::VectorNd mConstraintErrors;
|
||
|
|
double mObjValue;
|
||
|
|
RigidBodyDynamics::Math::VectorNd mWeights;
|
||
|
|
Millard2016TorqueMuscle &mTqMcl;
|
||
|
|
TorqueMuscleInfo mTmi;
|
||
|
|
TorqueMuscleSummary mTms;
|
||
|
|
};
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif
|
||
|
|
|