Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly
Appearance settings

Commit 9e47d41

Browse filesBrowse files
petercorkepetercorke
committed
add new function for angular velocity transforms
1 parent 506cbee commit 9e47d41
Copy full SHA for 9e47d41

File tree

Expand file treeCollapse file tree

2 files changed

+203
-0
lines changed
Filter options
Expand file treeCollapse file tree

2 files changed

+203
-0
lines changed

‎spatialmath/base/__init__.py

Copy file name to clipboardExpand all lines: spatialmath/base/__init__.py
+1Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -98,6 +98,7 @@
9898
'eul2jac',
9999
'exp2jac',
100100
'rot2jac',
101+
'angvelxform',
101102
'trprint',
102103
'trplot',
103104
'tranimate',

‎spatialmath/base/transforms3d.py

Copy file name to clipboardExpand all lines: spatialmath/base/transforms3d.py
+202Lines changed: 202 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1841,7 +1841,209 @@ def rot2jac(R, representation='rpy-xyz'):
18411841
return sp.linalg.block_diag(np.eye(3,3), np.linalg.inv(A))
18421842

18431843

1844+
def angvelxform(𝚪, inverse=False, full=True, representation='rpy/xyz'):
1845+
"""
1846+
Angular velocity transformation
1847+
1848+
:param 𝚪: angular representation
1849+
:type 𝚪: ndarray(3)
1850+
:param representation: defaults to 'rpy-xyz'
1851+
:type representation: str, optional
1852+
:param inverse: compute mapping from analytical rates to angular velocity
1853+
:type inverse: bool
1854+
:param full: return 6x6 transform for spatial velocity
1855+
:type full: bool
1856+
:return: angular velocity transformation matrix
1857+
:rtype: ndarray(6,6) or ndarray(3,3)
1858+
1859+
Computes the transformation from spatial velocity, where rotation rate is
1860+
expressed as angular velocity, to analytical rates where the rotational part
1861+
is expressed as rate of change in some other representation
1862+
1863+
Computes the transformation from spatial velocity :math:`\nu`, where
1864+
rotation rate is expressed as angular velocity, to analytical rates
1865+
:math:`\dvec{x}` where the rotational part is expressed as rate of change in
1866+
some other representation
1867+
1868+
.. math::
1869+
\dvec{x} = \mat{A} \vec{\nu}
18441870
1871+
where :math:`\mat{A}` is a block diagonal 6x6 matrix
1872+
1873+
================== ========================================
1874+
``representation`` Rotational representation
1875+
================== ========================================
1876+
``'rpy/xyz'`` RPY angular rates in XYZ order (default)
1877+
``'rpy/zyx'`` RPY angular rates in XYZ order
1878+
``'eul'`` Euler angular rates in ZYZ order
1879+
``'exp'`` exponential coordinate rates
1880+
================= ========================================
1881+
1882+
.. note:: Compared to :func:`eul2jac`, :func:`rpy2jac`, :func:`exp2jac`
1883+
- This performs the inverse mapping
1884+
- This maps a 6-vector, the others map a 3-vector
1885+
1886+
https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2016/RD2016script.pdf
1887+
:seealso: :func:`rot2jac`, :func:`eul2jac`, :func:`rpy2r`, :func:`exp2jac`
1888+
"""
1889+
1890+
if representation == 'rpy/xyz':
1891+
alpha = 𝚪[0]
1892+
beta = 𝚪[1]
1893+
gamma = 𝚪[2]
1894+
if inverse:
1895+
A = np.array([
1896+
[math.sin(beta), 0, 1],
1897+
[-math.sin(gamma)*math.cos(beta), math.cos(gamma), 0],
1898+
[math.cos(beta)*math.cos(gamma), math.sin(gamma), 0]
1899+
])
1900+
else:
1901+
A = np.array([
1902+
[0, -math.sin(gamma)/math.cos(beta), math.cos(gamma)/math.cos(beta)],
1903+
[0, math.cos(gamma), math.sin(gamma)],
1904+
[1, math.sin(gamma)*math.tan(beta), -math.cos(gamma)*math.tan(beta)]
1905+
])
1906+
1907+
1908+
elif representation == 'rpy/zyx':
1909+
alpha = 𝚪[0]
1910+
beta = 𝚪[1]
1911+
gamma = 𝚪[2]
1912+
1913+
if inverse:
1914+
A = np.array([
1915+
[math.cos(beta)*math.cos(gamma), -math.sin(gamma), 0],
1916+
[math.sin(gamma)*math.cos(beta), math.cos(gamma), 0],
1917+
[-math.sin(beta), 0, 1]
1918+
])
1919+
else:
1920+
A = np.array([
1921+
[math.cos(gamma)/math.cos(beta), math.sin(gamma)/math.cos(beta), 0],
1922+
[-math.sin(gamma), math.cos(gamma), 0],
1923+
[math.cos(gamma)*math.tan(beta), math.sin(gamma)*math.tan(beta), 1]
1924+
])
1925+
1926+
elif representation == 'eul':
1927+
phi = 𝚪[0]
1928+
theta = 𝚪[1]
1929+
psi = 𝚪[2]
1930+
1931+
if inverse:
1932+
A = np.array([
1933+
[0, -math.sin(phi), math.sin(theta)*math.cos(phi)],
1934+
[0, math.cos(phi), math.sin(phi)*math.sin(theta)],
1935+
[1, 0, math.cos(theta)]
1936+
])
1937+
else:
1938+
A = np.array([
1939+
[-math.cos(phi)/math.tan(theta), -math.sin(phi)/math.tan(theta), 1],
1940+
[-math.sin(phi), math.cos(phi), 0],
1941+
[math.cos(phi)/math.sin(theta), math.sin(phi)/math.sin(theta), 0]
1942+
])
1943+
elif representation == 'exp':
1944+
raise UserWarning('not implemented yet')
1945+
else:
1946+
raise ValueError('bad representation specified')
1947+
1948+
if full:
1949+
return sp.linalg.block_diag(np.eye(3,3), A)
1950+
else:
1951+
return A
1952+
1953+
def angvelxform_dot(𝚪, 𝚪d, full=True, representation='rpy/xyz'):
1954+
"""
1955+
Angular acceleratipn transformation
1956+
1957+
:param 𝚪: angular representation
1958+
:type 𝚪: ndarray(3)
1959+
:param 𝚪d: angular representation rate
1960+
:type 𝚪d: ndarray(3)
1961+
:param representation: defaults to 'rpy-xyz'
1962+
:type representation: str, optional
1963+
:param full: return 6x6 transform for spatial velocity
1964+
:type full: bool
1965+
:return: angular velocity transformation matrix
1966+
:rtype: ndarray(6,6) or ndarray(3,3)
1967+
1968+
Computes the transformation from spatial velocity, where rotation rate is
1969+
expressed as angular velocity, to analytical rates where the rotational part
1970+
is expressed as rate of change in some other representation
1971+
1972+
Computes the transformation from spatial velocity :math:`\nu`, where
1973+
rotation rate is expressed as angular velocity, to analytical rates
1974+
:math:`\dvec{x}` where the rotational part is expressed as rate of change in
1975+
some other representation
1976+
1977+
.. math::
1978+
\ddvec{x} = \mat{A}_d \dvec{\nu}
1979+
1980+
where :math:`\mat{A}_d` is a block diagonal 6x6 matrix
1981+
1982+
================== ========================================
1983+
``representation`` Rotational representation
1984+
================== ========================================
1985+
``'rpy/xyz'`` RPY angular rates in XYZ order (default)
1986+
``'rpy/zyx'`` RPY angular rates in XYZ order
1987+
``'eul'`` Euler angular rates in ZYZ order
1988+
``'exp'`` exponential coordinate rates
1989+
================= ========================================
1990+
1991+
.. note:: Compared to :func:`eul2jac`, :func:`rpy2jac`, :func:`exp2jac`
1992+
- This performs the inverse mapping
1993+
- This maps a 6-vector, the others map a 3-vector
1994+
1995+
https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2016/RD2016script.pdf
1996+
:seealso: :func:`rot2jac`, :func:`eul2jac`, :func:`rpy2r`, :func:`exp2jac`
1997+
"""
1998+
1999+
if representation == 'rpy/xyz':
2000+
alpha = 𝚪[0]
2001+
beta = 𝚪[1]
2002+
gamma = 𝚪[2]
2003+
alpha_dot = 𝚪d[0]
2004+
beta_dot = 𝚪d[1]
2005+
gamma_dot = 𝚪d[2]
2006+
Ad = np.array([
2007+
[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)],
2008+
[0, -gamma_dot*math.sin(gamma), gamma_dot*math.cos(gamma)],
2009+
[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)]
2010+
])
2011+
2012+
elif representation == 'rpy/zyx':
2013+
alpha = 𝚪[0]
2014+
beta = 𝚪[1]
2015+
gamma = 𝚪[2]
2016+
alpha_dot = 𝚪d[0]
2017+
beta_dot = 𝚪d[1]
2018+
gamma_dot = 𝚪d[2]
2019+
Ad = np.array([
2020+
[(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],
2021+
[-gamma_dot*math.cos(gamma), -gamma_dot*math.sin(gamma), 0],
2022+
[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]
2023+
])
2024+
2025+
elif representation == 'eul':
2026+
phi = 𝚪[0]
2027+
theta = 𝚪[1]
2028+
psi = 𝚪[2]
2029+
phi_dot = 𝚪d[0]
2030+
theta_dot = 𝚪d[1]
2031+
psi_dot = 𝚪d[2]
2032+
A = np.array([
2033+
[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],
2034+
[-phi_dot*math.cos(phi), -phi_dot*math.sin(phi), 0],
2035+
[-(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]
2036+
])
2037+
2038+
elif representation == 'exp':
2039+
raise UserWarning('not implemented yet')
2040+
else:
2041+
raise ValueError('bad representation specified')
2042+
2043+
if full:
2044+
return sp.linalg.block_diag(np.eye(3,3), A)
2045+
else:
2046+
return A
18452047

18462048
def tr2adjoint(T):
18472049
r"""

0 commit comments

Comments
0 (0)
Morty Proxy This is a proxified and sanitized view of the page, visit original site.