315 lines (229 with data), 9.6 kB

AnyFolder Push ={
  
  AnyKinLinear LinS={
    Ref=0;
    AnyRefFrame &BaseObject=..BaseObject;
    AnyRefFrame &StrengthObject=..StrengthObject;
  };
  
  
  AnyKinMeasureOrg LinMeasureOrg0 ={
    AnyKinLinear &ref=.LinS;
    MeasureOrganizer={..Direction[0]};
  };

    AnyKinMeasureOrg LinMeasureOrg1 ={
    AnyKinLinear &ref=.LinS;
    MeasureOrganizer={..Direction[1]};
  };

    AnyKinMeasureOrg LinMeasureOrg2 ={
    AnyKinLinear &ref=.LinS;
    MeasureOrganizer={..Direction[2]};
  };

  
  AnyVar UserDefinedMeasure=LinMeasureOrg0.Pos[0];
  AnyVar RadiusMeasure=sqrt(LinMeasureOrg1.Pos[0]^2+LinMeasureOrg2.Pos[0]^2);
  
  
  AnyIntVar LowerThanHighLimit = ltfun(UserDefinedMeasure,.UserDefinedLimitHigh);
  AnyIntVar GreaterThanLowLimit = gtfun(UserDefinedMeasure,.UserDefinedLimitLow);
  AnyIntVar LowerThanRadiusLimit = ltfun(RadiusMeasure,.UserDefinedRadiusLimit);

  AnyVar UserDefinedStrengthFunction= .Trigger*iffun( andfun(andfun(LowerThanHighLimit,GreaterThanLowLimit),LowerThanRadiusLimit),.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)};
  
  
  
  
  AnyMuscleModelUsr1 ContactMuscleModel=   {
    F0 = 0.000000;
    S = .UserDefinedStrengthFunction;
  };        
  
  AnyMuscleModelUsr1 ContactMuscleModel1=   {
    F0 = 0.000000;
    S = .UserDefinedStrengthFunction;
  };        
  
  AnyMuscleModelUsr1 ContactMuscleModel2=   {
    F0 = 0.000000;
    S = .UserDefinedStrengthFunction;
  };        
  
  
  AnyKinLinear Lin={
    AnyRefFrame &Box =..BaseObject;
    AnyRefFrame &Hand = ..TargetObject;
    Ref=0;
  };
  
  
  
  AnyFolder Direction0Muscle ={
    AnyRecruitedActuator NormalDirectionMuscle={
      AnyKinMeasureOrg NormalDir={
        AnyKinLinear &ref=...Lin;
        MeasureOrganizer={....Direction[0]}; //always pick first element
      };
      Strength = ..UserDefinedStrengthFunction;
      Type = NonNegative;
      SET_DEFAULT_ACTUATOR_VOLUME;
       
    };
  };
  
  
  
  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(.ContactMuscleModel.S,.ContactMuscleModel.S,0.00001);
    Vec =0*..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 =0*..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;
  };
  
  
  //resultann force given in global ref system
  AnyVec3 NormalForceVec = DrawNormalForce.Vec/(.ScaleFactor*.DrawRef.DrawSettingsSupport.Lin.ScaleFactor);
    AnyVar NormalForceScalar = (NormalForceVec[0]^2+NormalForceVec[1]^2+NormalForceVec[2]^2)^0.5;

  AnyVec3 FrictionForceVec = DrawFrictionForce.Vec/(.ScaleFactor*.DrawRef.DrawSettingsSupport.Lin.ScaleFactor);
    AnyVar FrictionForceScalar = (FrictionForceVec[0]^2+FrictionForceVec[1]^2+FrictionForceVec[2]^2)^0.5;
   
    //AnyVar TotalForceScalar = (TotalForceVec[0]^2+TotalForceVec[1]^2+TotalForceVec[2]^2)^0.5;



  /*
  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;
  };
  */
  
  
};