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)
