Name | Description |
---|---|
GravityPool | gravity pool |
AxesRotQ | computes a quaternion out of axes rotations |
MulQ | multiplies (rotates) quaternions |
cardanRotation | defines relation between the rotational matrix and the cardan angles |
planarRotation | defines relation between the rotational matrix and a planar rotation angle |
quaternionRotation | defines relation between the rotational matrix and quaternions |
Rotation | rotates R1 by Rrel o R2 or the other way round |
toCardanAngles | integrates the angular velocity into the cardan angles |
toQuaternions | integrates the angular velocity into the quaternions |
Translation | translates the position |
The gravity pool contains a finite set of bodies and applies the law of mutual gravitational attraction on them.
Name | Default | Description |
---|---|---|
poolSize | 0 |
model GravityPool "gravity pool" import SI = Modelica.SIunits; parameter Integer poolSize = 0; SI.Position pos[poolSize,3]; SI.Mass mass[poolSize]; public function gravityForce = gravityForce2 (poolSize=poolSize, pos=pos, mass=mass); protected function gravityForce2 "Gravity field acceleration depending on field type and position" extends Modelica.Icons.Function; input Integer gravityPoolIndex "Index of the object the gravity is acting on"; input Integer poolSize; input SI.Position pos[poolSize,3] "Pool of all positions"; input SI.Mass mass[poolSize] "Pool of all masses"; output SI.Force gF[3] "Gravity force for indeed object, resolved in world frame"; protected SI.Position curPos[3]; SI.Position myPos[3]; SI.Position r[3]; SI.Mass curMass; SI.Mass myMass; Real G = Modelica.Constants.G; algorithm gF := zeros(3); if gravityPoolIndex > 0 then curPos := zeros(3); r := zeros(3); myPos := pos[gravityPoolIndex,:]; curMass := 0; myMass := mass[gravityPoolIndex]; for i in 1:poolSize loop if i == gravityPoolIndex then gF := gF + zeros(3); else curMass := mass[i]; curPos := pos[i,:]; r := myPos-curPos; gF := gF + ((G*curMass*myMass)/(r*r))*(-r/sqrt(r*r)); end if; end for; end if; end gravityForce2; end GravityPool;
The translation is specified by the parameter r, which is resolved in the body system specified by the signal R.
The translation can optionally be amplified by the signal ampl.
Name | Default | Description |
---|---|---|
r[3] | {1,0,0} |
model Translation "translates the position" MultiBondLib.Interfaces.RealSignal x1[3]; MultiBondLib.Interfaces.RealSignal x2[3]; MultiBondLib.Interfaces.RealSignal R[3,3]; parameter Real r[3] = {1,0,0}; MultiBondLib.Interfaces.RealSignal ampl; equation if cardinality(ampl) == 0 then ampl = 1; end if; x2 = x1 + transpose(R)*(r*ampl); end Translation;
The boolean input signal dirForward determines the form of the statement.
model Rotation "rotates R1 by Rrel o R2 or the other way round" MultiBondLib.Interfaces.RealSignal R1[3,3]; MultiBondLib.Interfaces.RealSignal R2[3,3]; MultiBondLib.Interfaces.RealSignal Rrel[3,3]; Modelica.Blocks.Interfaces.BooleanInput dirForward; equation if dirForward then R2 = Rrel*R1; else R1 = transpose(Rrel)*R2; end if; end Rotation;
The rotation axis can be specified by the parameter n.
Name | Default | Description |
---|---|---|
n[3] | {0,0,1} |
model planarRotation "defines relation between the rotational matrix and a planar rotation angle" MultiBondLib.Interfaces.RealSignal phi; MultiBondLib.Interfaces.RealSignal Rrel[3,3]; parameter Real n[3] = {0,0,1}; equation Rrel = [n]*transpose([n]) + (identity(3) - [n]*transpose([n]))*cos(phi) - skew(n)*sin(phi); end planarRotation;
model toQuaternions "integrates the angular velocity into the quaternions" MultiBondLib.Interfaces.RealSignal Q[4]; MultiBondLib.Interfaces.RealSignal w[3]; equation // w= 2*[Q[4], -Q[3], Q[2], - Q[1]; Q[3], Q[4], -Q[1], -Q[2]; -Q[2], Q[1], Q[4], -Q[3]] *der(Q); w= 2*[Q[4], Q[3], -Q[2], - Q[1]; -Q[3], Q[4], Q[1], -Q[2]; Q[2], -Q[1], Q[4], -Q[3]] *der(Q); Q*Q = 1; end toQuaternions;
model quaternionRotation "defines relation between the rotational matrix and quaternions" MultiBondLib.Interfaces.RealSignal Q[4]; MultiBondLib.Interfaces.RealSignal Rrel[3,3]; protected Real u[3] = Q[1:3]; Real c = Q[4]; equation Rrel = 2*( ([u]*transpose([u])) - c*skew(u) + (c*c*identity(3))) - identity(3); //(identity(3) - [n]*transpose([n]))*cos(phi) - skew(n)*sin(phi); end quaternionRotation;
The three rotation axes can be specified by the parameter sequence_angles
Name | Default | Description |
---|---|---|
sequence_angles[3] | {1,2,3} |
model cardanRotation "defines relation between the rotational matrix and the cardan angles" MultiBondLib.Interfaces.RealSignal phi[3]; MultiBondLib.Interfaces.RealSignal Rrel[3,3]; parameter Integer[3] sequence_angles(min={1,1,1}, max={3,3,3}) = {1,2,3}; protected Real R1[3,3]; Real R2[3,3]; Real R3[3,3]; equation // Rx = [1,0,0;0,cos(phi[1]),sin(phi[1]);0,-sin(phi[1]),cos(phi[1])]; // Ry = [cos(phi[2]),0,-sin(phi[2]);0,1,0;sin(phi[2]),0,cos(phi[2])]; // Rz = [cos(phi[3]),sin(phi[3]),0;-sin(phi[3]),cos(phi[3]),0;0,0,1]; // Rrel = Rz*Ry*Rx; if sequence_angles[1] == 1 then R1 = [1,0,0;0,cos(phi[1]),sin(phi[1]);0,-sin(phi[1]),cos(phi[1])]; end if; if sequence_angles[2] == 1 then R2 = [1,0,0;0,cos(phi[2]),sin(phi[2]);0,-sin(phi[2]),cos(phi[2])]; end if; if sequence_angles[3] == 1 then R3 = [1,0,0;0,cos(phi[3]),sin(phi[3]);0,-sin(phi[3]),cos(phi[3])]; end if; if sequence_angles[1] == 2 then R1 = [cos(phi[1]),0,-sin(phi[1]);0,1,0;sin(phi[1]),0,cos(phi[1])]; end if; if sequence_angles[2] == 2 then R2 = [cos(phi[2]),0,-sin(phi[2]);0,1,0;sin(phi[2]),0,cos(phi[2])]; end if; if sequence_angles[3] == 2 then R3 = [cos(phi[3]),0,-sin(phi[3]);0,1,0;sin(phi[3]),0,cos(phi[3])]; end if; if sequence_angles[1] == 3 then R1 = [cos(phi[1]),sin(phi[1]),0;-sin(phi[1]),cos(phi[1]),0;0,0,1]; end if; if sequence_angles[2] == 3 then R2 = [cos(phi[2]),sin(phi[2]),0;-sin(phi[2]),cos(phi[2]),0;0,0,1]; end if; if sequence_angles[3] == 3 then R3 = [cos(phi[3]),sin(phi[3]),0;-sin(phi[3]),cos(phi[3]),0;0,0,1]; end if; Rrel = R3*R2*R1; end cardanRotation;
The three rotation axes can be specified by the parameter sequence_angles
Name | Default | Description |
---|---|---|
sequence_angles[3] | {1,2,3} |
model toCardanAngles "integrates the angular velocity into the cardan angles" import SI = Modelica.SIunits; MultiBondLib.Interfaces.RealSignal phi[3]; MultiBondLib.Interfaces.RealSignal w[3]; parameter Integer[3] sequence_angles(min={1,1,1}, max={3,3,3}) = {1,2,3}; protected Real R2[3,3]; Real R3[3,3]; SI.AngularVelocity w1[3]; SI.AngularVelocity w2[3]; SI.AngularVelocity w3[3]; equation if sequence_angles[1] == 1 then w1 = {der(phi[1]),0,0}; end if; if sequence_angles[2] == 1 then R2 = [1,0,0;0,cos(phi[2]),sin(phi[2]);0,-sin(phi[2]),cos(phi[2])]; w2 = {der(phi[2]),0,0}; end if; if sequence_angles[3] == 1 then R3 = [1,0,0;0,cos(phi[3]),sin(phi[3]);0,-sin(phi[3]),cos(phi[3])]; w3 = {der(phi[3]),0,0}; end if; if sequence_angles[1] == 2 then w1 = {0,der(phi[1]),0}; end if; if sequence_angles[2] == 2 then R2 = [cos(phi[2]),0,-sin(phi[2]);0,1,0;sin(phi[2]),0,cos(phi[2])]; w2 = {0,der(phi[2]),0}; end if; if sequence_angles[3] == 2 then R3 = [cos(phi[3]),0,-sin(phi[3]);0,1,0;sin(phi[3]),0,cos(phi[3])]; w3 = {0,der(phi[3]),0}; end if; if sequence_angles[1] == 3 then w1 = {0,0,der(phi[1])}; end if; if sequence_angles[2] == 3 then R2 = [cos(phi[2]),sin(phi[2]),0;-sin(phi[2]),cos(phi[2]),0;0,0,1]; w2 = {0,0,der(phi[2])}; end if; if sequence_angles[3] == 3 then R3 = [cos(phi[3]),sin(phi[3]),0;-sin(phi[3]),cos(phi[3]),0;0,0,1]; w3 = {0,0,der(phi[3])}; end if; w = w3 + R3*w2 + R3*R2*w1; // w = {0,0,der(phi[3])} + {sin(phi[3])*der(phi[2]),cos(phi[3])*der(phi[2]),0} + // {cos(phi[3])*cos(phi[2])*der(phi[1]),-sin(phi[3])*cos(phi[2])*der(phi[1]),sin(phi[2])*der(phi[1])}; end toCardanAngles;
Name | Default | Description |
---|---|---|
phi[3] | 3 cardan angles [rad] | |
seq_angles | sequence of axes |
Name | Default | Description |
---|---|---|
Q | resulting quaternion describing the rotation |
function AxesRotQ "computes a quaternion out of axes rotations" extends Modelica.Icons.Function; import SI = Modelica.SIunits; input SI.Angle phi[3] "3 cardan angles"; input Types.RotationSequence seq_angles "sequence of axes"; output Types.Quaternion Q "resulting quaternion describing the rotation"; protected Types.Quaternion Q1; Types.Quaternion Q2; Types.Quaternion Q3; algorithm if seq_angles[1] == 1 then Q1 := {sin(phi[1]/2),0,0, cos(phi[1]/2)}; end if; if seq_angles[2] == 1 then Q2 := {sin(phi[2]/2),0,0, cos(phi[2]/2)}; end if; if seq_angles[3] == 1 then Q3 := {sin(phi[3]/2),0,0, cos(phi[3]/2)}; end if; if seq_angles[1] == 2 then Q1 := {0,sin(phi[1]/2),0, cos(phi[1]/2)}; end if; if seq_angles[2] == 2 then Q2 := {0,sin(phi[2]/2),0, cos(phi[2]/2)}; end if; if seq_angles[3] == 2 then Q3 := {0,sin(phi[3]/2),0, cos(phi[3]/2)}; end if; if seq_angles[1] == 3 then Q1 := {0,0,sin(phi[1]/2),cos(phi[1]/2)}; end if; if seq_angles[2] == 3 then Q2 := {0,0,sin(phi[2]/2),cos(phi[2]/2)}; end if; if seq_angles[3] == 3 then Q3 := {0,0,sin(phi[3]/2),cos(phi[3]/2)}; end if; Q := MulQ(MulQ(Q1,Q2),Q3); end AxesRotQ;
Name | Default | Description |
---|---|---|
Q1 | Q1 | |
Q2 | Q2 |
Name | Default | Description |
---|---|---|
Q | Product = Q1*Q2 |
function MulQ "multiplies (rotates) quaternions" extends Modelica.Icons.Function; input Types.Quaternion Q1 "Q1"; input Types.Quaternion Q2 "Q2"; output Types.Quaternion Q "Product = Q1*Q2"; algorithm Q[4] := (Q1[4]*Q2[4]) - (Q1[1:3]*Q2[1:3]); Q[1:3] := cross(Q1[1:3],Q2[1:3]) + (Q1[4]*Q2[1:3]) + (Q2[4]*Q1[1:3]); end MulQ;
function gravityForce2 "Gravity field acceleration depending on field type and position" extends Modelica.Icons.Function; input Integer gravityPoolIndex "Index of the object the gravity is acting on"; input Integer poolSize; input SI.Position pos[poolSize,3] "Pool of all positions"; input SI.Mass mass[poolSize] "Pool of all masses"; output SI.Force gF[3] "Gravity force for indeed object, resolved in world frame"; protected SI.Position curPos[3]; SI.Position myPos[3]; SI.Position r[3]; SI.Mass curMass; SI.Mass myMass; Real G = Modelica.Constants.G; algorithm gF := zeros(3); if gravityPoolIndex > 0 then curPos := zeros(3); r := zeros(3); myPos := pos[gravityPoolIndex,:]; curMass := 0; myMass := mass[gravityPoolIndex]; for i in 1:poolSize loop if i == gravityPoolIndex then gF := gF + zeros(3); else curMass := mass[i]; curPos := pos[i,:]; r := myPos-curPos; gF := gF + ((G*curMass*myMass)/(r*r))*(-r/sqrt(r*r)); end if; end for; end if; end gravityForce2;