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)