--- a +++ b/Tools/ModelUtilities/FrictionContactMuscles/ContactLinPush.any @@ -0,0 +1,255 @@ + +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; + }; +*/ + + +}; \ No newline at end of file