224 lines (162 with data), 8.6 kB

/* 
HOW TO USE:

Add `#include "../path/to/KinLimitsDriver_template.any"` in the beginning
of your main file. Then create the `KinLimitsDriver` class inside Main.

In the following example we add a limit driver to `MyKinematicMeasure`, which limits the 
measures between -1 and 1. 

```
#include "../path/to/KinLimits_template.any"

Main = {

  KinLimitsDriver limit_driver(
      KinMeasure = ..path.to.MyKinematicMeasure
   ) = {
     LowerLimit = -1;
     HighLimit = 1;
   }; 
      
```

The class supports the following settings:

|-----------------------|--------------|------------------------------------------------------------------------------|
| Variable              | Type         |  Default                    | Desciption                                     |
|-----------------------|--------------|------------------------------------------------------------------------------|
| LowerLimit            | AnyFloat     |  -                          | The low limit of the driver                    |
| HighLimit             | AnyFloat     |  -                          | The high limit of the driver                   |
| DriverPhaseIn         | AnyFloat     |  0.1*(HighLimit-LowerLimit) | How fast the limits driver ramps up.           |
| HighLimitHard         | AnyFloat     |  HighLimit + DriverPhaseIn  | Low limit where the driver is fully ramped up  |
| LowerLimitHard        | AnyFloat     |  LowerLimit - DriverPhaseIn | High limit where the driver is fully ramped up |
| ErrorAtHardLimit      | AnyFloat     |  1                          | The kinematic error at the hard limits         |
| ErrorAtLowerHardLimit | AnyFloat     |  ErrorAtHardLimit           | The kinematic error at the lower hard limit    |
| ErrorAtLowerHardLimit | AnyFloat     |  ErrorAtHardLimit           | The kinematic error at the high hard limit     |
| MeasureConversion     | AnyFloat     |  1                          | Constant factor applied to the measure         |
|-----------------------|--------------|------------------------------------------------------------------------------| 


*/


/*
The class uses the builtin smoothramp function, to create a AnyFunInterpol function 
which is used by the AnyKinFunComb1 class. 

The smoothramp function is essentially this c++ function. 
This is just half a smoother step function which continues liniearly when it
reaches it steepest value. See https://en.wikipedia.org/wiki/Smoothstep

float smoothramp(float edge0, float edge1, float x)
{
    // Scale, and clamp x to 0..1 range
    x = clamp((x - edge0)/(2*(edge1 - edge0)), 0.0, 0.5);
    // Evaluate polynomial
    if (x < 0.5) {
      return x*x*x*(x*(x*6 - 15) + 10)
    }
    else {
      return 2*x - 0.5
    };
};
*/
#ifndef KINLIMIT_TEMPLATE_ANY
#define KINLIMIT_TEMPLATE_ANY


// The smoothramp function is not available in AMS 6 and below. 
// This just defines an equallent implementation directly in AnyScript.
#define KIN_LIMIT_SMOOTHRAMP_ANYSCRIPT(edge0, edge1, val) iffun(gtfun(((val)-(edge0))/(2*((edge1)-(edge0))), 0.5), -0.5+2*(((val)-(edge0))/(2*((edge1)-(edge0)))),\
                                                mult(iffun(ltfun(((val)-(edge0))/(2*((edge1)-(edge0))),0.0), 0*(val), iffun(gtfun(((val)-(edge0))/(2*((edge1)-(edge0))),0.5),0*(val)+0.5, ((val)-(edge0))/(2*((edge1)-(edge0)))))\
                                               ,mult(iffun(ltfun(((val)-(edge0))/(2*((edge1)-(edge0))),0.0), 0*(val), iffun(gtfun(((val)-(edge0))/(2*((edge1)-(edge0))),0.5),0*(val)+0.5, ((val)-(edge0))/(2*((edge1)-(edge0)))))\
                                               ,mult(iffun(ltfun(((val)-(edge0))/(2*((edge1)-(edge0))),0.0), 0*(val), iffun(gtfun(((val)-(edge0))/(2*((edge1)-(edge0))),0.5),0*(val)+0.5, ((val)-(edge0))/(2*((edge1)-(edge0)))))\
                                               ,(mult(iffun(ltfun(((val)-(edge0))/(2*((edge1)-(edge0))),0.0), 0*(val), iffun(gtfun(((val)-(edge0))/(2*((edge1)-(edge0))),0.5),0*(val)+0.5, ((val)-(edge0))/(2*((edge1)-(edge0)))))\
                                               ,(iffun(ltfun(((val)-(edge0))/(2*((edge1)-(edge0))),0.0), 0*(val), iffun(gtfun(((val)-(edge0))/(2*((edge1)-(edge0))),0.5),0*(val)+0.5, ((val)-(edge0))/(2*((edge1)-(edge0)))))\
                                                *6 - 15)) + 10) )))\
                                               )
                                               
#if ANYBODY_V1 >= 7
#define KIN_LIMIT_SMOOTHRAMP smoothramp
#else
#define KIN_LIMIT_SMOOTHRAMP KIN_LIMIT_SMOOTHRAMP_ANYSCRIPT
#endif
                                               




#class_template KinLimitsDriver(AnyKinMeasure& KinMeasure, _DEBUG=0) {

  
#var AnyVar LowerLimit; 
#var AnyVar HighLimit;

