TorqueMuscleFittingToolkit.h

Go to the documentation of this file.

#ifndef TORQUEMUSCLEFITTINGTOOLKIT_H_
#define TORQUEMUSCLEFITTINGTOOLKIT_H_
 
 
#include <vector>
#include <rbdl/rbdl_math.h>
#include "IpTNLP.hpp"
#include "Millard2016TorqueMuscle.h"
 
namespace RigidBodyDynamics {
  namespace Addons {
    namespace Muscle{
 
 
class TorqueMuscleFittingToolkit {
  public:
 
 
  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 &parametersOfBestFit,
    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