298 lines (229 with data), 10.5 kB

#ifndef MODELUTILS_RHYTHMS_SIMPLERHYTHMS_ANY
#define MODELUTILS_RHYTHMS_SIMPLERHYTHMS_ANY
/*
---
group: Rhythms
topic: Linear
descr: |
  Collection of class templates for linear and bilinear rhythm drivers. 
  These are for example used to create the spine rhythms in the AMMR.
---

To use these drivers import the file:
#include "<ANYBODY_PATH_MODELUTILS>/Rhythms/SimpleRhythms.any"

See the individual classes for more information.

*/


// Creates a linear rhythm between the degrees of freedom (DOF) of a kinematic measure. The rhythm is defined by 
// a `RhythmCoefficients` vector which defines the relative movement of the DOFs.
//
// In the example below the rhythm constrains two DOFs of the arm to move in a 1:4 ratio.
//
// :::{rubric} Example
// :::
//
// :::{code-block} AnyScriptDoc
// RhythmDriverLinear Rhythm = {
//    RhythmCoefficients = {0.2, 0.8};
//    Measures.Input = {
//        AnyJoint& ref1 = Main.HumanModel.BodyModel.Interface.Arm.Right.ShoulderElevation;
//        AnyJoint& ref2 = Main.HumanModel.BodyModel.Interface.Arm.Right.GHElevation;
//    };
//:::
//
#class_template RhythmDriverLinear(__CLASS__=AnyKinMotion, RELATIVE_TO_DOF=1,  _REDEFINE_VARIABLES=Off){
 
  // Arguments:
  // ----------------------
  //RhythmDriverLinear#RELATIVE_TO_DOF
  // Specifies which kinematic input measure is used in the implementation of the rhythm. 
  // By default (RELATIVE_TO_DOF=1) the first measure is used as the reference for the rest. 
  // I.e. all other measures are linked to some ratio of the first measure in the underlying 
  // matrix defining the rhythm. 
  // It is also possible to set this value to -1, in which case the rhythm is implemented
  // implicitly, i.e. all kinematic measures are linked together as in a chain. Note, that 
  // implicit implementation prevents the any of the RhythmCoefficients to be zero.


   // _REDREFINE_VARIABLES is a 'internal' flag used in the AMMR to allow
   // the `RhythmCoefficients` to be declared when the class is used.
   // Hence allowing the final user to still overwrite `RhythmCoefficients`
   // in the application examples. 
   #if _REDEFINE_VARIABLES != On

   /// RhythmDriverLinear
   /// The Coefficient which defines the relative weights of the 
   /// kinematic measures in the Rhythm. The default value is a vector of ones.
   #var AnyVector RhythmCoefficients = ones(1,Measures.Input.nDim)[0];

   #endif
   
   
   
   AnyInt DependentDOFs = set_difference(iarr(0, InputDim-1), {RELATIVE_TO_DOF});
   
   AnyIntVar InputDim = iffun(NumElemOf(Measures.Input.MeasureOrganizer), NumElemOf(Measures.Input.MeasureOrganizer), Measures.Input.nDim);
   
   AnyFolder Measures = 
   {
      AnyKinMeasureOrg Input = 
      {
        //RhythmDriverLinear.Measures.Input
        // A number of AnyKinMeasures that are used to define the rhythm
        // AnyKinMeaure MyMeasure = <...>; (1..Inf)

        #var MeasureOrganizer = #default;
      };
   };
      
   AnyKinMeasureLinComb RhythmMeasure = {
      AnyKinMeasureOrg Dependent = {
        AnyKinMeasure& ref = ..Measures.Input;
          #var MeasureOrganizer = ..DependentDOFs;
      };
      #if RELATIVE_TO_DOF != -1
      AnyKinMeasureOrg InDependent = {
        AnyKinMeasure& ref = ..Measures.Input;
        #var MeasureOrganizer = {RELATIVE_TO_DOF}; 
      };
      #endif
      
      OutDim = .InputDim-1;
      #var Const = zeros(1, OutDim)[0];
      
      #if RELATIVE_TO_DOF != -1
      AnyVector LinWeights = take(.RhythmCoefficients, .DependentDOFs)/take(.RhythmCoefficients, RELATIVE_TO_DOF);
      #var Coef = reshape(arrcat(-eye(OutDim), {LinWeights})', {OutDim, OutDim+1});
      #else
      AnyVector LinWeights = REPLACE_ZEROS(.RhythmCoefficients, 10^-3)^-1;
      #var Coef = reshape(
          arrcat(reshape(diag(take(LinWeights, iarr(0, OutDim-1)))',{OutDim,OutDim}), zeros(1,OutDim))'
          - arrcat(zeros(1,OutDim), diag( take(LinWeights, iarr(1,OutDim))))'
          , {OutDim,OutDim+1});
      #endif
      
   };
   
   AnyFolder Weights = {
       AnyFunConst Fun ={
           Value ??= repmat(..nDim, 0.01); 
       };  
   };
   
   CType ??= repmat(nDim, Hard);
   WeightFun ??= {&Weights.Fun};
   
    Reaction ={
      Type ??= repmat(nDim, Off);
    };     
 
 };
 

