def set_rpy(self, roll, pitch, yaw):
"""
set the pose of the object using rpy
:param roll: radian
:param pitch: radian
:param yaw: radian
:return:
author: weiwei
date: 20190513
"""
npmat3 = rm.rotmat_from_euler(roll, pitch, yaw, axes="sxyz")
self.set_rotmat(npmat3)
mcd.update_pose(self._cdmesh, self._objpdnp)
def set_homomat(self, npmat4):
self._objpdnp.setPosQuat(da.npv3_to_pdv3(npmat4[:3, 3]),
da.npmat3_to_pdquat(npmat4[:3, :3]))
mcd.update_pose(self._cdmesh, self._objpdnp)
def set_rotmat(self, npmat3):
self._objpdnp.setQuat(da.npmat3_to_pdquat(npmat3))
mcd.update_pose(self._cdmesh, self._objpdnp)
def set_pos(self, npvec3):
self._objpdnp.setPos(npvec3[0], npvec3[1], npvec3[2])
mcd.update_pose(self._cdmesh, self._objpdnp)
def set_pose(self, pos: npt.NDArray = np.zeros(3), rotmat: npt.NDArray = np.eye(3)):
self._objpdnp.setPosQuat(da.npv3_to_pdv3(pos), da.npmat3_to_pdquat(rotmat))
mcd.update_pose(self._cdmesh, self._objpdnp)
def set_rotmat(self, rotmat: npt.NDArray = np.eye(3)):
self._objpdnp.setQuat(da.npmat3_to_pdquat(rotmat))
mcd.update_pose(self._cdmesh, self._objpdnp)
def set_pos(self, pos: npt.NDArray = np.zeros(3)):
self._objpdnp.setPos(pos[0], pos[1], pos[2])
mcd.update_pose(self._cdmesh, self._objpdnp)