Skip to content

Commit bb482b2

Browse files
petercorkepetercorke
committed
Add exponential coordinates to angvelxform and angvelxform_dot
Update notebooks to generate the transformations Add tests
1 parent bc6dcb3 commit bb482b2

4 files changed

Lines changed: 512 additions & 53 deletions

File tree

‎spatialmath/base/transforms3d.py‎

Lines changed: 124 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -1768,6 +1768,9 @@ def exp2jac(v):
17681768
exponential coordinate
17691769
Guillermo Gallego, Anthony Yezzi
17701770
https://arxiv.org/pdf/1312.0788v1.pdf
1771+
- Robot Dynamics Lecture Notes
1772+
Robotic Systems Lab, ETH Zurich, 2018
1773+
https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2018/RD_HS2018script.pdf
17711774
17721775
:SymPy: supported
17731776
@@ -1779,15 +1782,24 @@ def exp2jac(v):
17791782
if theta is None:
17801783
return np.eye(3)
17811784

1782-
R = trexp(v)
1785+
# R = trexp(v)
1786+
# z = np.eye(3,3) - R
1787+
# # build the derivative columnwise
1788+
# A = []
1789+
# for i in range(3):
1790+
# # (III.7)
1791+
# dRdvi = vn[i] * base.skew(vn) + base.skew(np.cross(vn, z[:,i])) / theta
1792+
# x = base.vex(dRdvi)
1793+
# A.append(x)
1794+
# return np.c_[A].T
1795+
1796+
# from ETH paper
1797+
theta = base.norm(v)
1798+
sk = base.skew(v)
17831799

1784-
z = np.eye(3,3) - R
1785-
A = []
1786-
for i in range(3):
1787-
dRdvi = vn[i] * base.skew(vn) + base.skew(np.cross(vn, z[:,i])) / theta
1788-
x = base.vex(dRdvi)
1789-
A.append(x)
1790-
return np.c_[A].T
1800+
# (2.106)
1801+
E = np.eye(3) + sk * (1 - np.cos(theta)) / theta ** 2 + sk @ sk * (theta - np.sin(theta)) / theta ** 3
1802+
return E
17911803

17921804
def rot2jac(R, representation='rpy-xyz'):
17931805
"""
@@ -1841,8 +1853,8 @@ def rot2jac(R, representation='rpy-xyz'):
18411853
eul = tr2eul(R)
18421854
A = eul2jac(eul)
18431855
elif representation == 'exp':
1844-
(theta, v) = trlog(R)
1845-
A = exp2jac(v, theta)
1856+
v = trlog(R, twist=True)
1857+
A = exp2jac(v)
18461858
else:
18471859
raise ValueError('bad representation specified')
18481860

@@ -1864,10 +1876,6 @@ def angvelxform(𝚪, inverse=False, full=True, representation='rpy/xyz'):
18641876
:return: angular velocity transformation matrix
18651877
:rtype: ndarray(6,6) or ndarray(3,3)
18661878
1867-
Computes the transformation from spatial velocity, where rotation rate is
1868-
expressed as angular velocity, to analytical rates where the rotational part
1869-
is expressed as rate of change in some other representation
1870-
18711879
Computes the transformation from spatial velocity :math:`\nu`, where
18721880
rotation rate is expressed as angular velocity, to analytical rates
18731881
:math:`\dvec{x}` where the rotational part is expressed as rate of change in
@@ -1891,40 +1899,50 @@ def angvelxform(𝚪, inverse=False, full=True, representation='rpy/xyz'):
18911899
- This performs the inverse mapping
18921900
- This maps a 6-vector, the others map a 3-vector
18931901
1894-
https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2016/RD2016script.pdf
1902+
Reference:
1903+
1904+
- ``symbolic/angvelxform.ipynb`` in this Toolbox
1905+
- Robot Dynamics Lecture Notes
1906+
Robotic Systems Lab, ETH Zurich, 2018
1907+
https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2018/RD_HS2018script.pdf
1908+
18951909
:seealso: :func:`rot2jac`, :func:`eul2jac`, :func:`rpy2r`, :func:`exp2jac`
18961910
"""
18971911

18981912
if representation == 'rpy/xyz':
18991913
alpha = 𝚪[0]
19001914
beta = 𝚪[1]
19011915
gamma = 𝚪[2]
1916+
# autogenerated by symbolic/angvelxform.ipynb
19021917
if inverse:
1918+
# analytical rates -> angular velocity
19031919
A = np.array([
19041920
[math.sin(beta), 0, 1],
19051921
[-math.sin(gamma)*math.cos(beta), math.cos(gamma), 0],
19061922
[math.cos(beta)*math.cos(gamma), math.sin(gamma), 0]
19071923
])
19081924
else:
1925+
# angular velocity -> analytical rates
19091926
A = np.array([
19101927
[0, -math.sin(gamma)/math.cos(beta), math.cos(gamma)/math.cos(beta)],
19111928
[0, math.cos(gamma), math.sin(gamma)],
19121929
[1, math.sin(gamma)*math.tan(beta), -math.cos(gamma)*math.tan(beta)]
19131930
])
19141931