// Include wrapper template to call _RhythmDriverFunInterpol multiple times
#include "<AMMR_TOOLS>/Lib/classtools/MultipleSubTemplates.any"

// This helper class template used by the `RhythmDriverBiLinear` class 
// to implement the interpolation function used in the rhythm.
#class_template _RhythmDriverFunInterpol(IDX=0, ARG1=0.2, ARG2=0, ARG3=0, ARG4=0,
  __CLASS__= AnyFunInterpol) {
        
    AnyVector x_arr = linspace(ARG1/2, max({20*ARG1/2, 2*pi}), 100);
    Type=Bspline;
    T =     arrcat(-1*flip(x_arr), {0.0}, x_arr); 
    Data = { arrcat(
          .CoefficientsNeg[IDX]*-1*flip(x_arr),
          {0.0}, 
          .CoefficientsPos[IDX]* x_arr
        )};

};

// Creates a bi-linear rhythm between a number of input kinematic measures where the
// RhythmCoefficients can be different between the positive and negative values of
// the input measures. The rhythm is similar to simple `RhythmDriverLinear` template 
// but is defined by two sets of RhythmCoefficients. 
// 
// * `RhythmCoefficients`: for positive values of the input measures
// * `RhythmCoefficientsNegative`: for negative values of the input measures
// * `TransitionInterval`: The interval over which the transition between the two
//   sets of RhythmCoefficients is performed.
// 
// The RhythmCoefficients are defined as a vector of coefficients for each
// kinematic measure. The relative ratio between the coefficients defines the
// rhythm. If the coefficients sum to 1, then the coefficients can be interpreted
// as the relative fraction of the kinematic measures in the rhythm.
#class_template RhythmDriverBiLinear(__CLASS__=AnyKinEq, NDIM, RELATIVE_TO_DOF=1, _REDEFINE_VARIABLES=Off){

//RhythmDriverBiLinear#RELATIVE_TO_DOF
// Specifies which kinematic input measure is used in the implementation of the rhythm. 
// By default (RELATIVE_TO_DOF=1) the first measure is used as the reference for the rest. 
// I.e. all other measures are linked to some ratio of the first measure in the underlying 
// matrix defining the rhythm. 
// It is also possible to set this value to -1, in which case the rhythm is implemented
// implicitly, i.e. all kinematic measures are linked together as in a chain. Note, that 
// implicit implementation prevents the any of the RhythmCoefficients to be zero.
//
//RhythmDriverBiLinear#NDIM
// The number of kinematic measures used in the implementation of the rhythm. Due to 
// the implementation it of this class template it is necessary to specify this value
// explicitly.



