Name | Description |
---|---|
CollisionSpherePlane | models the collision between a spheres and a plane |
CollisionSphereSphere | models the collision between two spheres |
NoCollision | dummy collision element |
The characteristics of the geometry can be set by the parameters:
The parameter s_small in the advanced menu reduces the stiffness of the friction for slip velocities in the range from zero up to s_small.
Name | Default | Description |
---|---|---|
ra | 1 | radius of sphere at frame a [m] |
rb | 1 | radius of sphere at frame b [m] |
elasticity | 1.0 | Elasticity of impuls |
muR | 0.0 | friction coefficient |
Advanced | ||
s_small | 1e-4 | critical length [m] |
contactDuration | 1e-9 | time to handle the impulses |
useParameters | true | use the parameter values. |
model CollisionSphereSphere "models the collision between two spheres" import SI = Modelica.SIunits; Interfaces.IFrame_a frame_a; parameter SI.Radius ra = 1 "radius of sphere at frame a"; parameter SI.Radius rb = 1 "radius of sphere at frame b"; parameter SI.Position s_small = 1e-4 "|Advanced||critical length"; parameter Real elasticity = 1.0 "Elasticity of impuls"; parameter Real muR = 0.0 "friction coefficient"; parameter Real contactDuration = 1e-9 "|Advanced||time to handle the impulses"; parameter Boolean useParameters = true "|Advanced||use the parameter values."; SI.Radius ra_; SI.Radius rb_; Real elasticity_; Real muR_; protected SI.Distance r[3]; Real eR[3]; Real noteR[3]; Real eA[3]; Real eB[3]; Boolean contact; Integer seqState; Real frictionImpulseTime; Real correctionImpulseTime; SI.Impulse F[3]; SI.AngularImpulse T[3]; SI.Velocity vA[3]; SI.Velocity Vm[3]; SI.AngularVelocity Wm[3]; SI.Impulse FR; SI.Impulse FA0; SI.Impulse FB0; SI.Velocity VmR0; SI.Impulse FA1; SI.Impulse FB1; SI.Impulse FR1; SI.Velocity VFricElast[3]; Real eC[3]; Real noteC[3]; Real eD[3]; Real eE[3]; SI.Impulse FD2; SI.Impulse FE2; SI.Velocity VmC2; Real[3,3] R_a; Real[3,3] R_b; public Modelica.Blocks.Interfaces.BooleanOutput y; Interfaces.IFrame_b frame_b; initial equation contact = false; seqState = -1; frictionImpulseTime = 0; correctionImpulseTime = 0; equation algorithm when contact then contact :=false; end when; when sqrt(r*r) <= (ra_+rb_) then contact :=true; seqState := 0; frictionImpulseTime :=time + contactDuration/2; end when; when (time >= frictionImpulseTime) and (seqState == 0) then contact :=true; seqState := 1; frictionImpulseTime := 0; correctionImpulseTime := time + contactDuration/2; end when; when (time >= correctionImpulseTime) and (seqState == 1) then contact :=true; seqState := 2; correctionImpulseTime := 0; end when; equation if useParameters then ra_ = ra; rb_ = rb; elasticity_ = elasticity; muR_ = muR; end if; r = frame_b.P.x- frame_a.P.x; when contact and (seqState == 0) then eR = r/sqrt(r*r); noteR = if abs(eR[1]) > 0.1 then {0,1,0} else (if abs(eR[2]) > 0.1 then {0,0,1} else {1,0,0}); eA = cross(eR, noteR)/sqrt(cross(eR, noteR)*cross(eR, noteR)); eB = cross(eA, eR); end when; when contact then vA = (pre(frame_b.P.v)+cross(transpose(R_b)*pre(frame_b.P.w),-eR*rb_)) - (pre(frame_a.P.v)+cross(transpose(R_a)*pre(frame_a.P.w),eR*ra_)); R_a = frame_a.P.R; R_b = frame_b.P.R; end when; when contact then T = zeros(3); end when; when contact then FR = F*eR; end when; when contact and (seqState == 0) then VmR0 = (1-elasticity_)*(vA*eR)/2; FA0 = 0; FB0 = 0; end when; when contact and (seqState == 1) then FR1 = 0; FA1 = -muR_*abs(pre(FR))*((vA/sqrt(vA*vA))*eA); FB1 = -muR_*abs(pre(FR))*((vA/sqrt(vA*vA))*eB); VFricElast[1] = if sign(vA[1])*Vm[1] >= abs(vA[1]/2) then 0 else Vm[1]- (vA[1]/2); VFricElast[2] = if sign(vA[2])*Vm[2] >= abs(vA[2]/2) then 0 else Vm[2]- (vA[2]/2); VFricElast[3] = if sign(vA[3])*Vm[3] >= abs(vA[3]/2) then 0 else Vm[3]- (vA[3]/2); end when; when contact and (seqState == 2) and (pre(VFricElast)*pre(VFricElast) > 0) then eC = if pre(VFricElast)*pre(VFricElast) < s_small^2 then {1,0,0} else pre(VFricElast)/sqrt(pre(VFricElast)*pre(VFricElast)); noteC = if abs(eC[1]) > 0.1 then {0,1,0} else (if abs(eC[2]) > 0.1 then {0,0,1} else {1,0,0}); eD = cross(eC, noteC)/sqrt(cross(eC, noteC)*cross(eC, noteC)); eE = cross(eD, eC); VmC2 = sqrt(pre(VFricElast)*pre(VFricElast)); FD2 = 0; FE2 = 0; end when; if contact then if (seqState == 0) then Vm*eR = VmR0; F*eA = FA0; F*eB = FB0; else if (seqState == 1) then F*eR = FR1; F*eA = FA1; F*eB = FB1; else if (seqState == 2) and (pre(VFricElast)*pre(VFricElast) > 0) then Vm*eC = VmC2; F*eD = FD2; F*eE = FE2; else F = zeros(3); end if; end if; end if; else F = zeros(3); end if; y = contact; // Vm = frame_b.Vm - frame_a.Vm; Vm = (frame_b.Vm+cross(transpose(R_b)*frame_b.Wm,-eR*rb_)) - (frame_a.Vm+cross(transpose(R_a)*frame_a.Wm,eR*ra_)); F = -frame_b.F; F = frame_a.F; Wm = frame_b.Wm - frame_a.Wm;//actually incorrect but ok because Wm is of no relevance // T = -frame_b.T; //actually incorrect but ok because T=0; // T = frame_a.T; //actually incorrect but ok because T=0; T+R_b*cross(-eR*rb_,F) = -frame_b.T; //actually incorrect but ok because T=0; T+R_a*cross(eR*ra_,F) = frame_a.T; //actually incorrect but ok because T=0; frame_a.f = zeros(3); frame_a.t = zeros(3); frame_b.f = zeros(3); frame_b.t = zeros(3); end CollisionSphereSphere;
The characteristics of the geometry can be set by the parameters:
The parameter s_small in the advanced menu reduces the stiffness of the friction for slip velocities in the range from zero up to s_small.
Name | Default | Description |
---|---|---|
ra | 1 | radius of sphere at frame a [m] |
Nb[3] | {0,1,0} | normal vector of plane [m] |
elasticity | 1.0 | Elasticity of impuls |
muR | 0.0 | friction coefficient |
Advanced | ||
s_small | 1e-4 | critical length [m] |
contactDuration | 1e-9 | time to handle the impulses |
model CollisionSpherePlane "models the collision between a spheres and a plane" import SI = Modelica.SIunits; Interfaces.IFrame_a frame_a; parameter SI.Radius ra = 1 "radius of sphere at frame a"; parameter SI.Radius Nb[3] = {0,1,0} "normal vector of plane"; parameter SI.Position s_small = 1e-4 "|Advanced||critical length"; parameter Real elasticity = 1.0 "Elasticity of impuls"; parameter Real muR = 0.0 "friction coefficient"; parameter Real contactDuration = 1e-9 "|Advanced||time to handle the impulses"; protected SI.Distance d; Real eR[3]; Real noteR[3]; Real eA[3]; Real eB[3]; Boolean contact; Integer seqState; Real frictionImpulseTime; Real correctionImpulseTime; SI.Impulse F[3]; SI.AngularImpulse T[3]; SI.Velocity vA[3]; SI.Velocity Vm[3]; SI.AngularVelocity Wm[3]; SI.Impulse FR; SI.Impulse FA0; SI.Impulse FB0; SI.Velocity VmR0; SI.Impulse FA1; SI.Impulse FB1; SI.Impulse FR1; SI.Velocity VFricElast[3]; Real eC[3]; Real noteC[3]; Real eD[3]; Real eE[3]; SI.Impulse FD2; SI.Impulse FE2; SI.Velocity VmC2; Real[3,3] R_a; Real[3,3] R_b; public Modelica.Blocks.Interfaces.BooleanOutput y; Interfaces.IFrame_b frame_b; initial equation contact = false; seqState = -1; frictionImpulseTime = 0; correctionImpulseTime = 0; equation algorithm when contact then contact :=false; end when; when d <= ra then contact :=true; seqState := 0; frictionImpulseTime :=time + contactDuration/2; end when; when (time >= frictionImpulseTime) and (seqState == 0) then contact :=true; seqState := 1; frictionImpulseTime := 0; correctionImpulseTime := time + contactDuration/2; end when; when (time >= correctionImpulseTime) and (seqState == 1) then contact :=true; seqState := 2; correctionImpulseTime := 0; end when; equation d = (frame_a.P.x- frame_b.P.x)*(transpose(frame_b.P.R)*Nb); when contact and (seqState == 0) then eR = transpose(R_b)*Nb/sqrt(Nb*Nb); noteR = if abs(eR[1]) > 0.1 then {0,1,0} else (if abs(eR[2]) > 0.1 then {0,0,1} else {1,0,0}); eA = cross(eR, noteR)/sqrt(cross(eR, noteR)*cross(eR, noteR)); eB = cross(eA, eR); end when; when contact then vA = (pre(frame_b.P.v)+cross(transpose(R_b)*pre(frame_b.P.w),zeros(3))) - (pre(frame_a.P.v)+cross(transpose(R_a)*pre(frame_a.P.w),-eR*ra)); R_a = frame_a.P.R; R_b = frame_b.P.R; end when; when contact then T = zeros(3); end when; when contact then FR = F*eR; end when; when contact and (seqState == 0) then VmR0 = (1-elasticity)*(vA*eR)/2; FA0 = 0; FB0 = 0; end when; when contact and (seqState == 1) then FR1 = 0; FA1 = -muR*abs(pre(FR))*((vA/sqrt(vA*vA))*eA); FB1 = -muR*abs(pre(FR))*((vA/sqrt(vA*vA))*eB); VFricElast[1] = if sign(vA[1])*Vm[1] >= abs(vA[1]/2) then 0 else Vm[1]- (vA[1]/2); VFricElast[2] = if sign(vA[2])*Vm[2] >= abs(vA[2]/2) then 0 else Vm[2]- (vA[2]/2); VFricElast[3] = if sign(vA[3])*Vm[3] >= abs(vA[3]/2) then 0 else Vm[3]- (vA[3]/2); end when; when contact and (seqState == 2) and (pre(VFricElast)*pre(VFricElast) > 0) then eC = if pre(VFricElast)*pre(VFricElast) < s_small^2 then {1,0,0} else pre(VFricElast)/sqrt(pre(VFricElast)*pre(VFricElast)); noteC = if abs(eC[1]) > 0.1 then {0,1,0} else (if abs(eC[2]) > 0.1 then {0,0,1} else {1,0,0}); eD = cross(eC, noteC)/sqrt(cross(eC, noteC)*cross(eC, noteC)); eE = cross(eD, eC); VmC2 = sqrt(pre(VFricElast)*pre(VFricElast)); FD2 = 0; FE2 = 0; end when; if contact then if (seqState == 0) then Vm*eR = VmR0; F*eA = FA0; F*eB = FB0; else if (seqState == 1) then F*eR = FR1; F*eA = FA1; F*eB = FB1; else if (seqState == 2) and (pre(VFricElast)*pre(VFricElast) > 0) then Vm*eC = VmC2; F*eD = FD2; F*eE = FE2; else F = zeros(3); end if; end if; end if; else F = zeros(3); end if; y = contact; // Vm = frame_b.Vm - frame_a.Vm; Vm = (frame_b.Vm+cross(transpose(R_b)*frame_b.Wm,zeros(3))) - (frame_a.Vm+cross(transpose(R_a)*frame_a.Wm,-eR*ra)); F = -frame_b.F; F = frame_a.F; Wm = frame_b.Wm - frame_a.Wm;//actually incorrect but ok because Wm is of no relevance // T = -frame_b.T; //actually incorrect but ok because T=0; // T = frame_a.T; //actually incorrect but ok because T=0; T+R_b*cross(zeros(3),F) = -frame_b.T; //actually incorrect but ok because T=0; T+R_a*cross(-eR*ra,F) = frame_a.T; //actually incorrect but ok because T=0; frame_a.f = zeros(3); frame_a.t = zeros(3); frame_b.f = zeros(3); frame_b.t = zeros(3); end CollisionSpherePlane;
model NoCollision "dummy collision element" import SI = Modelica.SIunits; Interfaces.IFrame_a frame_a; public Modelica.Blocks.Interfaces.BooleanOutput y; Interfaces.IFrame_b frame_b; equation y = false; zeros(3) = -frame_b.F; zeros(3) = frame_a.F; zeros(3) = -frame_b.T; zeros(3) = frame_a.T; frame_a.f = zeros(3); frame_a.t = zeros(3); frame_b.f = zeros(3); frame_b.t = zeros(3); end NoCollision;