1915-
19161932
elif representation == 'rpy/zyx':
19171933
alpha = 𝚪[0]
19181934
beta = 𝚪[1]
19191935
gamma = 𝚪[2]
1920-
1936+
# autogenerated by symbolic/angvelxform.ipynb
19211937
if inverse:
1938+
# analytical rates -> angular velocity
19221939
A = np.array([
19231940
[math.cos(beta)*math.cos(gamma), -math.sin(gamma), 0],
19241941
[math.sin(gamma)*math.cos(beta), math.cos(gamma), 0],
19251942
[-math.sin(beta), 0, 1]
19261943
])
19271944
else:
1945+
# angular velocity -> analytical rates
19281946
A = np.array([
19291947
[math.cos(gamma)/math.cos(beta), math.sin(gamma)/math.cos(beta), 0],
19301948
[-math.sin(gamma), math.cos(gamma), 0],
@@ -1935,21 +1953,36 @@ def angvelxform(𝚪, inverse=False, full=True, representation='rpy/xyz'):
19351953
phi = 𝚪[0]
19361954
theta = 𝚪[1]
19371955
psi = 𝚪[2]
1938-
1956+
# autogenerated by symbolic/angvelxform.ipynb
19391957
if inverse:
1958+
# analytical rates -> angular velocity
19401959
A = np.array([
19411960
[0, -math.sin(phi), math.sin(theta)*math.cos(phi)],
19421961
[0, math.cos(phi), math.sin(phi)*math.sin(theta)],
19431962
[1, 0, math.cos(theta)]
19441963
])
19451964
else:
1965+
# angular velocity -> analytical rates
19461966
A = np.array([
19471967
[-math.cos(phi)/math.tan(theta), -math.sin(phi)/math.tan(theta), 1],
19481968
[-math.sin(phi), math.cos(phi), 0],
19491969
[math.cos(phi)/math.sin(theta), math.sin(phi)/math.sin(theta), 0]
19501970
])
19511971
elif representation == 'exp':
1952-
raise UserWarning('not implemented yet')
1972+
# from ETHZ class notes
1973+
sk = base.skew(𝚪)
1974+
theta = base.norm(𝚪)
1975+
if inverse:
1976+
# analytical rates -> angular velocity
1977+
# (2.106)
1978+
A = np.eye(3) + sk * (1 - np.cos(theta)) / theta ** 2 \
1979+
+ sk @ sk * (theta - np.sin(theta)) / theta ** 3
1980+
else:
1981+
# angular velocity -> analytical rates
1982+
# (2.107)
1983+
A = np.eye(3) - sk / 2 + sk @ sk / theta ** 2 * (
1984+
1 - (theta / 2) * (np.sin(theta) / (1 - np.cos(theta)))
1985+
)
19531986
else:
19541987
raise ValueError('bad representation specified')
19551988

@@ -1973,14 +2006,9 @@ def angvelxform_dot(𝚪, 𝚪d, full=True, representation='rpy/xyz'):
19732006
:return: angular velocity transformation matrix
19742007
:rtype: ndarray(6,6) or ndarray(3,3)
19752008
1976-
Computes the transformation from spatial velocity, where rotation rate is
1977-
expressed as angular velocity, to analytical rates where the rotational part
1978-
is expressed as rate of change in some other representation
1979-
1980-
Computes the transformation from spatial velocity :math:`\nu`, where
1981-
rotation rate is expressed as angular velocity, to analytical rates
1982-
:math:`\dvec{x}` where the rotational part is expressed as rate of change in
1983-
some other representation
2009+
Computes the transformation from spatial acceleration :math:`\dot{\nu}`, where the rotational part is
2010+
expressed as angular acceleration, to analytical rates :math:`\ddvec{x}` where the rotational part
2011+
is expressed as acceleration in some other representation
19842012
19852013
.. math::
19862014
\ddvec{x} = \mat{A}_d \dvec{\nu}
@@ -2000,58 +2028,110 @@ def angvelxform_dot(𝚪, 𝚪d, full=True, representation='rpy/xyz'):
20002028
- This performs the inverse mapping
20012029
- This maps a 6-vector, the others map a 3-vector
20022030
2003-
https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2016/RD2016script.pdf
2031+
Reference:
2032+
2033+
- ``symbolic/angvelxform.ipynb`` in this Toolbox
2034+
- ``symbolic/angvelxform_dot.ipynb`` in this Toolbox
2035+
20042036
:seealso: :func:`rot2jac`, :func:`eul2jac`, :func:`rpy2r`, :func:`exp2jac`
20052037
"""
20062038

20072039
if representation == 'rpy/xyz':
2040+
# autogenerated by symbolic/angvelxform.ipynb
20082041
alpha = 𝚪[0]
20092042
beta = 𝚪[1]
20102043
gamma = 𝚪[2]
20112044
alpha_dot = 𝚪d[0]
20122045
beta_dot = 𝚪d[1]
20132046
gamma_dot = 𝚪d[2]
20142047
Ad = np.array([
2015-
[0, -(beta_dot*math.sin(beta)*math.sin(gamma)/math.cos(beta) + gamma_dot*math.cos(gamma))/math.cos(beta), (beta_dot*math.sin(beta)*math.cos(gamma)/math.cos(beta) - gamma_dot*math.sin(gamma))/math.cos(beta)],
2016-
[0, -gamma_dot*math.sin(gamma), gamma_dot*math.cos(gamma)],
2017-
[0, beta_dot*math.sin(gamma)/math.cos(beta)**2 + gamma_dot*math.cos(gamma)*math.tan(beta), -beta_dot*math.cos(gamma)/math.cos(beta)**2 + gamma_dot*math.sin(gamma)*math.tan(beta)]
2018-
])
2048+
[ 0,
2049+
-(beta_dot*math.sin(beta)*math.sin(gamma)/math.cos(beta) \
2050+
+ gamma_dot*math.cos(gamma))/math.cos(beta), \
2051+
(beta_dot*math.sin(beta)*math.cos(gamma)/math.cos(beta) - \
2052+
gamma_dot*math.sin(gamma))/math.cos(beta)
2053+
],
2054+
[ 0, -gamma_dot*math.sin(gamma), gamma_dot*math.cos(gamma)],
2055+
[ 0,
2056+
beta_dot*math.sin(gamma)/math.cos(beta)**2 \
2057+
+ gamma_dot*math.cos(gamma)*math.tan(beta),
2058+
-beta_dot*math.cos(gamma)/math.cos(beta)**2 \
2059+
+ gamma_dot*math.sin(gamma)*math.tan(beta)
2060+
]
2061+
])
20192062

20202063
elif representation == 'rpy/zyx':
2064+
# autogenerated by symbolic/angvelxform.ipynb
20212065
alpha = 𝚪[0]
20222066
beta = 𝚪[1]
20232067
gamma = 𝚪[2]
20242068
alpha_dot = 𝚪d[0]
20252069
beta_dot = 𝚪d[1]
20262070
gamma_dot = 𝚪d[2]
20272071
Ad = np.array([
2028-
[(beta_dot*math.sin(beta)*math.cos(gamma)/math.cos(beta) - gamma_dot*math.sin(gamma))/math.cos(beta), (beta_dot*math.sin(beta)*math.sin(gamma)/math.cos(beta) + gamma_dot*math.cos(gamma))/math.cos(beta), 0],
2029-
[-gamma_dot*math.cos(gamma), -gamma_dot*math.sin(gamma), 0],
2030-
[beta_dot*math.cos(gamma)/math.cos(beta)**2 - gamma_dot*math.sin(gamma)*math.tan(beta), beta_dot*math.sin(gamma)/math.cos(beta)**2 + gamma_dot*math.cos(gamma)*math.tan(beta), 0]
2031-
])
2072+
[ (beta_dot*math.sin(beta)*math.cos(gamma)/math.cos(beta) \
2073+
- gamma_dot*math.sin(gamma))/math.cos(beta),
2074+
(beta_dot*math.sin(beta)*math.sin(gamma)/math.cos(beta) \
2075+
+ gamma_dot*math.cos(gamma))/math.cos(beta),
2076+
0
2077+
],
2078+
[ -gamma_dot*math.cos(gamma), -gamma_dot*math.sin(gamma), 0],
2079+
[ beta_dot*math.cos(gamma)/math.cos(beta)**2 \
2080+
- gamma_dot*math.sin(gamma)*math.tan(beta),
2081+
beta_dot*math.sin(gamma)/math.cos(beta)**2 \
2082+
+ gamma_dot*math.cos(gamma)*math.tan(beta),
2083+
0
2084+
]
2085+
])
20322086

20332087
elif representation == 'eul':
2088+
# autogenerated by symbolic/angvelxform.ipynb
20342089
phi = 𝚪[0]
20352090
theta = 𝚪[1]
20362091
psi = 𝚪[2]
20372092
phi_dot = 𝚪d[0]
20382093
theta_dot = 𝚪d[1]
20392094
psi_dot = 𝚪d[2]
2040-
A = np.array([
2041-
[phi_dot*math.sin(phi)/math.tan(theta) + theta_dot*math.cos(phi)/math.sin(theta)**2, -phi_dot*math.cos(phi)/math.tan(theta) + theta_dot*math.sin(phi)/math.sin(theta)**2, 0],
2042-
[-phi_dot*math.cos(phi), -phi_dot*math.sin(phi), 0],
2043-
[-(phi_dot*math.sin(phi) + theta_dot*math.cos(phi)*math.cos(theta)/math.sin(theta))/math.sin(theta), (phi_dot*math.cos(phi) - theta_dot*math.sin(phi)*math.cos(theta)/math.sin(theta))/math.sin(theta), 0]
2044-
])
2095+
Ad = np.array([
2096+
[ phi_dot*math.sin(phi)/math.tan(theta) \
2097+
+ theta_dot*math.cos(phi)/math.sin(theta)**2,
2098+
-phi_dot*math.cos(phi)/math.tan(theta) +\
2099+
theta_dot*math.sin(phi)/math.sin(theta)**2,
2100+
0
2101+
],
2102+
[ -phi_dot*math.cos(phi), -phi_dot*math.sin(phi), 0],
2103+
[ -(phi_dot*math.sin(phi) \
2104+
+ theta_dot*math.cos(phi)*math.cos(theta) \
2105+
/math.sin(theta))/math.sin(theta),
2106+
(phi_dot*math.cos(phi) \
2107+
- theta_dot*math.sin(phi)*math.cos(theta) \
2108+
/math.sin(theta))/math.sin(theta),
2109+
0
2110+
]
2111+
])
20452112

20462113
elif representation == 'exp':
2047-
raise UserWarning('not implemented yet')
2114+
# autogenerated by symbolic/angvelxform_dot.ipynb
2115+
v = 𝚪
2116+
vd = 𝚪d
2117+
sk = base.skew(v)
2118+
skd = base.skew(vd)
2119+
theta_dot = np.inner(𝚪, 𝚪d) / base.norm(𝚪)
2120+
theta = base.norm(𝚪)
2121+
Theta = (1 - theta / 2 * np.sin(theta) / (1 - np.cos(theta)))
2122+
Theta_dot = (-0.5*theta*theta_dot*math.cos(theta)/(1 - math.cos(theta)) \
2123+
+ 0.5*theta*theta_dot*math.sin(theta)**2/(1 - math.cos(theta))**2\
2124+
- 0.5*theta_dot*math.sin(theta)/(1 - math.cos(theta)))/theta**2\
2125+
- 2*theta_dot*(-1/2*theta*math.sin(theta)/(1 - math.cos(theta)) + 1)/theta**3
2126+
2127+
Ad = -0.5 * skd + 2 * sk @ skd * Theta + sk @ sk * Theta_dot
20482128
else:
20492129
raise ValueError('bad representation specified')
20502130

20512131
if full:
2052-
return sp.linalg.block_diag(np.eye(3,3), A)
2132+
return sp.linalg.block_diag(np.eye(3,3), Ad)
20532133
else:
2054-
return A
2134+
return Ad
20552135

20562136
def tr2adjoint(T):
20572137
r"""

symbolic/rotation-derivative.ipynb renamed to symbolic/angvelxform.ipynb

Lines changed: 11 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -6,32 +6,34 @@
66
"source": [
77
"# Determine derivatives of angular sequence to rotation matrix\n",
88
"\n",
9+
"Peter Corke 2021\n",
10+
"\n",
911
"SymPy code to evaluate the mappings from rotation angle sequences to rotation matrix, then compute the derivatives to determine the mapping from angular rates to angular velocity.\n",
1012
"\n",
1113
"Adjust the next cell to choose the particular angle sequence."
1214
]
1315
},
1416
{
1517
"cell_type": "code",
16-
"execution_count": 225,
18+
"execution_count": 226,
1719
"metadata": {},
1820
"outputs": [],
1921
"source": [
20-
"angle_names = ('alpha', 'beta', 'gamma')\n",
21-
"func = eul2r\n",
22-
"\n",
23-
"#angle_names = ('phi', 'theta', 'psi')\n",
24-
"#func = lambda Gamma: rpy2r(Gamma, order='xyz')"
22+
"from spatialmath.base import *\n",
23+
"from sympy import *"
2524
]
2625
},
2726
{
2827
"cell_type": "code",
29-
"execution_count": 226,
28+
"execution_count": 225,
3029
"metadata": {},
3130
"outputs": [],
3231
"source": [
33-
"from spatialmath.base import *\n",
34-
"from sympy import *"
32+
"angle_names = ('alpha', 'beta', 'gamma')\n",
33+
"func = eul2r\n",
34+
"\n",
35+
"#angle_names = ('phi', 'theta', 'psi')\n",
36+
"#func = lambda Gamma: rpy2r(Gamma, order='xyz')"
3537
]
3638
},
3739
{

0 commit comments

Comments
 (0)