-
-
Notifications
You must be signed in to change notification settings - Fork 68
/
rigid.clj
98 lines (87 loc) · 2.96 KB
/
rigid.clj
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
;
; Copyright © 2017 Colin Smith.
; This work is based on the Scmutils system of MIT/GNU Scheme:
; Copyright © 2002 Massachusetts Institute of Technology
;
; This is free software; you can redistribute it and/or modify
; it under the terms of the GNU General Public License as published by
; the Free Software Foundation; either version 3 of the License, or (at
; your option) any later version.
;
; This software is distributed in the hope that it will be useful, but
; WITHOUT ANY WARRANTY; without even the implied warranty of
; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
; General Public License for more details.
;
; You should have received a copy of the GNU General Public License
; along with this code; if not, see <http://www.gnu.org/licenses/>.
;
(ns sicmutils.mechanics.rigid
(:refer-clojure :exclude [+ - * / zero?])
(:require [sicmutils
[generic :refer :all]
[structure :refer :all]
[matrix :as matrix]
[function :refer :all]]
[sicmutils.calculus.derivative :refer :all]
[sicmutils.mechanics
[lagrange :refer :all]
[rotation :refer :all]]))
(defn antisymmetric->column-matrix
"Given an antisymmetric matrix-structure of dimension 3,
return the column vector of its positive components."
[a]
;; XXX: we should assert anstisymmetricity here
(matrix/column (matrix/get-in a [2 1])
(matrix/get-in a [0 2])
(matrix/get-in a [1 0])))
(defn M-of-q->omega-of-t
[M-of-q]
(fn [q]
(let [M-on-path (compose M-of-q q)]
(fn [t]
(let [omega-cross (fn [t]
(* ((D M-on-path) t)
(transpose (M-on-path t))))]
(antisymmetric->column-matrix (omega-cross t)))))))
(defn M-of-q->omega-body-of-t
[M-of-q]
(fn [q]
(fn [t]
(* (transpose (M-of-q (q t)))
(((M-of-q->omega-of-t M-of-q) q) t)))))
(defn M->omega
[M-of-q]
(-> M-of-q M-of-q->omega-of-t Γ-bar))
(defn M->omega-body
[M-of-q]
(-> M-of-q M-of-q->omega-body-of-t Γ-bar))
(defn Euler-state->omega-body
[[_ [θ _ ψ] [θdot φdot ψdot]]]
(let [ω-a (+ (* (sin ψ) (sin θ) φdot) (* (cos ψ) θdot))
ω-b (+ (* (cos ψ) (sin θ) φdot) (* -1 (sin ψ) θdot))
ω-c (+ (* (cos θ) φdot) ψdot)]
(up ω-a ω-b ω-c)))
(defn T-rigid-body
[A B C]
(fn [local]
(let [[ω0 ω1 ω2] (Euler-state->omega-body local)]
(* 1/2
(+ (* A (square ω0))
(* B (square ω1))
(* C (square ω2)))))))
(defn rigid-sysder
[A B C]
(Lagrangian->state-derivative (T-rigid-body A B C)))
(defn Euler-state->L-body
[A B C]
(fn [local]
(let [[ω0 ω1 ω2] (Euler-state->omega-body local)]
(matrix/column (* A ω0)
(* B ω1)
(* C ω2)))))
(defn Euler-state->L-space
[A B C]
(fn [[_ angles _ :as local]]
(* (Euler->M angles)
((Euler-state->L-body A B C) local))))