#if _REDEFINE_VARIABLES != On
  
   // RhythmDriverBiLinear
   /// The coefficient which defines the relative weights of the 
   /// kinematic measures in the Rhythm for positive inputs.
   #var AnyVector RhythmCoefficients;

   // RhythmDriverBiLinear
   /// The coefficient which defines the relative weights of the 
   /// kinematic measures in the Rhythm for negative inputs.
   #var AnyVector RhythmCoefficientsNegative;
   
   // RhythmDriverBiLinear
   /// The interval over which the transition between the two
   /// sets of RhythmCoefficients is performed.
   #var AnyVar TransitionInterval=0.1;
#endif   
   
   
   
   AnyInt DependentDOFs = set_difference(iarr(0, InputDim-1), {RELATIVE_TO_DOF});
   
   AnyIntVar InputDim = iffun(NumElemOf(Measures.Input.MeasureOrganizer), NumElemOf(Measures.Input.MeasureOrganizer), Measures.Input.nDim);

   
   AnyFolder Weights = {
       AnyFunConst Fun ={
           Value ??= repmat(..nDim, 0.01); 
       };  
   };
   
   CType ??= repmat(nDim, Hard);
   WeightFun ??= {&Weights.Fun};
   
   Reaction ={
     Type ??= repmat(nDim, Off);
   };
   
   
   
   AnyKinMeasureLinComb CombinedMeasure = {
      AnyKinMeasure& Dependent = .Measures.GearedMeasure;
      #if RELATIVE_TO_DOF != -1
      AnyKinMeasureOrg InDependent = {
        AnyKinMeasure& ref = ..Measures.Input;
        MeasureOrganizer = {RELATIVE_TO_DOF}; 
      };
      #endif
      
      OutDim = NDIM-1;
      Const = zeros(1, OutDim)[0];
      
      #if RELATIVE_TO_DOF != -1 
      
      Coef = reshape(arrcat(-eye(OutDim), ones(1,OutDim))', {OutDim,OutDim+1});
      #else
      Coef = reshape(
          arrcat(eye(OutDim), zeros(1,OutDim))'
          - arrcat(zeros(1,OutDim), eye(OutDim))'
          , {OutDim,OutDim+1});
      #endif
      
   };
   
   AnyFolder Measures = {
      AnyKinMeasureOrg Input = 
      {
        //RhythmDriverBiLinear.Measures.Input
        // A number of AnyKinMeasures that are used to define the rhythm
        // AnyKinMeasure MyMeasure = <...>; (1..Inf)


         #var MeasureOrganizer = #default;
      };
            
      AnyKinMeasureFunComb1 GearedMeasure =
      {
        AnyKinMeasureOrg Dependent = 
        {
          AnyKinMeasure& ref = ..Input;
          MeasureOrganizer = ...DependentDOFs;
        };
        
        Functions = ObjSearch("ParamFuns.Fun*", "AnyFunInterpol");
        #if RELATIVE_TO_DOF != -1
        MultipleSubTemplates ParamFuns(
           NUM=NDIM-1,
           CLASS_TEMPLATE = "_RhythmDriverFunInterpol",
           NAME_PREFIX=Fun,
           ARG1 = ....TransitionInterval
         ) = {
           #var AnyVector CoefficientsPos = take(...RhythmCoefficients, RELATIVE_TO_DOF) /take(REPLACE_ZEROS(...RhythmCoefficients, 10^-3), ...DependentDOFs );
           #var AnyVector CoefficientsNeg = take(...RhythmCoefficientsNegative, RELATIVE_TO_DOF)/take(REPLACE_ZEROS(...RhythmCoefficientsNegative, 10^-3), ...DependentDOFs );
         };
         #endif
         
         #if RELATIVE_TO_DOF == -1
         MultipleSubTemplates ParamFuns(
            NUM=NDIM,
            CLASS_TEMPLATE="_RhythmDriverFunInterpol",
            NAME_PREFIX=Fun,
            ARG1 = ....TransitionInterval
          ) = {
           #var AnyVector CoefficientsPos = 1/REPLACE_ZEROS(...RhythmCoefficients, 10^-3);
           #var AnyVector CoefficientsNeg = 1/REPLACE_ZEROS(...RhythmCoefficientsNegative, 10^-3); 
        };
        #endif

      };
      
      
   };
   
};


#endif