Skip to content

Commit d29f1ce

Browse files
edxmorganedxmorgan
committed
ned dynamic equivalent added
1 parent b45b6d0 commit d29f1ce

File tree

10 files changed

+186
-35
lines changed

10 files changed

+186
-35
lines changed

‎diffUV/base.py

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -3,8 +3,8 @@
33
from casadi import SX, horzcat, inv, sin,cos, fabs, Function, diag, pinv,substitute
44
from platform import machine, system
55
import diffUV.utils.operators as ops
6-
import diffUV.utils.transformation_matrix as T
7-
from diffUV.utils.operators import Smtrx, coriolis_lag_param
6+
import diffUV.utils.euler_ops as T
7+
from diffUV.utils.operators import cross_pO, coriolis_lag_param
88
from diffUV.utils.symbol import *
99

1010

@@ -30,7 +30,7 @@ def __repr__(self) -> str:
3030
def _initialize_inertia_matrix(self):
3131
"""Internal method to compute the UV inertia matrix based on vehicle parameters."""
3232
M_rb = SX(6,6)
33-
S = Smtrx(r_g)
33+
S = cross_pO(r_g)
3434
M_rb[:3,:3] = m*SX.eye(3)
3535
M_rb[:3,3:] = -m*S
3636
M_rb[3:,:3] = m*S

‎diffUV/dynamics_euler.py

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,16 @@
11
from casadi import inv
22
from diffUV.base import Base
33
from diffUV.utils.symbol import *
4-
import diffUV.utils.transformation_matrix as T_eul
4+
import diffUV.utils.euler_ops as T_eul
55

66
class DynamicsEuler(Base):
77
def __init__(self):
88
super().__init__()
9-
self.J_INV, _,_ = T_eul.inv_J_kin(phi, thet, psi)
9+
self.J, R, T = T_eul.J_kin(eul)
10+
self.J_INV, _,_ = T_eul.inv_J_kin(eul)
1011
self.J_INV_T = self.J_INV.T
1112
self.state_vector = vertcat(n,dn)
13+
self.J_dot, _, _ = T_eul.J_dot(eul,deul,dT_sp,eul_sp,w_nb)
1214

1315
def __repr__(self) -> str:
1416
"""Euler representation of the Dynamics instance in ned frame"""

‎diffUV/dynamics_quat.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
from casadi import inv
22
from diffUV.base import Base
33
from diffUV.utils.symbol import *
4-
import diffUV.utils.dual_quaternion as Tquat
4+
import diffUV.utils.quaternion_ops as Tquat
55

66
class DynamicsQuat(Base):
77
def __init__(self):

‎diffUV/kinematics.py

Lines changed: 7 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,18 +1,15 @@
11
"""This module contains a class for implementing fossen_thor_i_handbook_of_marine_craft_hydrodynamics_and_motion_control
22
"""
33
from diffUV.base import Base
4-
from diffUV.utils import transformation_matrix as Tm
4+
from diffUV.utils import euler_ops as T_eul
5+
from diffUV.utils import quaternion_ops as T_quat
56
from diffUV.utils.symbol import *
67

78
class Kinematics(Base):
89
def __init__(self):
9-
self.J, self.R, self.T = Tm.J_kin(phi, thet, psi)
10-
self.J_inv, self.R_inv, self.T_inv = Tm.inv_J_kin(phi, thet, psi)
11-
self.dR = Tm.T_diff(self.R, v_nb)
12-
self.dT = Tm.T_diff(self.T, w_nb)
13-
self.dJ = SX.zeros(6, 6)
14-
self.dJ[:3,:3] = self.dR
15-
self.dJ[3:,3:] = self.dT
10+
self.J, self.R, self.T = T_eul.J_kin(phi, thet, psi)
11+
self.J_inv, self.R_inv, self.T_inv = T_eul.inv_J_kin(phi, thet, psi)
12+
self.J_dot, _, _ = T_eul.J_dot(eul,deul, w_nb)
1613

