(self)
return root_pose_reward
Get the root pose reward.
Get the root pose reward.
Get the root pose reward .
def _calc_reward_root_pose(self):
    """Get the root pose reward."""
    root_pos_ref = self._get_ref_base_position()
    root_rot_ref = self._get_ref_base_rotation()
    root_pos_sim = self._get_sim_base_position()
    root_rot_sim = self._get_sim_base_rotation()

    root_pos_diff = root_pos_ref - root_pos_sim
    root_pos_err = root_pos_diff.dot(root_pos_diff)

    root_rot_diff = transformations.quaternion_multiply(
        root_rot_ref, transformations.quaternion_conjugate(root_rot_sim))
    _, root_rot_diff_angle = pose3d.QuaternionToAxisAngle(root_rot_diff)
    root_rot_diff_angle = motion_util.normalize_rotation_angle(
        root_rot_diff_angle)
    root_rot_err = root_rot_diff_angle * root_rot_diff_angle

    root_pose_err = root_pos_err + 0.5 * root_rot_err
    root_pose_reward = np.exp(-self._root_pose_err_scale * root_pose_err)

    return root_pose_reward
def _calc_reward_root_pose ( self ) : root_pos_ref = self . _get_ref_base_position ( ) root_rot_ref = self . _get_ref_base_rotation ( ) root_pos_sim = self . _get_sim_base_position ( ) root_rot_sim = self . _get_sim_base_rotation ( ) root_pos_diff = root_pos_ref - root_pos_sim root_pos_err = root_pos_diff . dot ( root_pos_diff ) root_rot_diff = transformations . quaternion_multiply ( root_rot_ref , transformations . quaternion_conjugate ( root_rot_sim ) ) _ , root_rot_diff_angle = pose3d . QuaternionToAxisAngle ( root_rot_diff ) root_rot_diff_angle = motion_util . normalize_rotation_angle ( root_rot_diff_angle ) root_rot_err = root_rot_diff_angle * root_rot_diff_angle root_pose_err = root_pos_err + 0.5 * root_rot_err root_pose_reward = np . exp ( - self . _root_pose_err_scale * root_pose_err ) return root_pose_reward