255 lines (190 with data), 7.9 kB

AnyFolder Push ={
  
    AnyKinPLine LinS={
      AnyRefFrame &BaseObject=..BaseObject;
      AnyRefFrame &StrengthObject=..StrengthObject;
    };
    
    AnyVar UserDefinedMeasure=LinS.Pos[0];
   /* 
    AnyVar UserDefinedStrengthFunction= 
    (UserDefinedMeasure-.UserDefinedLimitLow)/sqrt((UserDefinedMeasure-.UserDefinedLimitLow)^2)*
    (.UserDefinedLimitHigh-UserDefinedMeasure)/sqrt((.UserDefinedLimitHigh-UserDefinedMeasure)^2)*.Strength;
    */
    
  
    AnyIntVar LowerThanHighLimit = ltfun(UserDefinedMeasure,.UserDefinedLimitHigh);
  AnyIntVar GreaterThanLowLimit = gtfun(UserDefinedMeasure,.UserDefinedLimitLow);
  

  AnyVar UserDefinedStrengthFunction= iffun(andfun(LowerThanHighLimit,GreaterThanLowLimit),.Strength,0.0);

  
  //create the directions vectors 
  
  AnyVec3 NormalVector={ 
    iffun(.Direction[0]-0,0,1),
    iffun(.Direction[0]-1,0,1),
    iffun(.Direction[0]-2,0,1)};
  
  AnyVec3 FrictionVector1={ 
    iffun(.Direction[1]-0,0,1),
    iffun(.Direction[1]-1,0,1),
    iffun(.Direction[1]-2,0,1)};
  
  AnyVec3 FrictionVector2={ 
    iffun(.Direction[2]-0,0,1),
    iffun(.Direction[2]-1,0,1),
    iffun(.Direction[2]-2,0,1)};
     

  
  AnyKinLinear Lin={
    AnyRefFrame &Box =..BaseObject;
    AnyRefFrame &Hand = ..TargetObject;
    Ref=0;
  };
  
  
  
  
  AnyFolder Direction01Muscle={
    AnyFolder Push={
      AnyKinMeasureOrg NormalDir={
        AnyKinLinear &ref=...Lin;
        MeasureOrganizer={....Direction[0]}; //always pick first element
      };
      AnyKinMeasureOrg FrictionDir={
        AnyKinLinear &ref=...Lin;
        MeasureOrganizer={....Direction[1]}; // pick second element
      };
      AnyKinMeasureLinComb LinCombinationMeasure ={
        AnyKinMeasure& u1=.NormalDir;
        AnyKinMeasure& u2=.FrictionDir;    
        Coef={
          {1,....StaticFrictionCoefficient}
        };     
      };
      
      AnyRecruitedActuator FrictionMuscle={
         //       AnyKinMeasure& ref1=.FrictionDir;
        AnyKinMeasureLinComb &ref1=.LinCombinationMeasure;
        Strength = ...UserDefinedStrengthFunction;
        Type = NonNegative;
        SET_DEFAULT_ACTUATOR_VOLUME;
         
      };
      
    };//Push
    
    AnyFolder Pull={
      AnyKinMeasureOrg NormalDir={
        AnyKinLinear &ref=...Lin;
        MeasureOrganizer={....Direction[0]}; //always pick first element
      };
      AnyKinMeasureOrg FrictionDir={
        AnyKinLinear &ref=...Lin;
        MeasureOrganizer={....Direction[1]}; // pick second element
      };
      AnyKinMeasureLinComb LinCombinationMeasure ={
        AnyKinMeasure& u1=.NormalDir;
        AnyKinMeasure& u2=.FrictionDir;    
        Coef={
          {-1,....StaticFrictionCoefficient}
        };     
        
      };
      
      AnyRecruitedActuator FrictionMuscle={
        AnyKinMeasureLinComb &ref1=.LinCombinationMeasure;
        //        AnyKinMeasure& ref1=.FrictionDir;
        Strength = ...UserDefinedStrengthFunction;
        Type = NonPositive;
        SET_DEFAULT_ACTUATOR_VOLUME;
      };
      
    };//Pull
    
  };
  
  
  
  AnyFolder Direction02Muscle={
    AnyFolder Push={
      AnyKinMeasureOrg NormalDir={
        AnyKinLinear &ref=...Lin;
        MeasureOrganizer={....Direction[0]}; //always pick first element
      };
      AnyKinMeasureOrg FrictionDir={
        AnyKinLinear &ref=...Lin;
        MeasureOrganizer={....Direction[2]}; // pick last element
      };
      AnyKinMeasureLinComb LinCombinationMeasure ={
        AnyKinMeasure& u1=.NormalDir;
        AnyKinMeasure& u2=.FrictionDir;    
        Coef={
          {1,....StaticFrictionCoefficient}
        };     
        
      };
      
      AnyRecruitedActuator FrictionMuscle={
        AnyKinMeasureLinComb &ref1=.LinCombinationMeasure;
        //        AnyKinMeasure& ref1=.FrictionDir;
        Strength = ...UserDefinedStrengthFunction;
        Type = NonNegative;
        SET_DEFAULT_ACTUATOR_VOLUME;
      };
      
    };//Push
    
    AnyFolder Pull={
      AnyKinMeasureOrg NormalDir={
        AnyKinLinear &ref=...Lin;
        MeasureOrganizer={....Direction[0]}; //always pick first element
      };
      AnyKinMeasureOrg FrictionDir={
        AnyKinLinear &ref=...Lin;
        MeasureOrganizer={....Direction[2]}; // pick last element
      };
      AnyKinMeasureLinComb LinCombinationMeasure ={
        AnyKinMeasure& u1=.NormalDir;
        AnyKinMeasure& u2=.FrictionDir;    
        Coef={
          {-1,....StaticFrictionCoefficient}
        };     
        
      };
      
      AnyRecruitedActuator FrictionMuscle={
        AnyKinMeasureLinComb &ref1=.LinCombinationMeasure;
        //        AnyKinMeasure& ref1=.FrictionDir;
        Strength = ...UserDefinedStrengthFunction;
        Type = NonPositive;
        SET_DEFAULT_ACTUATOR_VOLUME;
         
      };
      
    };//Pull
    
  };
  
  AnyDrawVector DrawNormalForce =     {
    AnyRefFrame &ref=..TargetObject;
    AnyVar Strength=   iffun(.UserDefinedStrengthFunction,.UserDefinedStrengthFunction,0.00001);
    Vec =..ScaleFactor*..DrawRef.DrawSettingsSupport.Lin.ScaleFactor*(
    //.NormalVector*.Direction0Muscle.NormalDirectionMuscle.Fout[0]*..BaseObject.Axes'
    
    .NormalVector*.Direction01Muscle.Push.FrictionMuscle.Fout[0]*..BaseObject.Axes'
    +.NormalVector*.Direction02Muscle.Push.FrictionMuscle.Fout[0]*..BaseObject.Axes'
    +.NormalVector*.Direction01Muscle.Pull.FrictionMuscle.Fout[0]*..BaseObject.Axes'
    +.NormalVector*.Direction02Muscle.Pull.FrictionMuscle.Fout[0]*..BaseObject.Axes');
    AnyVar lenght=Strength*(2-(1-Strength/sqrt(Strength^2)));
    
 PointAway = Off;
  DrawCoord = Off;
  
  Line.RGB = ..DrawRef.DrawSettingsSupport.Lin.RGB;
  Line.Thickness = ..DrawRef.DrawSettingsSupport.Lin.Thickness;
  Line.End.Thickness = 2*..DrawRef.DrawSettingsSupport.Lin.Thickness;  
  Line.End.Length = 4*..DrawRef.DrawSettingsSupport.Lin.Thickness;

  };
  
  
  AnyDrawVector DrawFrictionForce =     {
    
    AnyRefFrame &ref=..TargetObject;
    
   Vec =..ScaleFactor*..DrawRef.DrawSettingsSupport.Lin.ScaleFactor*..StaticFrictionCoefficient*(
    .FrictionVector1*.Direction01Muscle.Push.FrictionMuscle.Fout[0]
    -.FrictionVector1*.Direction01Muscle.Pull.FrictionMuscle.Fout[0]
    +.FrictionVector2*.Direction02Muscle.Push.FrictionMuscle.Fout[0]
    -.FrictionVector2*.Direction02Muscle.Pull.FrictionMuscle.Fout[0])*..BaseObject.Axes';
    
    PointAway = Off;
  DrawCoord = Off;
  
  Line.RGB = ..DrawRef.DrawSettingsSupport.Lin.RGB;
  Line.Thickness = ..DrawRef.DrawSettingsSupport.Lin.Thickness;
  Line.End.Thickness = 2*..DrawRef.DrawSettingsSupport.Lin.Thickness;  
  Line.End.Length = 4*..DrawRef.DrawSettingsSupport.Lin.Thickness;
  };

  
  /*
    AnyDrawVector DrawFrictionForce1 =     {
    
    AnyRefFrame &ref=..TargetObject;
    
    Vec =0.001*..StaticFrictionCoefficient*(
    .FrictionVector1*.Direction01Muscle.Push.FrictionMuscle.Fout[0]*..BaseObject.Axes'+
    -.FrictionVector1*.Direction01Muscle.Pull.FrictionMuscle.Fout[0]*..BaseObject.Axes');
    
    PointAway = Off;
    DrawCoord = Off;
    Line.RGB = {0,1,0};
    Line.Thickness = 0.015;
    Line.End.Thickness = 2*0.015; 
    Line.End.Length = 4*0.015;
  };

  
  
  
  
    AnyDrawVector DrawFrictionForce2 =     {
    
    AnyRefFrame &ref=..TargetObject;
    
    Vec =0.001*..StaticFrictionCoefficient*(
    .FrictionVector2*.Direction02Muscle.Push.FrictionMuscle.Fout[0]*..BaseObject.Axes'+
    -.FrictionVector2*.Direction02Muscle.Pull.FrictionMuscle.Fout[0]*..BaseObject.Axes');
    
    PointAway = Off;
    DrawCoord = Off;
    Line.RGB = {1,0,0};
    Line.Thickness = 0.015;
    Line.End.Thickness = 2*0.015; 
    Line.End.Length = 4*0.015;
  };
*/
  
  
};