1714
def __repr__(self) -> str:
1815
return f'{super().__repr__()} Kinematics'
@@ -22,13 +19,13 @@ def ned_vel(self):
2219
return _dn
2320

2421
def ned_acc(self):
25-
_ddn = self.J@dx_nb + self.dJ@x_nb
22+
_ddn = self.J@dx_nb + self.J_dot@x_nb
2623
return _ddn
2724

2825
def body_position(self):
2926
v = self.J_inv@dn
3027
return v
3128

3229
def body_vel(self):
33-
dv = self.J_inv@(ddn - self.dJ@self.J_inv@dn)
30+
dv = self.J_inv@(ddn - self.J_dot@self.J_inv@dn)
3431
return dv

‎diffUV/utils/transformation_matrix.py renamed to ‎diffUV/utils/euler_ops.py

Lines changed: 19 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
1-
from casadi import cos, SX, sin, tan, skew, inv
1+
from casadi import cos, SX, sin, tan, inv, vertcat
2+
from diffUV.utils.operators import rot_diff, cross_pO, sympy2casadi
23

34
def linear_vel_R(phi, thet, psi):
45
R = SX(3, 3)
@@ -37,21 +38,30 @@ def inv_angular_vel_T(phi, thet):
3738
T_1[2,2] = cos(thet)*cos(phi)
3839
return T_1
3940

40-
def T_diff(R_n,w_b):
41-
S = skew(w_b)
42-
dR_n = R_n@S
43-
return dR_n
44-
45-
46-
def J_kin(phi, thet, psi):
41+
def J_kin(eul):
42+
phi, thet, psi = eul[0],eul[1],eul[2]
4743
R = linear_vel_R(phi, thet, psi)
4844
T = angular_vel_T(phi, thet)
4945
J = SX.zeros(6, 6)
5046
J[:3,:3] = R
5147
J[3:,3:] = T
5248
return J,R,T
5349

54-
def inv_J_kin(phi, thet, psi):
50+
def J_dot(eul, deul,dT, eul_sp, w_nb):
51+
phi, thet, _ = eul[0],eul[1],eul[2]
52+
dthet, dphi, _ = deul[0],deul[1],deul[2]
53+
theta_sp, dtheta_sp, phi_sp, dphi_sp = eul_sp[0], eul_sp[1], eul_sp[2], eul_sp[3]
54+
_,R,T = J_kin(eul)
55+
dR = rot_diff(R, w_nb)
56+
dT = sympy2casadi(dT, [theta_sp, dtheta_sp, phi_sp, dphi_sp], vertcat(thet,dthet,phi,dphi))
57+
dJ = SX.zeros(6, 6)
58+
dJ[:3,:3] = dR
59+
dJ[3:,3:] = dT
60+
return dJ, dR, dT
61+
62+
63+
def inv_J_kin(eul):
64+
phi, thet, psi = eul[0],eul[1],eul[2]
5565
RT = inv_linear_vel_R(phi, thet, psi)
5666
inv_T = inv_angular_vel_T(phi, thet)
5767
inv_J = SX.zeros(6, 6)

‎diffUV/utils/operators.py

Lines changed: 26 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
from casadi import skew, SX
2+
import casadi
23

3-
def Smtrx(v):
4+
def cross_pO(v):
45
S = skew(v)
56
return S
67

