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 update_input(self): self.arm_base_tf = update_tf(self._tf_buffer, self.arm_base_tf) # TODO: diff_poses may be wrong ee_pose_from_arm_base = diff_poses(self.ref_pose_from_origin_frame, self.arm_base_tf) return PoseStamped(header=Header(stamp=rospy.Time.now()), pose=ee_pose_from_arm_base)