AnyInt LowerHighCheck = assert(gteqfun(HighLimit, LowerLimit), "HighLimit must be greater than LowLimit: " + CompleteNameOf(ObjGetParent(1)));
  
#var AnyVar DriverPhaseIn = 0.01*(HighLimit - LowerLimit) ;
#var AnyVar LowerLimitHard= LowerLimit - DriverPhaseIn ;
#var AnyVar HighLimitHard= HighLimit + DriverPhaseIn ;

#var AnyVar ErrorAtHardLimit = 1;
#var AnyVar ErrorAtLowerHardLimit = ErrorAtHardLimit;
#var AnyVar ErrorAtHighHardLimit = ErrorAtHardLimit;

#var AnyVar MeasureConversion = 1;

/// Generates an non-equisample abscissa axis for the data. 
/// Most points are clustered around the transition zones.
AnyFunInterpol AbscissaFunction = 
{
  AnyFloat LowPhaseIn = abs(.LowerLimit-.LowerLimitHard);
  AnyFloat HighPhaseIn = abs(.HighLimit-.HighLimitHard);
  AnyFloat TotalRange = abs(.LowerLimitHard-.HighLimitHard) + 0*.LowerHighCheck; // force calculation of LowerHighCheck before error in interpolation function.

  #var Type = Bspline;
  #var BsplineOrder = 8;
       
  #var AnyMatrix M = {
                      {0, .LowerLimitHard-1e1*LowPhaseIn-1e1*TotalRange},
                      {80, .LowerLimitHard-1e1*LowPhaseIn},
                      {160,  .LowerLimitHard},
                      {490, .LowerLimit},
                      {510, .HighLimit},
                      {840, .HighLimitHard},
                      {920, .HighLimitHard+1e1*HighPhaseIn},
                      {999, .HighLimitHard+1e1*HighPhaseIn+1e1*TotalRange}                    
                    };  

                    
  T    = M'[0];
  Data = {M'[1]};
};



AnyFunInterpol JointLimitErrorFunction= 
{  
  Type = Bspline;
  T = .AbscissaFunction(1.0*iarr(0,999))' ;
  Data = {.ErrorAtLowerHardLimit*KIN_LIMIT_SMOOTHRAMP(.LowerLimit, .LowerLimitHard, T) + 
          .ErrorAtHighHardLimit*KIN_LIMIT_SMOOTHRAMP(.HighLimit, .HighLimitHard, T)} ;
};

#if _DEBUG
AnyFunInterpol &FT = JointLimitErrorFunction;
AnyFloat Test = {FT (-10.0), FT( -5.0), FT (0.0), FT (5.0), FT (10.0)}; 
AnyFloat test_fullrange = JointLimitErrorFunction(1.0*iarr(ceil(min(AbscissaFunction.M'[1])),floor(max(AbscissaFunction.M'[1]))))';
AnyFloat range = iarr(0, 1000-1)*(HighLimitHard-LowerLimitHard)/(1000-1)+LowerLimitHard
AnyFloat test_range = JointLimitErrorFunction(range)';
#endif


AnyKinMeasureFunComb1 JointLimitMeasure = {
  AnyKinMeasureLinComb ConvertedMeasure = {
    AnyKinMeasure& ref = ..KinMeasure;
    Coef  = {{..MeasureConversion }};
    OutDim = 1;
  };
  Functions = {&.JointLimitErrorFunction};
};

AnyKinEqSimpleDriver JointLimitDriver = 
{
  AnyKinMeasureFunComb1 & ref = .JointLimitMeasure;
  #var DriverPos = {0};
  #var DriverVel = {0};
  Reaction.Type = {Off};
  CType = {Soft};
  AnyFunConst ConstFun = 
  { 
    #var Value = {1};
  };
  #var WeightFun = {&ConstFun};      
};


};



#if ANYBODY_NAME_MAINFILE  == "KinLimit_template"


Main = 
{

AnyStringVar test = ANYBODY_NAME_MAINFILE;

AnyKinStudy Test_study =
{
  Gravity = {0,0,0.0};
  tStart = 0;
  tEnd = 100;
  AnyFloat Smoothramp_buitin = smoothramp(40, 60,t);
  AnyFloat Smoothramp_anyscript = KIN_LIMIT_SMOOTHRAMP_ANYSCRIPT(40, 60,t);
  AnyFloat diff = Smoothramp_buitin- Smoothramp_anyscript;
  
  
  AnyFolder test = 
  {
    #define DIFFMAT(N) arrcat(zeros(1,N-1), eye(N-1)')' - arrcat(eye(N-1)', zeros(1,N-1))'
    AnyMatrix diffmat = DIFFMAT(101);
    
    AnyFloat Vec = .tArray;
    
    AnyFloat smoothramp_builtin = smoothramp(40, 60, Vec);
    AnyFloat smoothramp_anyscript = KIN_LIMIT_SMOOTHRAMP_ANYSCRIPT(40, 60,Vec);

    AnyFloat smoothramp_builtin_diff = diffmat*smoothramp_builtin';
    AnyFloat smoothramp_anyscript_diff = diffmat*smoothramp_anyscript';
  
  
  
  
  };
};

};

#endif
#endif