def test_rpy_angle(self):
a, b = rpy_angle(rpy_matrix(pi / 6, pi / 5, pi / 3))
testing.assert_almost_equal(a, np.array([pi / 6, pi / 5, pi / 3]))
testing.assert_almost_equal(
b, np.array([3.66519143, 2.51327412, -2.0943951]))
rot = np.array([[0, 0, 1], [0, -1, 0], [1, 0, 0]])
testing.assert_almost_equal(
rpy_angle(rot)[0], np.array([0, -pi / 2.0, pi]))
def get_robot_config(robot_model, joint_list, with_base=False):
"""A utility function for getting robot state
Parameters
----------
robot_model : skrobot.model.CascadedLink
robot model
joint_list : list[Joint]
joint list of which you want to know the angles
with_base : bool
If set to `True`, base position is also computed.
Returns
-------
av_whole (or av_joint) : numpy.ndarray(n_dof,)
angle vector. If `with_base=False`, `n_dof` is the number of
joints `n_joint`, but if `with_base=True`, `n_dof = n_joint + 3`.
"""
av_joint = np.array([j.joint_angle() for j in joint_list])
if with_base:
x, y, _ = robot_model.translation
rpy = rpy_angle(robot_model.rotation)[0]
theta = rpy[0]
av_whole = np.hstack((av_joint, [x, y, theta]))
return av_whole
else:
return av_joint
def test_rodrigues(self):
mat = rpy_matrix(pi / 6, pi / 5, pi / 3)
theta, axis = rotation_angle(mat)
rec_mat = rodrigues(axis, theta)
testing.assert_array_almost_equal(mat, rec_mat)
mat = rpy_matrix(-pi / 6, -pi / 5, -pi / 3)
theta, axis = rotation_angle(mat)
rec_mat = rodrigues(axis, theta)
testing.assert_array_almost_equal(mat, rec_mat)
# case of theta is None
axis = np.array([np.pi, 0, 0], 'f')
rec_mat = rodrigues(axis)
testing.assert_array_almost_equal(
rpy_angle(rec_mat)[0], np.array([0.0, 0.0, -np.pi], 'f'))
def rpy_angle(self):
"""Return a pair of rpy angles of this coordinates.
Returns
-------
rpy_angle(self.rotation) : tuple(numpy.ndarray, numpy.ndarray)
a pair of rpy angles. See also skrobot.coordinates.math.rpy_angle
Examples
--------
>>> import numpy as np
>>> from skrobot.coordinates import Coordinates
>>> c = Coordinates().rotate(np.pi / 2.0, 'x').rotate(np.pi / 3.0, 'z')
>>> r.rpy_angle()
(array([ 3.84592537e-16, -1.04719755e+00, 1.57079633e+00]),
array([ 3.14159265, -2.0943951 , -1.57079633]))
"""
return rpy_angle(self.rotation)
def __str__(self):
self.worldrot()
pos = self.worldpos()
self.rpy = rpy_angle(self.rotation)[0]
if self.name:
prefix = self.__class__.__name__ + ':' + self.name
else:
prefix = self.__class__.__name__
return "#<{0} {1} "\
"{2:.3f} {3:.3f} {4:.3f} / {5:.1f} {6:.1f} {7:.1f}>".\
format(prefix,
hex(id(self)),
pos[0],
pos[1],
pos[2],
self.rpy[0],
self.rpy[1],
self.rpy[2])
def test_rpy_angle(self):
a, b = rpy_angle(rpy_matrix(pi / 6, pi / 5, pi / 3))
testing.assert_almost_equal(a, np.array([pi / 6, pi / 5, pi / 3]))
testing.assert_almost_equal(
b, np.array([3.66519143, 2.51327412, -2.0943951]))
