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]))