--- a +++ b/Tools/ModelUtilities/FrictionContactMuscles/ContactSurfaceLinPushTrigger.any @@ -0,0 +1,314 @@ + +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; + }; + */ + + +};