def _ee_vel_cur(self): return np.matrix(rosmsg2nparray(self._robot_state_cur.ee_twist)).T.round(decimals=self._decimals)
def _ee_state_err(self): pose_ref = self._mani_ref.ee_pose if self._is_initialized_mani_ref else self._robot_state_cur.ee_pose pose_err = diff_poses(self._robot_state_cur.ee_pose, pose_ref) return np.matrix(rosmsg2nparray(pose_err)).T.round(decimals=self._decimals)
def _ee_vel_ref(self): twist_ref = self._mani_ref.ee_twist if self._is_initialized_mani_ref else Twist() return np.matrix(rosmsg2nparray(twist_ref)).T.round(decimals=self._decimals)