@@ -14,7 +15,27 @@ def coriolis_lag_param(M, x_nb):
1415
M21 = M[3:, :3]
1516
M22 = M[3:, 3:]
1617
C = SX.zeros(6, 6)
17-
C[3:, :3] = -Smtrx(M11@v_nb + M12@w_nb)
18-
C[:3, 3:] = -Smtrx(M11@v_nb + M12@w_nb)
19-
C[3:, 3:] = -Smtrx(M21@v_nb + M22@w_nb)
20-
return C
18+
C[3:, :3] = -cross_pO(M11@v_nb + M12@w_nb)
19+
C[:3, 3:] = -cross_pO(M11@v_nb + M12@w_nb)
20+
C[3:, 3:] = -cross_pO(M21@v_nb + M22@w_nb)
21+
return C
22+
23+
def rot_diff(R_n, w_b):
24+
S = cross_pO(w_b)
25+
dR_n = R_n@S
26+
return dR_n
27+
28+
def sympy2casadi(sympy_expr,sympy_var,casadi_var):
29+
assert casadi_var.is_vector()
30+
if casadi_var.shape[1]>1:
31+
casadi_var = casadi_var.T
32+
casadi_var = casadi.vertsplit(casadi_var)
33+
from sympy.utilities.lambdify import lambdify
34+
35+
mapping = {'ImmutableDenseMatrix': casadi.blockcat,
36+
'MutableDenseMatrix': casadi.blockcat,
37+
'Abs':casadi.fabs
38+
}
39+
f = lambdify(sympy_var, sympy_expr,modules=[mapping, casadi])
40+
# print(casadi_var)
41+
return f(*casadi_var)

‎diffUV/utils/dual_quaternion.py renamed to ‎diffUV/utils/quaternion_ops.py

File renamed without changes.

‎diffUV/utils/symbol.py

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,6 @@
11
from casadi import SX, vertcat, DM, horzcat
2+
import sympy as sp
3+
from sympy.physics.mechanics import dynamicsymbols
24

35
# 6 DOF states vectors in body-fixed
46
u = SX.sym('u')
@@ -40,6 +42,20 @@
4042
ddp_n = vertcat(ddx, ddy, ddz)
4143

4244

45+
theta_sp, phi_sp = dynamicsymbols('theta phi')
46+
dtheta_sp = sp.diff(theta_sp,'t')
47+
dphi_sp =sp.diff(phi_sp,'t')
48+
49+
eul_sp = [theta_sp, dtheta_sp, phi_sp, dphi_sp]
50+
# Create the sympy matrix T
51+
T_sp = sp.Matrix([
52+
[1, sp.sin(phi_sp)*sp.tan(theta_sp), sp.cos(phi_sp)*sp.tan(theta_sp)],
53+
[0, sp.cos(phi_sp), -sp.sin(phi_sp)],
54+
[0, sp.sin(phi_sp)/sp.cos(theta_sp), sp.cos(phi_sp)/sp.cos(theta_sp)]
55+
])
56+
57+
dT_sp = sp.diff(T_sp,'t',1)
58+
4359
thet = SX.sym('thet')
4460
dthet = SX.sym('dthet')
4561
ddthet = SX.sym('ddthet')
@@ -61,6 +77,12 @@
6177
eta,eps1,eps2,eps3 = SX.sym('eta'),SX.sym('eps1'),SX.sym('eps2'),SX.sym('eps3')
6278
uq = vertcat(eta,eps1,eps2,eps3) #unit quaternion
6379

80+
deta = -0.5*(eps1*p + eps2*q + eps3*r)
81+
deps1 = 0.5*(eta*p - eps3*q + eps2*r)
82+
deps2 = 0.5*(eps3*p + eta*q - eps1*r)
83+
deps3 = 0.5*(-eps2*p + eps1*q - eta*r)
84+
duq = vertcat(deta,deps1,deps2,deps3) # differential unit quaternion
85+
6486
nq = vertcat(p_n, uq)
6587
###################################################
6688

‎examples/lab.ipynb

