Skip to content

Commit 21fcaa5

Browse files
MartinOtterMartinOtter
committed
- Bug fixed with regards to Object3D(..., angularVelocityResolvedInParent=true)
- Cleanup and improvements of Modia3D/src/Frames + documented in Modia3D/docs/Functions.md
1 parent 1c63d9a commit 21fcaa5

File tree

12 files changed

+286
-184
lines changed

12 files changed

+286
-184
lines changed

‎docs/make.jl‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@ makedocs(
2222
"Components/GravityField.md"
2323
"Components/ForceElements.md"
2424
],
25+
"Functions" => "Functions.md",
2526
"Internal" => [
2627
"internal/Profiling.md"
2728
"internal/DynamicDispatch.md"

‎docs/src/Functions.md‎

Lines changed: 137 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,137 @@
1+
# Functions
2+
3+
```@meta
4+
CurrentModule = Modia3D
5+
```
6+
7+
## 3D Vectors
8+
9+
Functions to construct 3D vectors `v::SVector{3,F}`.
10+
11+
```@docs
12+
ZeroVector3D
13+
axisValue
14+
```
15+
16+
## Rotation Matrices
17+
18+
Functions to construct rotation matrices `R::SMatrix{3,3,F,9}` rotating a
19+
coordinate-system/frame 1 into a coordinate-system/frame 2.
20+
21+
| Function | Description |
22+
|:-------------------------------------------------|:---------------------------------------------------|
23+
| [`Modia3D.NullRotation`](@ref)(::Type{F}) | No rotation from frame 1 to frame 2 |
24+
| [`Modia3D.assertRotationMatrix`](@ref)(R) | Assert that R is a rotation matrix |
25+
| [`Modia3D.rot1`](@ref)(angle) | Rotate around `angle` along x-axis |
26+
| [`Modia3D.rot2`](@ref)(angle) | Rotate around `angle` along y-axis |
27+
| [`Modia3D.rot3`](@ref)(angle) | Rotate around `angle` along z-axis |
28+
| [`Modia3D.rot123`](@ref)(angle1, angle2, angle3) | Rotate around angles along x,y,z-axes |
29+
| [`Modia3D.rot123`](@ref)(angles) | Rotate around angles along x,y,z-axes |
30+
| [`Modia3D.rotAxis`](@ref)(axis,angle) | Rotate around `angle` along `axis` (= 1,2,3) |
31+
| [`Modia3D.rotAxis`](@ref)(axis,positive,angle) | Rotate around `angle` if `positive`, else `-angle` |
32+
| [`Modia3D.rot_e`](@ref)(e, angle) | Rotate around `angle` along unit vector `e` |
33+
| [`Modia3D.rot_nxy`](@ref)(nx, ny) | `nx`/`ny` are in x/y-direction of frame 2 |
34+
| [`Modia3D.from_q`](@ref)(q) | Return rotation matrix from quaternion `q` |
35+
36+
37+
Examples
38+
39+
```julia
40+
using Modia3D
41+
42+
# R1,R2,R3 are the same RotationMatrices
43+
R1 = Modia3D.rot1(pi/2)
44+
R2 = Modia3D.rot1(90u"°")
45+
R3 = Modia3D.rot_e([1,0,0], pi/2)
46+
```
47+
48+
```@docs
49+
NullRotation
50+
assertRotationMatrix
51+
rot1
52+
rot2
53+
rot3
54+
rot123
55+
rotAxis
56+
rot_e
57+
rot_nxy
58+
from_q
59+
```
60+
61+
## Quaternions
62+
63+
Functions to construct quaternions `q::SVector{4,F}` rotating a
64+
coordinate-system/frame 1 into a coordinate-system/frame 2.
65+
66+
| Function | Description |
67+
|:--------------------------------------------------|:-------------------------------------------|
68+
| [`Modia3D.NullQuaternion`](@ref)(::Type{F}) | No rotation from frame 1 to frame 2 |
69+
| [`Modia3D.assertQuaternion`](@ref)(q) | Assert that q is a quaternion |
70+
| [`Modia3D.qrot1`](@ref)(angle) | Rotate around `angle` along x-axis |
71+
| [`Modia3D.qrot2`](@ref)(angle) | Rotate around `angle` along y-axis |
72+
| [`Modia3D.qrot3`](@ref)(angle) | Rotate around `angle` along z-axis |
73+
| [`Modia3D.qrot123`](@ref)(angle1, angle2, angle3) | Rotate around angles along x,y,z-axes |
74+
| [`Modia3D.qrot_e`](@ref)(e, angle) | Rotate around `angle` along unit vector `e`|
75+
| [`Modia3D.qrot_nxy`](@ref)(nx, ny) | `nx`/`ny` are in x/y-direction of frame 2 |
76+
| [`Modia3D.from_R`](@ref)(R) | Return `q` from rotation matrix `R` |
77+
78+
79+
```@docs
80+
NullQuaternion
81+
assertQuaternion
82+
qrot1
83+
qrot2
84+
qrot3
85+
qrot123
86+
qrot_e
87+
qrot_nxy
88+
from_R
89+
```
90+
91+
## Frame Transformations
92+
93+
Functions to transform vectors, rotation matrices, quaternions between coordinate systems
94+
and functions to determine properties from coordinate system transformations.
95+
96+
| Function | Description |
97+
|:-------------------------------------------------|:----------------------------------------------|
98+
| [`Modia3D.resolve1`](@ref)(rot, v2) | Transform vector `v` from frame 2 to frame 1 |
99+
| [`Modia3D.resolve2`](@ref)(rot, v1) | Transform vector `v` from frame 1 to frame 2 |
100+
| [`Modia3D.absoluteRotation`](@ref)(rot01, rot12) | Return rotation 0->2 from rot. 0->1 and 1->2 |
101+
| [`Modia3D.relativeRotation`](@ref)(rot01, rot02) | Return rotation 1->2 from rot. 0->1 and 0->2 |
102+
| [`Modia3D.inverseRotation`](@ref)(rot01) | Return rotation 1->0 from rot, 0->1 |
103+
| [`Modia3D.planarRotationAngle`](@ref)(e,v1,v2) | Return angle of planar rotation along `e` |
104+
| [`Modia3D.eAxis`](@ref)(axis) | Return unit vector `e` in direction of `axis` |
105+
| [`Modia3D.skew`](@ref)(v) | Return skew-symmetric matrix of vector v |
106+
107+
```@docs
108+
resolve1
109+
resolve2
110+
absoluteRotation
111+
relativeRotation
112+
inverseRotation
113+
planarRotationAngle
114+
eAxis
115+
skew
116+
```
117+
118+
## Frame Interpolations
119+
120+
Given a set of coordinate-systems/frames by a vector `r` of position vectors (to their origins) and
121+
and an optional vector `q` of Quaternions (of their absolute orientations), then
122+
the following functions interpolate linearly in these frames:
123+
124+
| Function | Description |
125+
|:------------------------------------------------|:-----------------------------------------------|
126+
| [`Modia3D.Path`](@ref)(r,q) | Return path defined by a vector of frames |
127+
| [`Modia3D.t_pathEnd`](@ref)(path) | Return path parameter `t_end` of last frame |
128+
| [`Modia3D.interpolate`](@ref)(path,t) | Return `(rt,qt)` of Path at path parameter `t` |
129+
| [`Modia3D.interpolate_r`](@ref)(path,t) | Return `rt` of Path at path parameter `t` |
130+
131+
132+
```@docs
133+
Path
134+
t_pathEnd
135+
interpolate
136+
interpolate_r
137+
```

‎docs/src/index.md‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -101,14 +101,16 @@ julia -JModia3D_sysimage.so (otherwise)
101101
- Additional keyword arguments of Object3D: `Object3D(..., fixedInParent=true, velocity=[0.0, 0.0, 0.0], angularVelocity=[0.0, 0.0, 0.0])`.
102102
A freely moving Object3D is defined with `Object3D(..., fixedInParent=false, ...)`. The states and other code for such Object3Ds are
103103
no longer visible in the generated code (so compilation is faster).
104+
105+
- New variants of functions: `Modia3D.rot1(angle,v), Modia3D.rot2(angle,v), Modia3D.rot3(angle,v), Modia3D.resolve1(rotation,v2), Modia3D.resolve2(rotation,v1)`.
106+
107+
**Deprecated**
104108

105109
- Joint `FreeMotion` is **deprecated**. Use instead `Object3D(..., fixedInParent=false, ...)`.
106110
Note, Object3D has variables `translation, rotation, velocity, angularVelocity` instead of `r, rot, v, w` of `FreeMotion`.
107111
Furthermore, `angularVelocity` is resolved in the parent `Object3D` whereas `w` in `FreeMotion(obj1=.., obj2=..., ..)` is resolved in
108112
`obj2` and not in `obj1`. This means in particular that the init/start value `FreeMotion(.., w=Var(start=w_start)...)` needs
109113
to be transformed in Object3D with `Object3D(..., fixedInParent=false, rotation=xxx, angularVelocity = Modia3D.resolve1(rotation,w_start))`.
110-
111-
- New variants of functions: `Modia3D.rot123(angles), Modia3D.resolve1(angles,v2), Modia3D.resolve2(angles,v1)`.
112114

113115

114116
**Non-backwards compatible changes**

‎src/Composition/_module.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,7 @@ export J123, J132, J123or132
4242

4343
export computeGeneralizedForces!
4444

45-
export distanceAndAngles, distance, planarRotationAngle
45+
export distanceAndAngles, distance
4646
export measFrameRotation, measFramePosition, measFrameDistance
4747
export measFrameRotVelocity, measFrameTransVelocity, measFrameDistVelocity
4848
export measFrameRotAcceleration, measFrameTransAcceleration, measFrameDistAcceleration

‎src/Composition/joints/joints.jl‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -89,7 +89,7 @@ mutable struct MultibodyData{F <: Modia3D.VarFloatType, TimeType}
8989

9090
# Define hidden model states and copy initial values into eqInfo
9191
path = obj.path * "."
92-
w_init = freeMotion.wResolvedInParent ? resolve2(freeMotion.rot, freeMotion.w) : freeMotion.w
92+
w_init = freeMotion.wResolvedInParent ? resolve2(freeMotion.rot, freeMotion.w, rotation123=freeMotion.isrot123) : freeMotion.w
9393
freeMotion.ix_hidden_r = Modia.newHiddenState!(partiallyInstantiatedModel, path*"translation" , path*"der(translation)" , freeMotion.r)
9494
freeMotion.ix_hidden_v = Modia.newHiddenState!(partiallyInstantiatedModel, path*"velocity" , path*"der(velocity)" , freeMotion.v)
9595
freeMotion.ix_hidden_rot = Modia.newHiddenState!(partiallyInstantiatedModel, path*"rotation" , path*"der(rotation)" , freeMotion.rot)
@@ -698,7 +698,7 @@ function setStatesHiddenJoints!(m::Modia.SimulationModel{F,TimeType}, mbs::Multi
698698
j3 = freeMotion.ix_hidden_v ; freeMotion.v = SVector{3,F}(x_hidden[j3], x_hidden[j3+1], x_hidden[j3+2])
699699
j4 = freeMotion.ix_hidden_w ; freeMotion.w = SVector{3,F}(x_hidden[j4], x_hidden[j4+1], x_hidden[j4+2])
700700
if freeMotion.wResolvedInParent
701-
freeMotion.w = resolve2(obj.R_rel,freeMotion.w)
701+
freeMotion.w = resolve2(freeMotion.rot, freeMotion.w, rotation123 = freeMotion.isrot123)
702702
end
703703

704704
# Copy some freeMotion state derivatives into der_x_hidden

‎src/Composition/sensors.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515
Under the assumption that the z-axes of frame1 and frame2 coincide, return the
1616
angle between the x-axis of frame1 and the position vector from frame1 to frame2.
1717
"""
18-
function planarRotationAngle(frame1::Object3D, frame2::Object3D)
18+
function Frames.planarRotationAngle(frame1::Object3D, frame2::Object3D)
1919
# Extract needed vectors
2020
e1z = frame1.R_abs[3,:]
2121
e2z = frame2.R_abs[3,:]

‎src/Frames/_module.jl‎

Lines changed: 3 additions & 107 deletions
Original file line numberDiff line numberDiff line change
@@ -1,117 +1,13 @@
11
# License for this file: MIT (expat)
2-
# Copyright 2017-2018, DLR Institute of System Dynamics and Control
2+
# Copyright 2017-2022, DLR Institute of System Dynamics and Control
33

44
"""
55
module Modia3D.Frames
66
77
This module contains functions for **frames** that is coordinate systems in 3D.
88
The orientation of a frame is described either with a 3x3 **rotation matrix**
9-
or with a **quaternion vector** and its origin is described with a **SVector{3,Float64}**:
10-
11-
- Rotation matrix `SMatrix{3,3,Float64,9}`:
12-
Type of a Rotation matrix to rotate from a frame 1 into a frame 2.
13-
14-
- `quaternion = SVector{4,Float64}`:
15-
Type of a quaternion vector to rotate from a frame 1 into a frame 2.
16-
17-
18-
The following constants are defined
19-
20-
- `Modia3D.NullRotation(F)`:
21-
rotation matrix with no rotation from a frame 1 into a frame 2.
22-
23-
- `Modia3D.NullQuaternion(F)`:
24-
Quaternion vector with no rotation from a frame 1 into a frame 2.
25-
26-
- `Modia3D.ZeroVector3D(F)`:
27-
SVector{3,Float64} with only zero elements.
28-
29-
If an angle is given as an argument to one of the functions below, it might be a
30-
number (interpreted as having unit `rad`) or a number with a unit
31-
(for example: `using Unitful; angle = 90u"°"`).
32-
33-
34-
# Constructors for a RotationMatrix R
35-
36-
The following functions return a rotation matrix `R`
37-
to rotate a frame 1 into a frame 2.
38-
39-
| Function | Description |
40-
|:-------------------------------------------------|:---------------------------------------------------|
41-
| [`Modia3D.rot1`](@ref)(angle) | Rotate around `angle` along x-axis |
42-
| [`Modia3D.rot2`](@ref)(angle) | Rotate around `angle` along y-axis |
43-
| [`Modia3D.rot3`](@ref)(angle) | Rotate around `angle` along z-axis |
44-
| [`Modia3D.rot123`](@ref)(angle1, angle2, angle3) | Rotate around angles along x,y,z-axes |
45-
| [`Modia3D.rotAxis`](@ref)(axis,angle) | Rotate around `angle` along `axis` (= 1,2,3) |
46-
| [`Modia3D.rotAxis`](@ref)(axis,positive,angle) | Rotate around `angle` if `positive`, else `-angle` |
47-
| [`Modia3D.rot_e`](@ref)(e, angle) | Rotate around `angle` along unit vector `e` |
48-
| [`Modia3D.rot_nxy`](@ref)(nx, ny) | `nx`/`ny` are in x/y-direction of frame 2 |
49-
| [`Modia3D.from_q`](@ref)(q) | Return `R` from quaternion `q` |
50-
51-
52-
# Constructors for a quaternion q
53-
54-
The following functions return a quaternion SVector{4,F} `q`
55-
to rotate a frame 1 into a frame 2.
56-
Since `q` and `-q` define the same rotation the constructor functions have
57-
a keyword argument `q_guess::SVector{4,F} = NullQuaternion(F)`.
58-
From the two possible solutions `q` the one is returned that is closer to `q_guess`.
59-
60-
| Function | Description |
61-
|:--------------------------------------------------|:-------------------------------------------|
62-
| [`Modia3D.qrot1`](@ref)(angle) | Rotate around `angle` along x-axis |
63-
| [`Modia3D.qrot2`](@ref)(angle) | Rotate around `angle` along y-axis |
64-
| [`Modia3D.qrot3`](@ref)(angle) | Rotate around `angle` along z-axis |
65-
| [`Modia3D.qrot123`](@ref)(angle1, angle2, angle3) | Rotate around angles along x,y,z-axes |
66-
| [`Modia3D.qrot_e`](@ref)(e, angle) | Rotate around `angle` along unit vector `e`|
67-
| [`Modia3D.qrot_nxy`](@ref)(nx, ny) | `nx`/`ny` are in x/y-direction of frame 2 |
68-
| [`Modia3D.from_R`](@ref)(R) | Return `q` from SMatrix{3,3,F,9} `R` |
69-
70-
71-
# Operations on Frames
72-
73-
The following functions provide operations on frames. The orientation of a frame is
74-
defined with argument `Rq` meaning it can be either a
75-
rotation matrix ` R` or a
76-
quaternion ` q` (to rotate a frame 1 into a frame 2).
77-
78-
| Function | Description |
79-
|:-----------------------------------------------|:----------------------------------------------|
80-
| [`Modia3D.resolve1`](@ref)(Rq, v2) | Transform vector `v` from frame 2 to frame 1 |
81-
| [`Modia3D.resolve2`](@ref)(Rq, v1) | Transform vector `v` from frame 1 to frame 2 |
82-
| [`Modia3D.absoluteRotation`](@ref)(Rq01, Rq12) | Return rotation 0->2 from rot. 0->1 and 1->2 |
83-
| [`Modia3D.relativeRotation`](@ref)(Rq01, Rq02) | Return rotation 1->2 from rot. 0->1 and 0->2 |
84-
| [`Modia3D.inverseRotation`](@ref)(Rq01) | Return rotation 1->0 from rot, 0->1 |
85-
| [`Modia3D.planarRotationAngle`](@ref)(e,v1,v2) | Return angle of planar rotation along `e` |
86-
| [`Modia3D.eAxis`](@ref)(axis) | Return unit vector `e` in direction of `axis` |
87-
| [`Modia3D.skew`](@ref)(v) | Return skew-symmetric matrix of vector v |
88-
89-
90-
# Interpolation of Frames
91-
92-
Given a set of frames by a vector `r` of position vectors to their origins and
93-
and an optional vector `q` of Quaternions of their absolute orientations, then
94-
the following functions interpolate linearly in these frames:
95-
96-
| Function | Description |
97-
|:------------------------------------------------|:-----------------------------------------------|
98-
| [`Modia3D.Path`](@ref)(r,q) | Return path defined by a vector of frames |
99-
| [`Modia3D.t_pathEnd`](@ref)(path) | Return path parameter `t_end` of last frame |
100-
| [`Modia3D.interpolate`](@ref)(path,t) | Return `(rt,qt)` of Path at path parameter `t` |
101-
| [`Modia3D.interpolate_r`](@ref)(path,t) | Return `rt` of Path at path parameter `t` |
102-
103-
104-
# Examples
105-
106-
```julia
107-
using Modia3D
108-
using Unitful
109-
110-
# R1,R2,R3 are the same RotationMatrices
111-
R1 = Modia3D.rot1(pi/2)
112-
R2 = Modia3D.rot1(90u"°")
113-
R3 = Modia3D.rot_e([1,0,0], pi/2)
114-
```
9+
or with a **quaternion vector** and its origin is described with a **SVector{3,Float64}**.
10+
For details see Modia3D/docs/Functions.md.
11511
11612
# Main developer
11713

‎src/Frames/quaternion.jl‎

Lines changed: 10 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ q = [e*sin(angle/2),
2525
2626
Constant quaternion vector of a null rotation (= no rotation from frame 1 to frame 2)
2727
"""
28-
NullQuaternion(::Type{F}) where F <: Modia3D.VarFloatType = SVector{4,F}(0.0, 0.0, 0.0, 1.0)
28+
NullQuaternion(::Type{F}) where F <: Modia3D.VarFloatType = SVector{4,F}(0, 0, 0, 1)
2929

3030

3131

@@ -46,21 +46,17 @@ end
4646
4747
Return SMatrix{4,4,F,16} `R` from quaternion `q`.
4848
"""
49-
from_q(q::SVector{4,F}) where F <: Modia3D.VarFloatType = SMatrix{3,3,F,9}( 2.0 * (q[1] * q[1] + q[4] * q[4]) - 1.0,
50-
2.0 * (q[2] * q[1] - q[3] * q[4]),
51-
2.0 * (q[3] * q[1] + q[2] * q[4]),
52-
2.0 * (q[1] * q[2] + q[3] * q[4]),
53-
2.0 * (q[2] * q[2] + q[4] * q[4]) - 1.0,
54-
2.0 * (q[3] * q[2] - q[1] * q[4]),
55-
2.0 * (q[1] * q[3] - q[2] * q[4]),
56-
2.0 * (q[2] * q[3] + q[1] * q[4]),
57-
2.0 * (q[3] * q[3] + q[4] * q[4]) - 1.0)
49+
from_q(q::SVector{4,F}) where F <: Modia3D.VarFloatType = SMatrix{3,3,F,9}( F(2.0) * (q[1] * q[1] + q[4] * q[4]) - F(1.0),
50+
F(2.0) * (q[2] * q[1] - q[3] * q[4]),
51+
F(2.0) * (q[3] * q[1] + q[2] * q[4]),
52+
F(2.0) * (q[1] * q[2] + q[3] * q[4]),
53+
F(2.0) * (q[2] * q[2] + q[4] * q[4]) - F(1.0),
54+
F(2.0) * (q[3] * q[2] - q[1] * q[4]),
55+
F(2.0) * (q[1] * q[3] - q[2] * q[4]),
56+
F(2.0) * (q[2] * q[3] + q[1] * q[4]),
57+
F(2.0) * (q[3] * q[3] + q[4] * q[4]) - F(1.0) )
5858

5959

60-
61-
const p4limit = 0.1
62-
const c4limit = 4.0 * p4limit * p4limit
63-
6460
"""
6561
q = Modia3D.from_R(R::SMatrix{3,3,F,9};
6662
q_guess = NullQuaternion(F))

0 commit comments

Comments
 (0)