625 lines (517 with data), 24.4 kB
AnyFolder Push ={
AnyKinLinear LinS={
Ref=0;
AnyRefFrame &BaseObject=..BaseObject;
AnyRefFrame &StrengthObject=..StrengthObject;
};
LinS.StrengthObject ={
AnyDrawSphere DrwSphere ={
ScaleXYZ =0.01*{1,1,1};
Opacity =...ContactStrengthSmoothed/....Strength;
};
};
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 UserDefinedDistMeasure=LinMeasureOrg0.Pos[0];
AnyVar RadiusMeasure=sqrt(LinMeasureOrg1.Pos[0]^2+LinMeasureOrg2.Pos[0]^2);
AnyVar UserDefinedVelMeasure = vnorm(LinS.Vel-.UserDefinedGroundVel,2);
AnyFloatVar LowerThanHighDistLimit = ltfun(UserDefinedDistMeasure,.UserDefinedDistLimitHigh);
AnyFloatVar GreaterThanLowDistLimit = gtfun(UserDefinedDistMeasure,.UserDefinedDistLimitLow);
AnyFloatVar LowerThanRadiusLimit = ltfun(RadiusMeasure,.UserDefinedRadiusLimit);
AnyFloatVar LowerThanHighVelLimit = ltfun(UserDefinedVelMeasure,.UserDefinedVelLimitHigh);
AnyVar UserDefinedStrengthFunction= .Strength;
AnyVar VelSize = UserDefinedVelMeasure;//vnorm(LinS.Vel, 2);
AnyVar VelRatio = VelSize /.UserDefinedVelLimitHigh ;
AnyFloatVar ContactDisCheck = andfun(andfun(LowerThanHighDistLimit,GreaterThanLowDistLimit),LowerThanRadiusLimit);
AnyFloatVar HeightRatio = iffun(gteqfun(UserDefinedDistMeasure, 0.0),
UserDefinedDistMeasure/.UserDefinedDistLimitHigh,
iffun(lteqfun(abs(UserDefinedDistMeasure/.UserDefinedDistLimitLow), 1.0), 0.0, 1.0));
AnyFloatVar RadiusRatio = RadiusMeasure/.UserDefinedRadiusLimit;
AnyFloatVar FirstControlVel = .UserDefinedVelLimitHigh * FirstControlVelRatio ;
AnyFloatVar SecondControlVel = .UserDefinedVelLimitHigh * SecondControlVelRatio ;
AnyIntVar FirstControlVelCheck = lteqfun(VelRatio, FirstControlVelRatio );
AnyIntVar SecondControlVelCheck = lteqfun(VelRatio, SecondControlVelRatio );
AnyIntVar FirstControlHeightCheck = lteqfun(HeightRatio, FirstControlHeightRatio);
AnyIntVar SecondControlHeightCheck = lteqfun(HeightRatio, SecondControlHeightRatio);
AnyIntVar FirstControlRadiusCheck = lteqfun(RadiusRatio, FirstControlRadiusRatio);
AnyIntVar SecondControlRadiusCheck = lteqfun(RadiusRatio, SecondControlRadiusRatio);
AnyFunPolynomial VelFactorFun_Lin =
{
AnyVar v1 = .FirstControlVelRatio;
AnyVar v2 = .SecondControlVelRatio ;
PolyCoef = (1.0/(v2-v1))*{-v1, 1.0};
};
AnyFunPolynomial HeightFactorFun_Lin =
{
AnyVar h1 = .FirstControlHeightRatio;
AnyVar h2 = .SecondControlHeightRatio ;
PolyCoef = (1.0/(h2-h1))*{-h1, 1.0};
};
AnyFunPolynomial RadiusFactorFun_Lin =
{
AnyVar r1 = .FirstControlRadiusRatio;
AnyVar r2 = .SecondControlRadiusRatio ;
PolyCoef = (1.0/(r2-r1))*{-r1, 1.0};
};
AnyFunSquareWave VelFactorFun_SW =
{
AnyVar v1 = .FirstControlVelRatio;
AnyVar v2 = .SecondControlVelRatio ;
InitialValues = {1.0};
Ts = {v1};
Values = {{0.0}};
dT = {abs(v1-v2)};
};
AnyFunSquareWave HeightFactorFun_SW =
{
AnyVar h1 = .FirstControlHeightRatio;
AnyVar h2 = .SecondControlHeightRatio ;
InitialValues = {1.0};
Ts = {h1};
Values = {{0.0}};
dT = {abs(h1-h2)};
};
AnyFunSquareWave RadiusFactorFun_SW =
{
AnyVar r1 = .FirstControlRadiusRatio;
AnyVar r2 = .SecondControlRadiusRatio ;
InitialValues = {1.0};
Ts = {r1};
Values = {{0.0}};
dT = {abs(r1-r2)};
};
AnyFunInterpol VelFactorFun_BSpline4 =
{
AnyVar v1 = .FirstControlVelRatio;
AnyVar v2 = .SecondControlVelRatio;
AnyVar delta = abs(v1-v2);
AnyVar SlopeControl = 25/100;
Type = Bspline;
BsplineOrder = 4;
T = { v2-delta*100/100, v2-delta*50/100, v2-delta*SlopeControl,
v2,
v2+delta*SlopeControl,
(v2+v1)*50/100,
v1-delta*SlopeControl,
v1,
v1+delta*SlopeControl, v1+delta*50/100, v1+delta*100/100};
Data = {{1.0, 1.0, 1.0,
1.0, //v2
1.0,
0.5, // average of v2 and v1
0.0,
0.0, //v1
0.0, 0.0, 0.0}};
};
AnyFunInterpol HeightFactorFun_BSpline4 =
{
AnyVar h1 = .FirstControlHeightRatio;
AnyVar h2 = .SecondControlHeightRatio ;
AnyVar delta = abs(h1-h2);
AnyVar SlopeControl = 25/100;
Type = Bspline;
BsplineOrder = 4;
T = { h2-delta*100/100, h2-delta*50/100, h2-delta*SlopeControl,
h2,
h2+delta*SlopeControl,
(h2+h1)*50/100,
h1-delta*SlopeControl,
h1,
h1+delta*SlopeControl, h1+delta*50/100, h1+delta*100/100};
Data = {{1.0, 1.0, 1.0,
1.0, //h2
1.0,
0.5, // average of h2 and h1
0.0,
0.0, //h1
0.0, 0.0, 0.0}};
};
AnyFunInterpol RadiusFactorFun_BSpline4 =
{
AnyVar r1 = .FirstControlRadiusRatio;
AnyVar r2 = .SecondControlRadiusRatio ;
AnyVar delta = abs(r1-r2);
AnyVar SlopeControl = 25/100;
Type = Bspline;
BsplineOrder = 4;
T = { r2-delta*100/100*10, r2-delta*50/100, r2-delta*SlopeControl,
r2,
r2+delta*SlopeControl,
(r2+r1)*50/100,
r1-delta*SlopeControl,
r1,
r1+delta*SlopeControl, r1+delta*50/100, r1+delta*100/100*10};
Data = {{1.0, 1.0, 1.0,
1.0, //r2
1.0,
0.5, // average of r2 and r1
0.0,
0.0, //r1
0.0, 0.0, 0.0}};
};
#ifdef _SMOOTHING_FUNCTION_PROFILE_
#undef _SMOOTHING_FUNCTION_PROFILE_
#endif
//#define _SMOOTHING_FUNCTION_PROFILE_ "FUNC_PROFILE_SW"
//#define _SMOOTHING_FUNCTION_PROFILE_ "FUNC_PROFILE_BSPLINE"
//#define _SMOOTHING_FUNCTION_PROFILE_ "FUNC_PROFILE_COSINE"
#define _SMOOTHING_FUNCTION_PROFILE_ "FUNC_PROFILE_LINEAR"
//#define _SMOOTHING_FUNCTION_PROFILE_ "FUNC_PROFILE_TEST"
#if _SMOOTHING_FUNCTION_PROFILE_ == "FUNC_PROFILE_SW"
AnyVar FirstControlHeightRatio = 1.0;
AnyVar SecondControlHeightRatio = 0.1;
AnyVar FirstControlRadiusRatio = 1.0;
AnyVar SecondControlRadiusRatio = 0.9;
AnyVar FirstControlVelRatio = 1.0;
AnyVar SecondControlVelRatio = 0.1;
AnyVar HeightFactorFun = iffun(FirstControlHeightCheck, HeightFactorFun_SW(HeightRatio)[0], 0.0);
AnyVar RadiusFactorFun = iffun(FirstControlRadiusCheck, RadiusFactorFun_SW(RadiusRatio)[0], 0.0);
AnyVar VelFactorFun = iffun(FirstControlVelCheck, VelFactorFun_SW(VelRatio)[0], 0.0);
#endif
#if _SMOOTHING_FUNCTION_PROFILE_ == "FUNC_PROFILE_BSPLINE"
AnyVar FirstControlHeightRatio = 1.0;
AnyVar SecondControlHeightRatio = 0.1;
AnyVar FirstControlRadiusRatio = 1.0;
AnyVar SecondControlRadiusRatio = 0.9;
AnyVar FirstControlVelRatio = 1.0;
AnyVar SecondControlVelRatio = 0.1;
//Ensure the heightratio is not outside the interpol fun
AnyVar HeightRatioMaxInSplineFun =max(HeightFactorFun_BSpline4.T)-0.0001; //find max "T" in spline function subtract tol
AnyVar HeightRatioMinInSplineFun =min(HeightFactorFun_BSpline4.T)+0.0001;//find min "T" in spline function add tol
AnyVar HeightRatioCapped_max = iffun(gtfun(HeightRatio,HeightRatioMaxInSplineFun),HeightRatioMaxInSplineFun,HeightRatio) ; //cap ratio
AnyVar HeightRatioCapped = iffun(ltfun(HeightRatioCapped_max,HeightRatioMinInSplineFun),HeightRatioMinInSplineFun,HeightRatioCapped_max) ;
//Ensure the radiusratio is not outside the interpol fun
AnyVar RadiusRatioMaxInSplineFun =max(RadiusFactorFun_BSpline4.T)-0.0001; //find max "T" in spline function subtract tol
AnyVar RadiusRatioMinInSplineFun =min(RadiusFactorFun_BSpline4.T)+0.0001;//find min "T" in spline function add tol
AnyVar RadiusRatioCapped_max = iffun(gtfun(RadiusRatio,RadiusRatioMaxInSplineFun),RadiusRatioMaxInSplineFun,RadiusRatio) ; //cap ratio
AnyVar RadiusRatioCapped = iffun(ltfun(RadiusRatioCapped_max,RadiusRatioMinInSplineFun),RadiusRatioMinInSplineFun,RadiusRatioCapped_max) ;
//Ensure the Velatio is not outside the interpol fun
AnyVar VelRatioMaxInSplineFun =max(VelFactorFun_BSpline4.T)-0.0001; //find max "T" in spline function subtract tol
AnyVar VelRatioMinInSplineFun =min(VelFactorFun_BSpline4.T)+0.0001;//find min "T" in spline function add tol
AnyVar VelRatioCapped_max = iffun(gtfun(VelRatio,VelRatioMaxInSplineFun),VelRatioMaxInSplineFun,VelRatio) ; //cap ratio
AnyVar VelRatioCapped = iffun(ltfun(VelRatioCapped_max,VelRatioMinInSplineFun),VelRatioMinInSplineFun,VelRatioCapped_max) ;
AnyVar HeightFactorFun = iffun(lteqfun(HeightRatio, FirstControlHeightRatio),
// iffun(ltfun(HeightRatio, SecondControlHeightRatio), 1.0, HeightFactorFun_BSpline4(HeightRatio)[0]),
iffun(ltfun(HeightRatio, SecondControlHeightRatio), 1.0, HeightFactorFun_BSpline4(HeightRatioCapped)[0]),
0.0);
AnyVar RadiusFactorFun = iffun(lteqfun(RadiusRatio, FirstControlRadiusRatio),
//iffun(ltfun(RadiusRatio, SecondControlRadiusRatio), 1.0, RadiusFactorFun_BSpline4(RadiusRatio)[0]),
iffun(ltfun(RadiusRatio, SecondControlRadiusRatio), 1.0, RadiusFactorFun_BSpline4(RadiusRatioCapped)[0]),
0.0);
AnyVar VelFactorFun = iffun(lteqfun(VelRatio, FirstControlVelRatio),
// iffun(ltfun(VelRatio, SecondControlVelRatio), 1.0, VelFactorFun_BSpline4(VelRatio)[0]),
iffun(ltfun(VelRatio, SecondControlVelRatio), 1.0, VelFactorFun_BSpline4(VelRatioCapped)[0]),
0.0);
#endif
#if _SMOOTHING_FUNCTION_PROFILE_ == "FUNC_PROFILE_COSINE"
AnyVar FirstControlHeightRatio = 1.0;
AnyVar SecondControlHeightRatio = 0.8;
AnyVar FirstControlRadiusRatio = 1.0;
AnyVar SecondControlRadiusRatio = 0.9;
AnyVar FirstControlVelRatio = 1.0;
AnyVar SecondControlVelRatio = 0.15;
AnyVar HeightFactorFun = iffun(lteqfun(HeightRatio, FirstControlHeightRatio),
iffun(ltfun(HeightRatio, SecondControlHeightRatio), 1.0,
(cos((HeightRatio-SecondControlHeightRatio)/(FirstControlHeightRatio-SecondControlHeightRatio)*pi)+1)*0.5),
0.0);
AnyVar RadiusFactorFun = iffun(lteqfun(RadiusRatio, FirstControlRadiusRatio),
iffun(ltfun(RadiusRatio, SecondControlRadiusRatio), 1.0,
(cos((RadiusRatio-SecondControlRadiusRatio)/(FirstControlRadiusRatio-SecondControlRadiusRatio)*pi)+1)*0.5),
0.0);
AnyVar VelFactorFun = iffun(lteqfun(VelRatio, FirstControlVelRatio),
iffun(ltfun(VelRatio, SecondControlVelRatio), 1.0,
(cos((VelRatio-SecondControlVelRatio)/(FirstControlVelRatio-SecondControlVelRatio)*pi)+1)*0.5),
0.0);
#endif
#if _SMOOTHING_FUNCTION_PROFILE_ == "FUNC_PROFILE_LINEAR"
AnyVar FirstControlHeightRatio = 1.0;
AnyVar SecondControlHeightRatio = 0.8;
AnyVar FirstControlRadiusRatio = 1.0;
AnyVar SecondControlRadiusRatio = 0.9;
AnyVar FirstControlVelRatio = 1.0;
AnyVar SecondControlVelRatio = 0.15;
AnyVar HeightFactorFun = iffun(lteqfun(HeightRatio, FirstControlHeightRatio),
iffun(ltfun(HeightRatio, SecondControlHeightRatio), 1.0, HeightFactorFun_Lin(HeightRatio)),
0.0);
AnyVar RadiusFactorFun = iffun(lteqfun(RadiusRatio, FirstControlRadiusRatio),
iffun(ltfun(RadiusRatio, SecondControlRadiusRatio), 1.0, RadiusFactorFun_Lin(RadiusRatio)),
0.0);
AnyVar VelFactorFun = iffun(lteqfun(VelRatio, FirstControlVelRatio),
iffun(ltfun(VelRatio, SecondControlVelRatio), 1.0, VelFactorFun_Lin(VelRatio)),
0.0);
#endif
#if _SMOOTHING_FUNCTION_PROFILE_ == "FUNC_PROFILE_TEST"
AnyVar FirstControlHeightRatio = 1.0;
AnyVar SecondControlHeightRatio = 0.8;
AnyVar FirstControlRadiusRatio = 1.0;
AnyVar SecondControlRadiusRatio = 0.9;
AnyVar FirstControlVelRatio = 1.0;
AnyVar SecondControlVelRatio = 0.15;
AnyVar HeightFactorFun = iffun(FirstControlHeightCheck,
iffun(SecondControlHeightCheck, 1.0, HeightFactorFun_Lin(HeightRatio)),
0.0);
AnyVar RadiusFactorFun = iffun(FirstControlRadiusCheck,
iffun(SecondControlRadiusCheck, 1.0, RadiusFactorFun_Lin(RadiusRatio)),
0.0);
// AnyVar VelFactorFun = iffun(FirstControlVelCheck,
// iffun(SecondControlVelCheck, 1.0, VelFactorFun_BSpline4(VelRatio)[0]),
// 0.0);
// AnyVar VelFactorFun = iffun(FirstControlVelCheck,
// VelFactorFun_SW(VelRatio)[0],
// 0.0);
AnyVar VelFactorFun = iffun(FirstControlVelCheck,
iffun(SecondControlVelCheck , 1.0,
(cos((VelRatio-SecondControlVelRatio)/(FirstControlVelRatio-SecondControlVelRatio)*pi)+1)*0.5),
0.0);
#endif
#undef _SMOOTHING_FUNCTION_PROFILE_
AnyVar DisFactorFun = HeightFactorFun * RadiusFactorFun;
AnyVar SmoothingFunction = DisFactorFun * VelFactorFun;
AnyVar ContactStrengthSmoothed = SmoothingFunction * .Strength;
AnyVar ContactStrengthDisAndVel = iffun(andfun(andfun(andfun(LowerThanHighDistLimit,GreaterThanLowDistLimit),LowerThanRadiusLimit),LowerThanHighVelLimit), .Strength, 0.0);
AnyVar ContactStrengthOriginal = iffun( andfun(andfun(LowerThanHighDistLimit,GreaterThanLowDistLimit),LowerThanRadiusLimit),.Strength,0.0);
//<--Test
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 Direction0Muscle ={
AnyRecruitedActuator NormalDirectionMuscle={
AnyKinMeasureOrg NormalDir={
AnyKinLinear &ref=...Lin;
MeasureOrganizer={....Direction[0]}; //always pick first element
};
Strength = ..ContactStrengthSmoothed;
Type = NonNegative;
//AnyVar StrengthTestVal =
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}
};
OutDim = 1;
};
AnyRecruitedActuator FrictionMuscle={
// AnyKinMeasure& ref1=.FrictionDir;
AnyKinMeasureLinComb &ref1=.LinCombinationMeasure;
Strength = ...ContactStrengthSmoothed;
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}
};
OutDim = 1;
};
AnyRecruitedActuator FrictionMuscle={
AnyKinMeasureLinComb &ref1=.LinCombinationMeasure;
// AnyKinMeasure& ref1=.FrictionDir;
Strength = ...ContactStrengthSmoothed;
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}
};
OutDim = 1;
};
AnyRecruitedActuator FrictionMuscle={
AnyKinMeasureLinComb &ref1=.LinCombinationMeasure;
// AnyKinMeasure& ref1=.FrictionDir;
Strength = ...ContactStrengthSmoothed;
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}
};
OutDim = 1;
};
AnyRecruitedActuator FrictionMuscle={
AnyKinMeasureLinComb &ref1=.LinCombinationMeasure;
// AnyKinMeasure& ref1=.FrictionDir;
Strength = ...ContactStrengthSmoothed;
Type = NonPositive;
SET_DEFAULT_ACTUATOR_VOLUME;
};
};//Pull
};
AnyFolder RefMuscles =
{
AnyRecruitedForce& ref0 = .Direction0Muscle.NormalDirectionMuscle;
AnyRecruitedForce& ref1 = .Direction01Muscle.Push.FrictionMuscle;
AnyRecruitedForce& ref2 = .Direction01Muscle.Pull.FrictionMuscle;
AnyRecruitedForce& ref3 = .Direction02Muscle.Push.FrictionMuscle;
AnyRecruitedForce& ref4 = .Direction02Muscle.Pull.FrictionMuscle;
};
// AnyForceMomentMeasure ForceMomentMeasure =
// {
// AnyForceBase& ref0 = .Direction0Muscle.NormalDirectionMuscle;
// AnyForceBase& ref1 = .Direction01Muscle.Push.FrictionMuscle;
// AnyForceBase& ref2 = .Direction01Muscle.Pull.FrictionMuscle;
// AnyForceBase& ref3 = .Direction02Muscle.Push.FrictionMuscle;
// AnyForceBase& ref4 = .Direction02Muscle.Pull.FrictionMuscle;
//
// AnyRefFrame& base = ..BaseObject;
//
// AnyVec3 Flocal = F * base.Axes;
// AnyVec3 Mlocal = M * base.Axes;
// };
AnyDrawVector DrawNormalForce =
{
AnyRefFrame &ref=..TargetObject;
AnyVar Strength= max({.ContactStrengthSmoothed, 0.00001});
Vec =..ScaleFactor*..DrawRef.DrawSettingsSupport.Lin.ScaleFactor*(
abs(.NormalVector*.Direction0Muscle.NormalDirectionMuscle.Fout[0])*..BaseObject.Axes'
+ abs(.NormalVector*.Direction01Muscle.Push.FrictionMuscle.Fout[0])*..BaseObject.Axes'
+ abs(.NormalVector*.Direction02Muscle.Push.FrictionMuscle.Fout[0])*..BaseObject.Axes'
+ abs(.NormalVector*.Direction01Muscle.Pull.FrictionMuscle.Fout[0])*..BaseObject.Axes'
+ abs(.NormalVector*.Direction02Muscle.Pull.FrictionMuscle.Fout[0])*..BaseObject.Axes');
AnyVar length=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= max({.ContactStrengthSmoothed,0.00001});
AnyVec3 NormalForceVector =..ScaleFactor*..DrawRef.DrawSettingsSupport.Lin.ScaleFactor*(
abs(.NormalVector*.Direction0Muscle.NormalDirectionMuscle.Fout[0])*..BaseObject.Axes'
+abs(.NormalVector*.Direction01Muscle.Push.FrictionMuscle.Fout[0])*..BaseObject.Axes'
+abs(.NormalVector*.Direction02Muscle.Push.FrictionMuscle.Fout[0])*..BaseObject.Axes'
+abs(.NormalVector*.Direction01Muscle.Pull.FrictionMuscle.Fout[0])*..BaseObject.Axes'
+abs(.NormalVector*.Direction02Muscle.Pull.FrictionMuscle.Fout[0])*..BaseObject.Axes');
AnyVar length=Strength*(2-(1-Strength/sqrt(Strength^2)));
};
AnyDrawVector DrawFrictionForce =
{
AnyRefFrame &ref=..TargetObject;
Vec =..ScaleFactor*..DrawRef.DrawSettingsSupport.Lin.ScaleFactor*..StaticFrictionCoefficient*(
abs(.FrictionVector1*.Direction01Muscle.Push.FrictionMuscle.Fout[0])
-abs(.FrictionVector1*.Direction01Muscle.Pull.FrictionMuscle.Fout[0])
+abs(.FrictionVector2*.Direction02Muscle.Push.FrictionMuscle.Fout[0])
-abs(.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 = NormalForceValue.NormalForceVector/(.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;
//AnyVar TotalForceScalar = (TotalForceVec[0]^2+TotalForceVec[1]^2+TotalForceVec[2]^2)^0.5;
/*
AnyDrawVector DrawFrictionForce1 = {
AnyRefFrame &ref=..TargetObject;
Vec =0.001*..StaticFrictionCoefficient*(
abs(.FrictionVector1*.Direction01Muscle.Push.FrictionMuscle.Fout[0])*..BaseObject.Axes')+
-abs(.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*(
abs(.FrictionVector2*.Direction02Muscle.Push.FrictionMuscle.Fout[0])*..BaseObject.Axes')+
-abs(.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;
};
*/
};
Datasets
Models
