Datasets
Models
Diff of
/Tools/ModelUtilities/FrictionContactMuscles/ContactSurfaceLinPush.any
[000000]
..
[38ba34]
Switch to side-by-side view
--- a +++ b/Tools/ModelUtilities/FrictionContactMuscles/ContactSurfaceLinPush.any @@ -0,0 +1,363 @@ +AnyFolder Push ={ + AnyComponentDefinition obj = {}; + 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= 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; + + }; + + AnyForceMomentMeasure NetMuscleEffect = + { + AnyForceBase& mus = .NormalDirectionMuscle; + AnyRefFrame& TargetObject = ...TargetObject; + }; + AnyVec3 NetMuscleEffect_Force_Local = NetMuscleEffect.F * ..BaseObject.Axes; + + }; + + 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; + OutDim = 1; + Coef={ + {1,....StaticFrictionCoefficient} + }; + }; + + AnyRecruitedActuator FrictionMuscle={ + // AnyKinMeasure& ref1=.FrictionDir; + AnyKinMeasureLinComb &ref1=.LinCombinationMeasure; + Strength = ...UserDefinedStrengthFunction; + Type = NonNegative; + SET_DEFAULT_ACTUATOR_VOLUME; + + }; + + AnyForceMomentMeasure NetMuscleEffect = + { + AnyForceBase& mus = .FrictionMuscle; + AnyRefFrame& TargetObject = ....TargetObject; + }; + AnyVec3 NetMuscleEffect_Force_Local = NetMuscleEffect.F * ...BaseObject.Axes; + + };//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; + OutDim = 1; + Coef={ + {-1,....StaticFrictionCoefficient} + }; + }; + + AnyRecruitedActuator FrictionMuscle={ + AnyKinMeasureLinComb &ref1=.LinCombinationMeasure; + // AnyKinMeasure& ref1=.FrictionDir; + Strength = ...UserDefinedStrengthFunction; + Type = NonPositive; + SET_DEFAULT_ACTUATOR_VOLUME; + + }; + + AnyForceMomentMeasure NetMuscleEffect = + { + AnyForceBase& mus = .FrictionMuscle; + AnyRefFrame& TargetObject = ....TargetObject; + }; + AnyVec3 NetMuscleEffect_Force_Local = NetMuscleEffect.F * ...BaseObject.Axes; + };//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; + OutDim = 1; + Coef={ + {1,....StaticFrictionCoefficient} + }; + }; + + AnyRecruitedActuator FrictionMuscle={ + AnyKinMeasureLinComb &ref1=.LinCombinationMeasure; + //AnyKinMeasure& ref1=.FrictionDir; + + Strength = ...UserDefinedStrengthFunction; + Type = NonNegative; + SET_DEFAULT_ACTUATOR_VOLUME; + + }; + + AnyForceMomentMeasure NetMuscleEffect = + { + AnyForceBase& mus = .FrictionMuscle; + AnyRefFrame& TargetObject = ....TargetObject; + }; + AnyVec3 NetMuscleEffect_Force_Local = NetMuscleEffect.F * ...BaseObject.Axes; + + };//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; + OutDim = 1; + Coef={ + {-1,....StaticFrictionCoefficient} + }; + }; + + AnyRecruitedActuator FrictionMuscle={ + AnyKinMeasureLinComb &ref1=.LinCombinationMeasure; + // AnyKinMeasure& ref1=.FrictionDir; + Strength = ...UserDefinedStrengthFunction; + Type = NonPositive; + SET_DEFAULT_ACTUATOR_VOLUME; + + }; + + AnyForceMomentMeasure NetMuscleEffect = + { + AnyForceBase& mus = .FrictionMuscle; + AnyRefFrame& TargetObject = ....TargetObject; + }; + AnyVec3 NetMuscleEffect_Force_Local = NetMuscleEffect.F * ...BaseObject.Axes; + + };//Pull + + }; + + AnyDrawVector DrawNormalForce = + { + AnyRefFrame &ref=..TargetObject; + Vec =..ScaleFactor*..DrawRef.DrawSettingsSupport.Lin.ScaleFactor*.NormalForceVec_Global; + + PointAway = Off; + DrawCoord = Off; + GlobalCoord = On; + + 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*.FrictionForceVec_Global; + + PointAway = Off; + DrawCoord = Off; + GlobalCoord = On; + + 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; + }; + + //Resultant force calculation w.r.t either local or global reference systems + AnyVec3 TotalForceVec_Global = + Direction0Muscle.NetMuscleEffect.F + + Direction01Muscle.Push.NetMuscleEffect.F + + Direction01Muscle.Pull.NetMuscleEffect.F + + Direction02Muscle.Push.NetMuscleEffect.F + + Direction02Muscle.Pull.NetMuscleEffect.F ; + + AnyVec3 TotalForceVec_Local = TotalForceVec_Global * .BaseObject.Axes; + + AnyVec3 NormalForceVec_Local = (TotalForceVec_Local* NormalVector') * NormalVector ; + AnyVec3 FrictionForceVec1_Local = (TotalForceVec_Local* FrictionVector1') * FrictionVector1; + AnyVec3 FrictionForceVec2_Local = (TotalForceVec_Local* FrictionVector2') * FrictionVector2; + AnyVec3 FrictionForceVec_Local = FrictionForceVec1_Local + FrictionForceVec2_Local; + AnyVec3 NormalForceVec_Global = NormalForceVec_Local * .BaseObject.Axes'; + AnyVec3 FrictionForceVec_Global = FrictionForceVec_Local * .BaseObject.Axes'; + + AnyVec3 NormalForceVec = NormalForceVec_Global; + AnyVar NormalForceScalar = vnorm(NormalForceVec); + AnyVec3 NormalForceVec2 = NormalForceVec; + AnyVar NormalForceScalar2 = NormalForceScalar ; + AnyVec3 FrictionForceVec = FrictionForceVec_Global ; + AnyVar FrictionForceScalar = vnorm(FrictionForceVec) ; + + //--> Old code for calculating the normal and the friction forces. Let's keep these for a while. + // AnyVec3 NormalForceVec = DrawNormalForce.Vec/(.ScaleFactor*.DrawRef.DrawSettingsSupport.Lin.ScaleFactor); + // AnyVar NormalForceScalar = (NormalForceVec[0]^2+NormalForceVec[1]^2+NormalForceVec[2]^2)^0.5; + // AnyVec3 NormalForceVec2 = NormalForceValue.NormalForceVector/(.ScaleFactor*.DrawRef.DrawSettingsSupport.Lin.ScaleFactor); + // AnyVar NormalForceScalar2 = (NormalForceVec2[0]^2+NormalForceVec2[1]^2+NormalForceVec2[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; + // AnyDrawVector DrawNormalForce = { + // AnyRefFrame &ref=..TargetObject; + // AnyVar Strength= iffun(.ContactMuscleModel.S,.ContactMuscleModel.S,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; + // + // }; + // + // AnyFolder NormalForceValue = { + // AnyRefFrame &ref=..TargetObject; + // AnyVar Strength= iffun(.ContactMuscleModel.S,.ContactMuscleModel.S,0.00001); + // AnyVec3 NormalForceVector =..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))); + // + // + // }; + // + // 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; + // }; + //<-- Old code +};