8000 add new function for angular velocity transforms · flyinger/spatialmath-python@9e47d41 · GitHub
[go: up one dir, main page]
Skip to content

Commit 9e47d41

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

File tree

2 files changed

+203
-0
lines changed

2 files changed

+203
-0
lines changed

‎spatialmath/base/__init__.py

Lines 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

Lines 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