Lines changed: 103 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -2,22 +2,121 @@
22
"cells": [
33
{
44
"cell_type": "code",
5-
"execution_count": 10,
5+
"execution_count": 1,
6+
"metadata": {},
7+
"outputs": [],
8+
"source": [
9+
"from casadi import vertcat,SX\n",
10+
"import sympy as sp\n",
11+
"from sympy.physics.mechanics import dynamicsymbols"
12+
]
13+
},
14+
{
15+
"cell_type": "code",
16+
"execution_count": 2,
17+
"metadata": {},
18+
"outputs": [],
19+
"source": [
20+
"def sympy2casadi(sympy_expr,sympy_var,casadi_var):\n",
21+
" import casadi\n",
22+
" assert casadi_var.is_vector()\n",
23+
" if casadi_var.shape[1]>1:\n",
24+
" casadi_var = casadi_var.T\n",
25+
" casadi_var = casadi.vertsplit(casadi_var)\n",
26+
" from sympy.utilities.lambdify import lambdify\n",
27+
"\n",
28+
" mapping = {'ImmutableDenseMatrix': casadi.blockcat,\n",
29+
" 'MutableDenseMatrix': casadi.blockcat,\n",
30+
" 'Abs':casadi.fabs\n",
31+
" }\n",
32+
" f = lambdify(sympy_var, sympy_expr,modules=[mapping, casadi])\n",
33+
" print(casadi_var)\n",
34+
" return f(*casadi_var)"
35+
]
36+
},
37+
{
38+
"cell_type": "code",
39+
"execution_count": 3,
40+
"metadata": {},
41+
"outputs": [
42+
{
43+
"data": {
44+
"text/latex": [
45+
"$\\displaystyle \\left[\\begin{matrix}0 & \\left(\\tan^{2}{\\left(\\theta{\\left(t \\right)} \\right)} + 1\\right) \\sin{\\left(\\phi{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta{\\left(t \\right)} + \\cos{\\left(\\phi{\\left(t \\right)} \\right)} \\tan{\\left(\\theta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\phi{\\left(t \\right)} & \\left(\\tan^{2}{\\left(\\theta{\\left(t \\right)} \\right)} + 1\\right) \\cos{\\left(\\phi{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta{\\left(t \\right)} - \\sin{\\left(\\phi{\\left(t \\right)} \\right)} \\tan{\\left(\\theta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\phi{\\left(t \\right)}\\\\0 & - \\sin{\\left(\\phi{\\left(t \\right)} \\right)} \\frac{d}{d t} \\phi{\\left(t \\right)} & - \\cos{\\left(\\phi{\\left(t \\right)} \\right)} \\frac{d}{d t} \\phi{\\left(t \\right)}\\\\0 & \\frac{\\sin{\\left(\\phi{\\left(t \\right)} \\right)} \\sin{\\left(\\theta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta{\\left(t \\right)}}{\\cos^{2}{\\left(\\theta{\\left(t \\right)} \\right)}} + \\frac{\\cos{\\left(\\phi{\\left(t \\right)} \\right)} \\frac{d}{d t} \\phi{\\left(t \\right)}}{\\cos{\\left(\\theta{\\left(t \\right)} \\right)}} & - \\frac{\\sin{\\left(\\phi{\\left(t \\right)} \\right)} \\frac{d}{d t} \\phi{\\left(t \\right)}}{\\cos{\\left(\\theta{\\left(t \\right)} \\right)}} + \\frac{\\sin{\\left(\\theta{\\left(t \\right)} \\right)} \\cos{\\left(\\phi{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta{\\left(t \\right)}}{\\cos^{2}{\\left(\\theta{\\left(t \\right)} \\right)}}\\end{matrix}\\right]$"
46+
],
47+
"text/plain": [
48+
"Matrix([\n",
49+
"[0, (tan(theta(t))**2 + 1)*sin(phi(t))*Derivative(theta(t), t) + cos(phi(t))*tan(theta(t))*Derivative(phi(t), t), (tan(theta(t))**2 + 1)*cos(phi(t))*Derivative(theta(t), t) - sin(phi(t))*tan(theta(t))*Derivative(phi(t), t)],\n",
50+
"[0, -sin(phi(t))*Derivative(phi(t), t), -cos(phi(t))*Derivative(phi(t), t)],\n",
51+
"[0, sin(phi(t))*sin(theta(t))*Derivative(theta(t), t)/cos(theta(t))**2 + cos(phi(t))*Derivative(phi(t), t)/cos(theta(t)), -sin(phi(t))*Derivative(phi(t), t)/cos(theta(t)) + sin(theta(t))*cos(phi(t))*Derivative(theta(t), t)/cos(theta(t))**2]])"
52+
]
53+
},
54+
"execution_count": 3,
55+
"metadata": {},
56+
"output_type": "execute_result"
57+
}
58+
],
59+
"source": [
60+
"# Define the symbols\n",
61+
"theta_sp, phi_sp = dynamicsymbols('theta phi')\n",
62+
"# Create the matrix T\n",
63+
"T = sp.Matrix([\n",
64+
" [1, sp.sin(phi_sp)*sp.tan(theta_sp), sp.cos(phi_sp)*sp.tan(theta_sp)],\n",
65+
" [0, sp.cos(phi_sp), -sp.sin(phi_sp)],\n",
66+
" [0, sp.sin(phi_sp)/sp.cos(theta_sp), sp.cos(phi_sp)/sp.cos(theta_sp)]\n",
67+
"])\n",
68+
"\n",
69+
"\n",
70+
"dtheta_sp = sp.diff(theta_sp,'t')\n",
71+
"dphi_sp =sp.diff(phi_sp,'t')\n",
72+
"\n",
73+
"dT_sp = sp.diff(T,'t',1)\n",
74+
"\n",
75+
"dT_sp"
76+
]
77+
},
78+
{
79+
"cell_type": "code",
80+
"execution_count": 5,
681
"metadata": {},
782
"outputs": [
883
{
984
"name": "stdout",
1085
"output_type": "stream",
1186
"text": [
12-
"/Users/edwardmorgan/dev_ws/src/Diff_UV/.venv/bin/python\n"
87+
"[SX(thet), SX(dthet), SX(phi), SX(dphi)]\n"
1388
]
89+
},
90+
{
91+
"data": {
92+
"text/plain": [
93+
"SX(@1=0, @2=1, \n",
94+
"[[@1, (((dthet*(sq(tan(thet))+@2))*sin(phi))+((dphi*cos(phi))*tan(thet))), (((dthet*(sq(tan(thet))+@2))*cos(phi))-((dphi*sin(phi))*tan(thet)))], \n",
95+
" [@1, (-(dphi*sin(phi))), (-(dphi*cos(phi)))], \n",
96+
" [@1, ((((dthet*sin(thet))*sin(phi))/sq(cos(thet)))+((dphi*cos(phi))/cos(thet))), ((((dthet*sin(thet))*cos(phi))/sq(cos(thet)))-((dphi*sin(phi))/cos(thet)))]])"
97+
]
98+
},
99+
"execution_count": 5,
100+
"metadata": {},
101+
"output_type": "execute_result"
14102
}
15103
],
16104
"source": [
17-
"import sys\n",
18-
"print(sys.executable)"
105+
"theta = SX.sym(\"thet\")\n",
106+
"dtheta = SX.sym(\"dthet\")\n",
107+
"phi = SX.sym(\"phi\")\n",
108+
"dphi = SX.sym(\"dphi\")\n",
109+
"dT = sympy2casadi(dT_sp, [theta_sp, dtheta_sp, phi_sp, dphi_sp], vertcat(theta,dtheta,phi,dphi))\n",
110+
"dT"
19111
]
20112
},
113+
{
114+
"cell_type": "code",
115+
"execution_count": null,
116+
"metadata": {},
117+
"outputs": [],
118+
"source": []
119+
},
21120
{
22121
"cell_type": "code",
23122
"execution_count": null,

‎setup.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@
2929
# For a discussion on single-sourcing the version across setup.py and the
3030
# project code, see
3131
# https://packaging.python.org/guides/single-sourcing-package-version/
32-
version="0.1.3", # Required
32+
version="0.1.4", # Required
3333
# This is a one-line description or tagline of what your project does. This
3434
# corresponds to the "Summary" metadata field:
3535
# https://packaging.python.org/specifications/core-metadata/#summary

0 commit comments

Comments
 (0)