@@ -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:`\n u`, 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} \v ec{\n u}
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:`\n u`, 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{\n u}
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
18462048def tr2adjoint (T ):
18472049 r"""
0 commit comments