text:
# This code is part of Qiskit.
#
# (C) Copyright IBM 2018, 2020
#
# This code is licensed under the Apache License, Version 2.0. You may
# obtain a copy of this license in the LICENSE.txt file in the root directory
# of this source tree or at http://www.apache.org/licenses/LICENSE-2.0.
#
# Any modifications or derivative works of this code must retain this
# copyright notice, and modified files need to carry a notice indicating
# that they have been altered from the originals.
"""
This module contains the definition of a base class for a chemistry operator.
Such an operator takes a QMolecule and produces an input for
a quantum algorithm
"""

from abc import ABC, abstractmethod
import warnings
import logging
from typing import Dict, Union, List, Tuple, Optional, cast
import numpy as np

from qiskit.aqua.algorithms import MinimumEigensolverResult, EigensolverResult, AlgorithmResult
from qiskit.chemistry import QMolecule

logger = logging.getLogger(__name__)

# A dipole moment, when present as X, Y and Z components will normally have float values for all
# the components. However when using Z2Symmetries, if the dipole component operator does not
# commute with the symmetry then no evaluation is done and None will be used as the 'value'
# indicating no measurement of the observable took place
DipoleTuple = Tuple[Optional[float], Optional[float], Optional[float]]

class ChemistryOperator(ABC):
    """
    Base class for ChemistryOperator.
    """

    INFO_NUM_PARTICLES = 'num_particles'
    INFO_NUM_ORBITALS = 'num_orbitals'
    INFO_TWO_QUBIT_REDUCTION = 'two_qubit_reduction'
    INFO_Z2SYMMETRIES = 'z2symmetries'

    @abstractmethod
    def __init__(self):
        warnings.warn('The ChemistryOperator is deprecated as of Qiskit Aqua 0.8.0 and will be '
                      'removed no earlier than 3 months after the release date. Instead, the '
                      'FermionicTransformation can be used to transform QMolecules and construct '
                      'ground state result objects.', DeprecationWarning, stacklevel=2)
        self._molecule_info = {}

    @abstractmethod
    def run(self, qmolecule):
        """
        Convert the qmolecule, according to the ChemistryOperator, into an Operator
        that can be given to a QuantumAlgorithm

        Args:
            qmolecule (QMolecule): from a chemistry driver

        Returns:
            Tuple: (qubit_op, aux_ops)
        """
        raise NotImplementedError

    def process_algorithm_result(
            self, algo_result: Union[dict,
                                     MinimumEigensolverResult,
                                     EigensolverResult]) -> Union[Tuple[List[str], dict],
                                                                  'MolecularGroundStateResult',
                                                                  'MolecularExcitedStatesResult']:
        """
        Takes the algorithm result and processes it as required, e.g. by
        combination of any parts that were classically computed, for the
        final result.

        Args:
            algo_result: Result from algorithm

        Returns:
            Final chemistry result computed from the algorithm result
        """
        if isinstance(algo_result, MinimumEigensolverResult):
            return self._process_algorithm_result(algo_result)
        elif isinstance(algo_result, EigensolverResult):
            return self._process_algorithm_result(algo_result)
        else:
            lines, result = self._process_algorithm_result(algo_result)
            result['algorithm_retvals'] = algo_result
            return lines, result

    @abstractmethod
    def _process_algorithm_result(self, algo_result):
        raise NotImplementedError

    @property
    def molecule_info(self):
        """ returns molecule info """
        return self._molecule_info

    def _add_molecule_info(self, key, value):
        self._molecule_info[key] = value

class MolecularChemistryResult(AlgorithmResult):
    """
    Molecular chemistry Result

    Energies are in Hartree and dipole moments in A.U unless otherwise stated.
    """

    def __init__(self, a_dict: Optional[Dict] = None) -> None:
        super().__init__(a_dict)
        warnings.warn('The qiskit.chemistry.chemistry_operator.MolecularChemistryResult object is '
                      'deprecated as of 0.8.0 and will be removed no sooner than 3 months after the'
                      ' release. You should use qiskit.chemistry.algorithms.ground_state_solvers.'
                      'FermionicGroundStateResult instead.', DeprecationWarning, stacklevel=2)

    @property
    def algorithm_result(self) -> AlgorithmResult:
        """ Returns raw algorithm result """
        return self.get('algorithm_result')

    @algorithm_result.setter
    def algorithm_result(self, value: AlgorithmResult) -> None:
        """ Sets raw algorithm result """
        self.data['algorithm_result'] = value

    @property
    def hartree_fock_energy(self) -> float:
        """ Returns Hartree-Fock energy """
        return self.get('hartree_fock_energy')

    @hartree_fock_energy.setter
    def hartree_fock_energy(self, value: float) -> None:
        """ Sets Hartree-Fock energy """
        self.data['hartree_fock_energy'] = value

    @property
    def nuclear_repulsion_energy(self) -> Optional[float]:
        """ Returns nuclear repulsion energy when available from driver """
        return self.get('nuclear_repulsion_energy')

    @nuclear_repulsion_energy.setter
    def nuclear_repulsion_energy(self, value: float) -> None:
        """ Sets nuclear repulsion energy """
        self.data['nuclear_repulsion_energy'] = value

    @property
    def nuclear_dipole_moment(self) -> Optional[DipoleTuple]:
        """ Returns nuclear dipole moment X,Y,Z components in A.U when available from driver """
        return self.get('nuclear_dipole_moment')

    @nuclear_dipole_moment.setter
    def nuclear_dipole_moment(self, value: DipoleTuple) -> None:
        """ Sets nuclear dipole moment in A.U """
        self.data['nuclear_dipole_moment'] = value

class MolecularGroundStateResult(MolecularChemistryResult):
    """
    Molecular Ground State Energy Result.

    Energies are in Hartree and dipole moments in A.U unless otherwise stated.
    """

    def __init__(self, a_dict: Optional[Dict] = None) -> None:
        super().__init__(a_dict)
        warnings.warn('The qiskit.chemistry.chemistry_operator.MolecularGroundStateResult object '
                      'is deprecated as of 0.8.0 and will be removed no sooner than 3 months after '
                      'the release. You should use qiskit.chemistry.algorithms.'
                      'ground_state_solvers.FermionicGroundStateResult instead.',
                      DeprecationWarning, stacklevel=2)

    @property
    def energy(self) -> Optional[float]:
        """ Returns ground state energy if nuclear_repulsion_energy is available from driver """
        nre = self.nuclear_repulsion_energy
        return self.electronic_energy + nre if nre is not None else None

    @property
    def electronic_energy(self) -> float:
        """ Returns electronic part of ground state energy """
        return (self.computed_electronic_energy
                + self.ph_extracted_energy
                + self.frozen_extracted_energy)

    @property
    def computed_electronic_energy(self) -> float:
        """ Returns computed electronic part of ground state energy """
        return self.get('computed_electronic_energy')

    @computed_electronic_energy.setter
    def computed_electronic_energy(self, value: float) -> None:
        """ Sets computed electronic part of ground state energy """
        self.data['computed_electronic_energy'] = value

    @property
    def ph_extracted_energy(self) -> float:
        """ Returns particle hole extracted part of ground state energy """
        return self.get('ph_extracted_energy')

    @ph_extracted_energy.setter
    def ph_extracted_energy(self, value: float) -> None:
        """ Sets particle hole extracted part of ground state energy """
        self.data['ph_extracted_energy'] = value

    @property
    def frozen_extracted_energy(self) -> float:
        """ Returns frozen extracted part of ground state energy """
        return self.get('frozen_extracted_energy')

    @frozen_extracted_energy.setter
    def frozen_extracted_energy(self, value: float) -> None:
        """ Sets frozen extracted part of ground state energy """
        self.data['frozen_extracted_energy'] = value

    # Dipole moment results. Note dipole moments of tuples of X, Y and Z components. Chemistry
    # drivers either support dipole integrals or not. Note that when using Z2 symmetries of

    def has_dipole(self) -> bool:
        """ Returns whether dipole moment is present in result or not """
        return self.nuclear_dipole_moment is not None and self.electronic_dipole_moment is not None

    @property
    def reverse_dipole_sign(self) -> bool:
        """ Returns if electronic dipole moment sign should be reversed when adding to nuclear """
        return self.get('reverse_dipole_sign')

    @reverse_dipole_sign.setter
    def reverse_dipole_sign(self, value: bool) -> None:
        """ Sets if electronic dipole moment sign should be reversed when adding to nuclear """
        self.data['reverse_dipole_sign'] = value

    @property
    def total_dipole_moment(self) -> Optional[float]:
        """ Returns total dipole of moment """
        if self.dipole_moment is None:
            return None  # No dipole at all
        if np.any(np.equal(list(self.dipole_moment), None)):
            return None  # One or more components in the dipole is None
        return np.sqrt(np.sum(np.power(list(self.dipole_moment), 2)))

    @property
    def total_dipole_moment_in_debye(self) -> Optional[float]:
        """ Returns total dipole of moment in Debye """
        tdm = self.total_dipole_moment
        return tdm / QMolecule.DEBYE if tdm is not None else None

    @property
    def dipole_moment(self) -> Optional[DipoleTuple]:
        """ Returns dipole moment """
        edm = self.electronic_dipole_moment
        if self.reverse_dipole_sign:
            edm = cast(DipoleTuple, tuple(-1 * x if x is not None else None for x in edm))
        return _dipole_tuple_add(edm, self.nuclear_dipole_moment)

    @property
    def dipole_moment_in_debye(self) -> Optional[DipoleTuple]:
        """ Returns dipole moment in Debye """
        dipm = self.dipole_moment
        if dipm is None:
            return None
        dipmd0 = dipm[0]/QMolecule.DEBYE if dipm[0] is not None else None
        dipmd1 = dipm[1]/QMolecule.DEBYE if dipm[1] is not None else None
        dipmd2 = dipm[2]/QMolecule.DEBYE if dipm[2] is not None else None
        return dipmd0, dipmd1, dipmd2

    @property
    def electronic_dipole_moment(self) -> Optional[DipoleTuple]:
        """ Returns electronic dipole moment """
        return _dipole_tuple_add(self.computed_dipole_moment,
                                 _dipole_tuple_add(self.ph_extracted_dipole_moment,
                                                   self.frozen_extracted_dipole_moment))

    @property
    def computed_dipole_moment(self) -> Optional[DipoleTuple]:
        """ Returns computed electronic part of dipole moment """
        return self.get('computed_dipole_moment')

    @computed_dipole_moment.setter
    def computed_dipole_moment(self, value: DipoleTuple) -> None:
        """ Sets computed electronic part of dipole moment """
        self.data['computed_dipole_moment'] = value

    @property
    def ph_extracted_dipole_moment(self) -> Optional[DipoleTuple]:
        """ Returns particle hole extracted part of dipole moment """
        return self.get('ph_extracted_dipole_moment')

    @ph_extracted_dipole_moment.setter
    def ph_extracted_dipole_moment(self, value: DipoleTuple) -> None:
        """ Sets particle hole extracted part of dipole moment """
        self.data['ph_extracted_dipole_moment'] = value

    @property
    def frozen_extracted_dipole_moment(self) -> Optional[DipoleTuple]:
        """ Returns frozen extracted part of dipole moment """
        return self.get('frozen_extracted_dipole_moment')

    @frozen_extracted_dipole_moment.setter
    def frozen_extracted_dipole_moment(self, value: DipoleTuple) -> None:
        """ Sets frozen extracted part of dipole moment """
        self.data['frozen_extracted_dipole_moment'] = value

    # Other measured operators. If these are not evaluated then None will be returned
    # instead of any measured value.

    def has_observables(self):
        """ Returns whether result has aux op observables such as spin, num particles """
        return self.total_angular_momentum is not None \
            or self.num_particles is not None \
            or self.magnetization is not None

    @property
    def total_angular_momentum(self) -> Optional[float]:
        """ Returns total angular momentum (S^2) """
        return self.get('total_angular_momentum')

    @total_angular_momentum.setter
    def total_angular_momentum(self, value: float) -> None:
        """ Sets total angular momentum """
        self.data['total_angular_momentum'] = value

    @property
    def spin(self) -> Optional[float]:
        """ Returns computed spin """
        if self.total_angular_momentum is None:
            return None
        return (-1.0 + np.sqrt(1 + 4 * self.total_angular_momentum)) / 2

    @property
    def num_particles(self) -> Optional[float]:
        """ Returns measured number of particles """
        return self.get('num_particles')

    @num_particles.setter
    def num_particles(self, value: float) -> None:
        """ Sets measured number of particles """
        self.data['num_particles'] = value

    @property
    def magnetization(self) -> Optional[float]:
        """ Returns measured magnetization """
        return self.get('magnetization')

    @magnetization.setter
    def magnetization(self, value: float) -> None:
        """ Sets measured magnetization """
        self.data['magnetization'] = value

    def __str__(self) -> str:
        """ Printable formatted result """
        return '\n'.join(self.formatted)

    @property
    def formatted(self) -> List[str]:
        """ Formatted result as a list of strings """
        lines = []
        lines.append('=== GROUND STATE ENERGY ===')
        lines.append(' ')
        lines.append('* Electronic ground state energy (Hartree): {}'.
                     format(round(self.electronic_energy, 12)))
        lines.append('  - computed part:      {}'.
                     format(round(self.computed_electronic_energy, 12)))
        lines.append('  - frozen energy part: {}'.
                     format(round(self.frozen_extracted_energy, 12)))
        lines.append('  - particle hole part: {}'
                     .format(round(self.ph_extracted_energy, 12)))
        if self.nuclear_repulsion_energy is not None:
            lines.append('~ Nuclear repulsion energy (Hartree): {}'.
                         format(round(self.nuclear_repulsion_energy, 12)))
            lines.append('> Total ground state energy (Hartree): {}'.
                         format(round(self.energy, 12)))
        if self.has_observables():
            line = '  Measured::'
            if self.num_particles is not None:
                line += ' # Particles: {:.3f}'.format(self.num_particles)
            if self.spin is not None:
                line += ' S: {:.3f}'.format(self.spin)
            if self.total_angular_momentum is not None:
                line += ' S^2: {:.3f}'.format(self.total_angular_momentum)
            if self.magnetization is not None:
                line += ' M: {:.5f}'.format(self.magnetization)
            lines.append(line)

        if self.has_dipole():
            lines.append(' ')
            lines.append('=== DIPOLE MOMENT ===')
            lines.append(' ')
            lines.append('* Electronic dipole moment (a.u.): {}'
                         .format(_dipole_to_string(self.electronic_dipole_moment)))
            lines.append('  - computed part:      {}'
                         .format(_dipole_to_string(self.computed_dipole_moment)))
            lines.append('  - frozen energy part: {}'
                         .format(_dipole_to_string(self.frozen_extracted_dipole_moment)))
            lines.append('  - particle hole part: {}'
                         .format(_dipole_to_string(self.ph_extracted_dipole_moment)))
            if self.nuclear_dipole_moment is not None:
                lines.append('~ Nuclear dipole moment (a.u.): {}'
                             .format(_dipole_to_string(self.nuclear_dipole_moment)))
                lines.append('> Dipole moment (a.u.): {}  Total: {}'
                             .format(_dipole_to_string(self.dipole_moment),
                                     _float_to_string(self.total_dipole_moment)))
                lines.append('               (debye): {}  Total: {}'
                             .format(_dipole_to_string(self.dipole_moment_in_debye),
                                     _float_to_string(self.total_dipole_moment_in_debye)))
        return lines

class MolecularExcitedStatesResult(MolecularChemistryResult):
    """
    Molecular Excited States Result

    Energies are in Hartree and dipole moments in A.U unless otherwise stated.
    """
    # TODO This needs completing once EigenSolver interface/result is final
    @property
    def energies(self) -> Tuple:
        """ Returns ground state energy """
        return self.get('energies')

    @energies.setter
    def energies(self, value: Tuple) -> None:
        """ Sets ground state energy """
        self.data['energies'] = value

def _dipole_tuple_add(x: Optional[DipoleTuple],
                      y: Optional[DipoleTuple]) -> Optional[DipoleTuple]:
    """ Utility to add two dipole tuples element-wise for dipole additions """
    if x is None or y is None:
        return None
    return _element_add(x[0], y[0]), _element_add(x[1], y[1]), _element_add(x[2], y[2])

def _element_add(x: Optional[float], y: Optional[float]):
    """ Add dipole elements where a value may be None then None is returned """
    return x + y if x is not None and y is not None else None

def _dipole_to_string(dipole: DipoleTuple):
    dips = [round(x, 8) if x is not None else x for x in dipole]
    value = '['
    for i, _ in enumerate(dips):
        value += _float_to_string(dips[i]) if dips[i] is not None else 'None'
        value += '  ' if i < len(dips)-1 else ']'
    return value

def _float_to_string(value: Optional[float], precision: int = 8) -> str:
    if value is None:
        return 'None'
    else:
        return '0.0' if value == 0 else ('{:.' + str(precision) + 'f}').format(value).rstrip('0')

max_stars_repo_name: stefan-woerner/aqua

max_issues_repo_name: stefan-woerner/aqua

max_forks_repo_name: stefan-woerner/aqua


text:
#!usr/bin/env python
# -*- coding: utf-8 -*-
# author: kuangdd
# date: 2019/12/1
"""
### audio_io
语音IO，语音保存、读取，支持wav和mp3格式，语音形式转换（np.array,bytes,io.BytesIO），支持【.】操作符的字典。
"""
from pathlib import Path
import logging

logging.basicConfig(level=logging.INFO)
logger = logging.getLogger(Path(__file__).stem)

from scipy.io import wavfile
from pathlib import Path
import numpy as np
import librosa
import io
import json
from dotmap import DotMap

_sr = 16000
_int16_max = 2 ** 15 - 1

class Dict2Obj(DotMap):
    """
    修正DotMap的get方法生成DotMap对象的bug。
    Dict2Obj的get方法和dict的get功能相同。
    """

    def __getitem__(self, k):
        if k not in self._map:
            return None
        else:
            return self._map[k]

    def parse(self, json_string):
        if json_string.strip():
            _hp = json.loads(json_string)
            for k, v in _hp.items():
                self[k] = v
        return self

def load_wav(path, sr=None, with_sr=False):
    """
    导入语音信号。支持wav和mp3格式。
    :param path: 文件路径。
    :param sr: 采样率，None: 自动识别采样率。
    :param with_sr: 是否返回采样率。
    :return: np.ndarray
    """
    return load_wav_librosa(path, sr=sr, with_sr=with_sr)

def save_wav(wav, path, sr=_sr):
    save_wav_wavfile(wav, path=path, sr=sr)

def load_wav_librosa(path, sr=_sr, with_sr=False):
    wav, sr = librosa.load(path, sr=sr)
    return (wav, sr) if with_sr else wav

def load_wav_wavfile(path, sr=None, with_sr=False):
    sr, wav = wavfile.read(path)
    wav = wav / max(np.max(np.abs(wav)), 0.01)
    return (wav, sr) if with_sr else wav

def save_wav_librosa(wav, path, sr=_sr):
    librosa.output.write_wav(path, wav, sr=sr)

def save_wav_wavfile(wav, path, sr=_sr, volume=1.):
    out = wav * _int16_max * volume / max(0.01, np.max(np.abs(wav)))
    # proposed by @dsmiller
    wavfile.write(path, sr, out.astype(np.int16))

def anything2bytesio(src, sr=_sr, volume=1.):
    if isinstance(src, (str, Path)):
        src = load_wav(src, sr=sr)
    if isinstance(src, (list, np.ndarray, np.matrix)):
        out_io = io.BytesIO()
        save_wav_wavfile(src, out_io, sr=sr, volume=volume)
    elif isinstance(src, bytes):
        out_io = io.BytesIO(src)
    elif isinstance(src, io.BytesIO):
        out_io = src
    else:
        raise TypeError
    return out_io

def anything2wav(src, sr=_sr, volume=1.):
    if isinstance(src, (list, np.ndarray, np.matrix)):
        return np.array(src)
    else:
        bysio = anything2bytesio(src, sr=sr, volume=volume)
        return load_wav_wavfile(bysio, sr=sr)

def anything2bytes(src, sr=_sr, volume=1.):
    if isinstance(src, bytes):
        return src
    else:
        bysio = anything2bytesio(src, sr=sr, volume=volume)
        return bysio.getvalue()

if __name__ == "__main__":
    print(__file__)
    inpath = r"../hello.wav"
    bys = anything2bytesio(inpath, sr=16000)
    print(bys)
    wav = anything2wav(bys, sr=16000)
    print(wav)

ext: py

max_stars_repo_name: KuangDD/aukit

max_issues_repo_name: kuangdd/aukit

max_forks_repo_name: KuangDD/aukit

reason: import numpy,from scipy


text:
import numpy as np

from fedot.core.data.data import InputData

def get_metric_position(metrics, metric_type):
    for num, metric in enumerate(metrics):
        if isinstance(metric, metric_type):
            return num

def nested_list_transform_to_tuple(data_field):
    if isinstance(data_field, (list, np.ndarray)):
        transformed = tuple(map(nested_list_transform_to_tuple, data_field))
    else:
        transformed = data_field
    return transformed

def input_data_characteristics(data: InputData, log):
    data_type = data.data_type
    if data.features is not None:
        features_hash = hash(nested_list_transform_to_tuple(data.features))
    else:
        features_hash = None
        log.info('Input data features is None')
    if data.target is not None:
        target_hash = hash(nested_list_transform_to_tuple(data.target))
    else:
        log.info('Input data target is None')
        target_hash = None
    return data_type, features_hash, target_hash

max_stars_repo_name: vishalbelsare/FEDOT

max_issues_repo_name: vishalbelsare/FEDOT

max_forks_repo_name: vishalbelsare/FEDOT

reason: import numpy


text:
import torch
import torch.nn as nn
from torch.autograd import grad as torch_grad
from torch.autograd import Variable
import numpy as np

'''
Set of helpers functions for losses an penalties
'''

class WassersteinLoss(torch.nn.Module):
    """
    Wasserstein Loss
    """

    def __init__(self):
        super(WassersteinLoss, self).__init__()

    def forward(self, x, y):
        return torch.mean(x)

class WassersteinNegLoss(torch.nn.Module):
    """
    Wasserstein Loss
    """

    def __init__(self):
        super(WassersteinNegLoss, self).__init__()

    def forward(self, x, y):
        return -torch.mean(x)

class HingeLoss(torch.nn.Module):
    """
    Hinge Loss
    """

    def __init__(self):
        super(HingeLoss, self).__init__()

    def forward(self, x, y):
        return torch.mean(torch.nn.ReLU()(1.0 + x))

class HingeNegLoss(torch.nn.Module):
    """
    Hinge Loss
    """

    def __init__(self):
        super(HingeNegLoss, self).__init__()

    def forward(self, x, y):
        return torch.mean(torch.nn.ReLU()(1.0 - x))

def zero_penalty(self, real_data, fake_data):
    """
    No penalty
    """
    return 0

def clamp_parameters(self):
    """
    Clamp
    """
    for p in self.D.parameters():
        p.data.clamp_(self.clamp_lower, self.clamp_upper)

def get_interpolated_gradient(self, real_data, fake_data):
    """
    Common function to gradient penalty functions
    """
    alpha = torch.rand(self.batch_size, 1)  # , 1, 1)
    alpha = alpha.expand_as(real_data)
    if str(self.device) != 'cpu':
        alpha = alpha.cuda()

    # interpolated
    interpolated = alpha * real_data + (1 - alpha) * fake_data
    interpolated = Variable(interpolated, requires_grad=True)
    if str(self.device) != 'cpu':
        interpolated = interpolated.cuda()

    # Calculate probability of interpolated examples
    prob_interpolated = self.D(interpolated)

    # gradient
    if str(self.device) != 'cpu':
        ones = torch.ones(prob_interpolated.size()).cuda()
    else:
        ones = torch.ones(prob_interpolated.size())

    gradients = torch_grad(outputs=prob_interpolated,
                           inputs=interpolated,
                           grad_outputs=ones,
                           create_graph=True,  # needed ?
                           retain_graph=True,  # needed ?
                           only_inputs=True)[0]

    # gradients.requires_grad_(False) # FIXME
    return gradients

def gradient_penalty(self, real_data, fake_data):
    """
    Gulrajani2017 $(||\nabla_a D(a)||_2 - 1)^2$
    with a = interpolated samples.
    Called two-sided penalty.
    1-GP = one centered GP.
    """

    # gradient
    gradients = get_interpolated_gradient(self, real_data, fake_data)

    # norm
    # gradients_norm = torch.sqrt(torch.sum(gradients ** 2, dim=1) + 1e-12)
    gradients_norm = gradients.norm(2, dim=1)

    # Two sides penalty
    gradient_penalty = ((gradients_norm - 1) ** 2).mean()

    return gradient_penalty

def gradient_penalty_centered(self, real_data, fake_data):
    """
    Thanh-Tung2019 $(||\nabla_a D(a)||_2)^2$
    with a = interpolated samples.
    0-GP = zero centered GP.
    """

    # gradient
    gradients = get_interpolated_gradient(self, real_data, fake_data)

    # norm
    # gradients_norm = torch.sqrt(torch.sum(gradients ** 2, dim=1) + 1e-12)
    gradients_norm = gradients.norm(2, dim=1)

    # Two sides penalty
    gradient_penalty = ((gradients_norm) ** 2).mean()

    return gradient_penalty

def gradient_penalty_max(self, real_data, fake_data):
    """
    Petzka2018 $(max||\nabla_a D(a)||_2 - 1)^2$
    with a = interpolated samples.
    """

    # gradient
    gradients = get_interpolated_gradient(self, real_data, fake_data)

    # norm
    # gradients_norm = torch.sqrt(torch.sum(gradients ** 2, dim=1) + 1e-12)
    gradients_norm = gradients.norm(2, dim=1)

    # max FIXME why  **2 ????
    gradient_penalty = (torch.max(gradients_norm - 1, torch.zeros_like(gradients_norm)) ** 2).mean()
    # ? gradient_penalty = torch.nn.ReLU()(gradients_norm - 1, torch.zeros_like(gradients_norm))**2).mean()

    return gradient_penalty

def gradient_penalty_linf_hinge(self, real_data, fake_data):
    """
    Jolicoeur2019 (TEST)
    """

    # gradient
    gradients = get_interpolated_gradient(self, real_data, fake_data)

    # L_inf margin
    gradients_norm = gradients.norm(1, dim=1)

    # hinge
    gradient_penalty = torch.nn.ReLU()(gradients_norm - 1).mean()

    return gradient_penalty

def gradient_penalty_linf_hinge_abs(self, real_data, fake_data):
    """
    Jolicoeur2019 (TEST), idem with abs
    """

    # gradient
    gradients = get_interpolated_gradient(self, real_data, fake_data)

    # abs gradients
    gradients = torch.abs(gradients)

    # L_inf margin
    gradients_norm = gradients.norm(1, dim=1)

    # hinge
    gradient_penalty = torch.nn.ReLU()(gradients_norm - 1).mean()

    return gradient_penalty

def GP_L1_LS(self, real_data, fake_data):
    # gradient
    gradients = get_interpolated_gradient(self, real_data, fake_data)
    # norm
    gradients_norm = gradients.norm(1, dim=1)
    # penalty
    gradient_penalty = (gradients_norm - 1).pow(2)
    return gradient_penalty.mean()

def GP_L2_LS(self, real_data, fake_data):
    # Gulrajani2017 $(||\nabla_a D(a)||_2 - 1)^2$
    # gradient
    gradients = get_interpolated_gradient(self, real_data, fake_data)
    # norm
    gradients_norm = gradients.norm(2, dim=1)
    # penalty
    gradient_penalty = (gradients_norm - 1).pow(2)
    return gradient_penalty.mean()

def GP_Linf_LS(self, real_data, fake_data):
    # gradient
    gradients = get_interpolated_gradient(self, real_data, fake_data)
    # norm
    gradients_norm = gradients.norm(p=float("inf"), dim=1)
    # inf norm is defined as max(sum(abs(x), axis=1)) for matrix and max(abs(x)) for vector
    # https://github.com/pytorch/pytorch/issues/24802
    # grad_abs = torch.abs(grad)  # Absolute value of gradient
    # grad_norm, _ = torch.max(grad_abs, 1)
    # penalty
    gradient_penalty = (gradients_norm - 1).pow(2)
    return gradient_penalty.mean()

def GP_L1_Hinge(self, real_data, fake_data):
    # gradient
    gradients = get_interpolated_gradient(self, real_data, fake_data)
    # norm
    gradients_norm = gradients.norm(1, dim=1)
    # penalty
    gradient_penalty = torch.nn.ReLU()(gradients_norm - 1)
    return gradient_penalty.mean()

def GP_L2_Hinge(self, real_data, fake_data):
    # Gulrajani2017 $(||\nabla_a D(a)||_2 - 1)^2$
    # gradient
    gradients = get_interpolated_gradient(self, real_data, fake_data)
    # norm
    gradients_norm = gradients.norm(2, dim=1)
    # penalty
    gradient_penalty = torch.nn.ReLU()(gradients_norm - 1)
    return gradient_penalty.mean()

def GP_Linf_Hinge(self, real_data, fake_data):
    # gradient
    gradients = get_interpolated_gradient(self, real_data, fake_data)
    # norm
    gradients_norm = gradients.norm(p=float("inf"), dim=1)
    # inf norm is defined as max(sum(abs(x), axis=1)) for matrix and max(abs(x)) for vector
    # https://github.com/pytorch/pytorch/issues/24802
    # grad_abs = torch.abs(grad)  # Absolute value of gradient
    # grad_norm, _ = torch.max(grad_abs, 1)
    # penalty
    gradient_penalty = torch.nn.ReLU()(gradients_norm - 1)
    return gradient_penalty.mean()

def GP_0GP(self, real_data, fake_data):
    # thanh-tung2019, zero-centered
    # gradient
    gradients = get_interpolated_gradient(self, real_data, fake_data)
    # norm
    gradients_norm = gradients.norm(2, dim=1)
    # penalty
    gradient_penalty = (gradients_norm).pow(2)
    return gradient_penalty.mean()

def GP_SquareHinge(self, real_data, fake_data):
    # petzka2018, square hinge
    # gradient
    gradients = get_interpolated_gradient(self, real_data, fake_data)
    # norm
    gradients_norm = gradients.norm(2, dim=1)
    # penalty
    gradient_penalty = (torch.nn.ReLU()(gradients_norm - 1)).pow(2)
    return gradient_penalty.mean()

def langevin_latent_sampling(G, D, params, z):
    print('generate with langevin')
    n = len(z)
    print('size', n)

    """
    line 85 run_synthetic.py
    line 49 langevin.py
    https://github.com/clear-nus/DGflow/blob/main/dgflow.py
    """
    # params
    n_steps = params['lgs_nb_steps']
    eta = params['lgs_lr']
    gamma = params['lgs_gamma']
    f = params['lgs_f']

    # parameters (will be in params)
    # n_steps = 20  # 50000 in che2020 ? ; 25 in ansari2020
    # alpha = 1.0  # for rejection
    # step_lr = 1e-4  ## see eta
    # eps_std = 1e-2
    # eta = 0.1  # step_lr = step_size
    # f = 'KL'  # KL logD JS ; used in "velocity"
    # gamma = 0.02  # diffusion term

    fn = None
    if f == 'KL':
        fn = lambda d_score: torch.ones_like(d_score.detach())
    if f == 'logD':
        fn = lambda d_score: 1 / (1 + d_score.detach().exp())
    if f == 'JS':
        fn = lambda d_score: 1 / (1 + 1 / d_score.detach().exp())
    if not fn:
        print('ERROR lgs_f must be KL, LogD or JS')
        return z

    noise_factor = np.sqrt(gamma)
    print('langevin nb steps ', n_steps)
    print('langevin lr       ', eta)
    print('langevin gamma    ', gamma)
    print('langevin f        ', f)

    """
    KL-divergence by setting f = r log r
    ==> equivalent to DDLS Che2020 ? NO
    s = torch.ones_like(d_score.detach())

    f == 'logD' => s = 1 / (1 + d_score.detach().exp())
    Discriminator Optimal Transport (DOT) (Tanaka, 2019) ; with λ = 1/2 ? unsure
    NO it is different

    f == 'JS' => s = 1 / (1 + 1 / d_score.detach().exp())
    proposed by ansari2020 ?

    DGflow performs well even without the diffusion term (i.e., with γ = 0)
    """

    def velocity(z):
        z_t = z.clone()
        z_t.requires_grad_(True)
        if z_t.grad is not None:
            z_t.grad.zero_()
        x_t = G(z_t)
        d_score = D(x_t)
        # s = torch.ones_like(d_score.detach())
        # s = 1 / (1 + d_score.detach().exp())
        # s = 1 / (1 + 1 / d_score.detach().exp())  # FIXME use f
        s = fn(d_score)
        s.expand_as(z_t)
        d_score.backward(torch.ones_like(d_score).to(z.device))
        grad = z_t.grad
        return s.data * grad.data

    # loop on number of steps
    for i in range(n_steps):
        print(i, len(z))
        v = velocity(z)
        # FIXME warning randn !
        z = z.data + eta * v + np.sqrt(2 * eta) * noise_factor * torch.randn_like(z)
        """
            v = velocity(z)  <--- grad ?
            z = z.data + eta * v + np.sqrt(2*eta) * noise_factor * torch.randn_like(z)
        """
        """
            eps = eps_std * xp.random.randn(batch_size, z_dim).astype(xp.float32)
            E, grad = e_grad(z, P, gen, dis, alpha, ret_e=True)
            z = z - step_lr * grad[0] + eps
        """

    return z

max_stars_repo_name: dsarrut/gaga-phsp

max_issues_repo_name: dsarrut/gaga-phsp

max_forks_repo_name: dsarrut/gaga-phsp


text:
import numpy as np
import tensorflow as tf
from typing import Union, Callable, List

def crf(inputs, tag_indices, num_labels, true_sequence_lengths,
    transitions_name="transitions", inference_only=False):
  """ Performs tensorflow crf decoding.
  Args:
    inputs: A [batch_size, max_seq_len, num_tags] tensor of unary
        potentials to use as input to the CRF layer.
    tag_indices: A [batch_size, max_seq_len] matrix of tag indices
        for which we compute the log-likelihood.
    num_labels: A int number indicates the number of possible tags.
    true_sequence_lengths: A [batch_size] vector of true sequence lengths.
  Returns:
    per_example_loss: A [batch_size] Tensor containing the negative
        log-likelihood of each example, given the sequence of tag indices.
    predictions: A [batch_size, max_seq_len] CRF decode_tags represent
        the most probable tags sequence.
    best_score: A [batch_size] vector, containing the score of decode_tags.
  """
  with tf.variable_scope('crf'):
    transition_params = tf.get_variable(
      transitions_name,
      shape=[num_labels, num_labels],
      initializer=tf.zeros_initializer()
    )
  per_example_loss = None
  if not inference_only:
    log_likelihood, transition_params = tf.contrib.crf.crf_log_likelihood(
      inputs=inputs,
      tag_indices=tag_indices,
      transition_params=transition_params,
      sequence_lengths=true_sequence_lengths
    )
    per_example_loss = -log_likelihood
  # NOTE CRF decode, decode_tags [batch_size, max_seq_len] most probable path
  decode_tags, best_score = tf.contrib.crf.crf_decode(
    potentials=inputs,
    transition_params=transition_params,
    # NOTE sequence_length: [batch_size] vector of true sequence lengths.
    sequence_length=true_sequence_lengths
  )
  # A [batch_size] Tensor containing the -log_likelihood of each example
  predictions = decode_tags
  return per_example_loss, predictions, best_score

def softmax(logits, labels, num_classes, mask=None, label_smoothing=0):
  """ Perform softmax operation with optional label smoothing.
  Args:
    logits: Logits outputs of the network, last dimension should be num_classes,
        ie.. [batch_size, seq_length ?, num_classes];
    labels: A [batch_size, seq_length ?] tensor represent true label ids.
    mask: mask should be the same shape as logits.
  Return:
    per_example_loss, a [batch_size] tensor containing the cross_entropy loss.
  """
  # # shape: batch x features_tokens x depth if axis==-1, same shape as logits
  one_hot_labels = tf.one_hot(labels, depth=num_classes, dtype=tf.float32)
  # # shape: batch x features_tokens
  if label_smoothing > 0:
    smooth_positives = 1.0 - label_smoothing
    smooth_negatives = label_smoothing / num_classes
    one_hot_labels = one_hot_labels * smooth_positives + smooth_negatives

  # An alternative way, less efficient, but allow self-define mask
  log_probs = tf.nn.log_softmax(logits, axis=-1) # NOTE same shape as logits
  # NOTE shape (batch_size, max_seq_len, depth)
  per_token_loss = one_hot_labels * log_probs

  if mask is not None and len(per_token_loss.shape.as_list()) > 2:
    mask = tf.cast(mask, tf.float32)
    per_token_loss = tf.einsum("BTH,BT->BTH", per_token_loss, mask)

  per_example_loss = -tf.reduce_sum(per_token_loss, axis=-1)

  probabilities = tf.nn.softmax(logits, axis=-1) # NOTE same shape as logits
  predictions = tf.argmax(probabilities, axis=-1, output_type=tf.int64)

  return per_example_loss, predictions

def am_softmax(logits, labels, num_classes, scale=30, margin=0.35):
  """ Addictive angular softmax, which defers from regular softmax for:
  1. Perform L2 normalization on output layer's input and weight,
      the logits, i.e. dot product between input and weight, become COS.
  2. COS value minus a constant positive margin for positive label,
      then scale by factor scale.
  """
  with tf.name_scope('am_softmax'):
    # shape: batch x features_tokens x depth if axis == -1, same shape as logits
    one_hot_labels = tf.one_hot(labels, depth=num_classes, dtype=tf.float32)
    logits = one_hot_labels * (logits - margin) + (1 - one_hot_labels) * logits
    logits *= scale
    # shape: batch x features_tokens
    per_token_loss = tf.nn.softmax_cross_entropy_with_logits(logits=logits, labels=one_hot_labels)
    probabilities = tf.nn.softmax(logits, axis=-1) # NOTE same shape as logits
    predictions = tf.argmax(probabilities, axis=-1, output_type=tf.int64)
    return per_token_loss, predictions

def contrastive_loss(model1, model2, y, margin):
  """ Pairwise contrasting two data points representations """
	with tf.name_scope("contrastive-loss"):
		distance = tf.sqrt(tf.reduce_sum(tf.pow(model1 - model2, 2), 1, keepdims=True))
		similarity = y * tf.square(distance) # keep the similar label (1) close to each other
		dissimilarity = (1 - y) * tf.square(tf.maximum((margin - distance), 0))
    # give penalty to dissimilar label if the distance is bigger than margin
		return tf.reduce_mean(dissimilarity + similarity) / 2

from tensorflow.python.ops import array_ops

def focal_loss(logits, targets, weights=None, alpha=0.25, gamma=2):
  r"""Compute focal loss for predictions.
      Multi-labels Focal loss formula:
          FL = -alpha * (z-p)^gamma * log(p) -(1-alpha) * p^gamma * log(1-p)
                ,which alpha = 0.25, gamma = 2, p = sigmoid(x), z = targets.
  Args:
    logits: A float tensor of shape [batch_size, num_anchors,
      num_classes] representing the predicted logits for each class
    targets: A float tensor of shape [batch_size, num_anchors,
      num_classes] representing one-hot encoded classification targets
    weights: A float tensor of shape [batch_size, num_anchors]
    alpha: A scalar tensor for focal loss alpha hyper-parameter
    gamma: A scalar tensor for focal loss gamma hyper-parameter
  Returns:
      loss: A (scalar) tensor representing the value of the loss function for each samples
  """
  sigmoid_p = tf.nn.sigmoid(logits)
  zeros = array_ops.zeros_like(sigmoid_p, dtype=sigmoid_p.dtype)

  # For poitive prediction, only need consider front part loss, back part is 0;
  # targets > zeros <=> z=1, so poitive coefficient = z - p.
  pos_p_sub = array_ops.where(targets > zeros, targets - sigmoid_p, zeros)

  # For negative prediction, only need consider back part loss, front part is 0;
  # targets > zeros <=> z=1, so negative coefficient = 0.
  neg_p_sub = array_ops.where(targets > zeros, zeros, sigmoid_p)
  per_entry_cross_ent = - alpha * (pos_p_sub ** gamma) * tf.log(tf.clip_by_value(sigmoid_p, 1e-8, 1.0)) \
                        - (1 - alpha) * (neg_p_sub ** gamma) * tf.log(tf.clip_by_value(1.0 - sigmoid_p, 1e-8, 1.0))
  return tf.reduce_sum(per_entry_cross_ent, axis=-1)

Number = Union[
  float,
  int,
  np.float16,
  np.float32,
  np.float64,
  np.int8,
  np.int16,
  np.int32,
  np.int64,
  np.uint8,
  np.uint16,
  np.uint32,
  np.uint64,
]

TensorLike = Union[
  List[Union[Number, list]],
  tuple,
  Number,
  np.ndarray,
  tf.Tensor,
  tf.SparseTensor,
  tf.Variable,
]

FloatTensorLike = Union[tf.Tensor, float, np.float16, np.float32, np.float64]

def sigmoid_focal_crossentropy(
        y_true: TensorLike,
        y_pred: TensorLike,
        alpha: FloatTensorLike = 0.25,
        gamma: FloatTensorLike = 2.0,
        from_logits: bool = False,
        mask: TensorLike = None,
) -> tf.Tensor:
  """Implements the focal loss function.
  Focal loss was first introduced in the RetinaNet paper
  (https://arxiv.org/pdf/1708.02002.pdf). Focal loss is extremely useful for
  classification when you have highly imbalanced classes. It down-weights
  well-classified examples and focuses on hard examples. The loss value is
  much high for a sample which is misclassified by the classifier as compared
  to the loss value corresponding to a well-classified example. One of the
  best use-cases of focal loss is its usage in object detection where the
  imbalance between the background class and other classes is extremely high.
  Args
      y_true: true targets tensor.
      y_pred: predictions tensor.
      alpha: balancing factor.
      gamma: modulating factor.
  Returns:
      Weighted loss float `Tensor`. If `reduction` is `NONE`,this has the
      same shape as `y_true`; otherwise, it is scalar.
  """
  import tensorflow.keras.backend as K
  if gamma and gamma < 0:
      raise ValueError("Value of gamma should be greater than or equal to zero")

  y_pred = tf.convert_to_tensor(y_pred)
  y_true = tf.convert_to_tensor(y_true, dtype=y_pred.dtype)

  # Get the cross_entropy for each entry
  ce = K.binary_crossentropy(y_true, y_pred, from_logits=from_logits)

  # If logits are provided then convert the predictions into probabilities
  if from_logits:
      pred_prob = tf.sigmoid(y_pred)
  else:
      pred_prob = y_pred

  p_t = (y_true * pred_prob) + ((1 - y_true) * (1 - pred_prob))
  alpha_factor = 1.0
  modulating_factor = 1.0

  if alpha:
      alpha = tf.convert_to_tensor(alpha, dtype=K.floatx())
      alpha_factor = y_true * alpha + (1 - y_true) * (1 - alpha)

  if gamma:
      gamma = tf.convert_to_tensor(gamma, dtype=K.floatx())
      modulating_factor = tf.pow((1.0 - p_t), gamma)

  # compute the final loss and return
  per_entry_cross_ent = alpha_factor * modulating_factor * ce
  if mask is not None:
      mask = tf.cast(mask, tf.float32)
      per_entry_cross_ent = tf.einsum("BFN,BF->BFN", per_entry_cross_ent, mask)

  return tf.reduce_sum(per_entry_cross_ent, axis=-1)

max_stars_repo_name: congchan/nnnlp

max_issues_repo_name: congchan/nnnlp

max_forks_repo_name: congchan/nnnlp


text:
import logging

import pungi.agents.qlearning.qlearning as qlearning
import pungi.config as conf
import pungi.environment.backend as backend
import pungi.environment.state as state

logger = logging.getLogger('root')

import gym

from gym import spaces
import numpy as np

class SnakeEnv(gym.Env):
    metadata = {'render.modes': ['human']}

    def __init__(self):
        self.current_game_id = None
        self.observation_space = spaces.Box(0,
                                            3,
                                            (int(conf.CONF.get_value("board_width")),
                                             int(conf.CONF.get_value("board_height"))))
        self.action_space = spaces.Discrete(4)
        self._ACTION_TO_STRING = {0: "left", 1: "right", 2: "up", 3: "down"}

    def step(self, action):
        action_to_string = self._ACTION_TO_STRING.get(action, action)
        next_state = backend.make_move(action_to_string, self.current_game_id)
        reward = self.get_reward(next_state)
        game_over = next_state["game-over"]
        info = {"score": next_state["score"]}
        return np.array(next_state["board"], dtype=np.int8) * (1 / 3), reward, game_over, info

    def reset(self):
        self.current_game_id = backend.register_new_game()
        game_info = backend.get_game_info(self.current_game_id)
        return np.array(game_info["board"], dtype=np.int8) * (1 / 3)

    def render(self, mode='human', close=False):
        pass  # Here one can log human readable state of the environment

    def close(self):
        pass

    @staticmethod
    def get_reward(game_state):
        if game_state["game-over"]:
            return float(conf.CONF.get_value("game_over_reward"))
        elif game_state["ate-food"]:
            return float(conf.CONF.get_value("ate_food_reward"))
        else:
            return float(conf.CONF.get_value("snake_moved_reward"))

class PartialInformationSnakeEnv(SnakeEnv):
    def step(self, action):
        next_state, reward, done, info = super().step(action)
        return np.array(state.extract_head_and_food(next_state)), reward, done, info

    def reset(self):
        return np.array(state.extract_head_and_food(super().reset()))

max_stars_repo_name: DiscoverAI/pungi

max_issues_repo_name: DiscoverAI/pungi

max_forks_repo_name: DiscoverAI/pungi


text:
#!/usr/bin/env python3
#
# Authors: Enrico Sutera

# This node is responsible for intefacing with gazebo:
# Sensors reading
# Velocities communication
# Reset of enviroment
# Environment changes

import os
import random
import sys
import time

from gazebo_msgs.srv import DeleteEntity
from gazebo_msgs.srv import SpawnEntity
from geometry_msgs.msg import Pose
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from rclpy.qos import qos_profile_sensor_data
from std_srvs.srv import Empty
from pic4rl_msgs.srv import State, Reset
import numpy, math

#sensors
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan

#from omnirob_sensors.Sensor import OdomSensor, LaserScanSensor
#from rclpy.executors import SingleThreadedExecutor

class Pic4rlGazebo(Node):
    def __init__(self):
        super().__init__('pic4rl_gazebo')
        self.get_logger().info("Beginning initialization...")
        rclpy.logging.set_logger_level('pic4rl_gazebo', 10)
        """************************************************************
        ** Initialise ROS publishers and subscribers
        ************************************************************"""
        qos = QoSProfile(depth=10)

        # Initialise publishers
        self.cmd_vel_pub = self.create_publisher(
            Twist,
            'cmd_vel',
            qos)

        # Initialise subscribers
        self.odom_sub = self.create_subscription(
            Odometry,
            'odom',
            self.odom_callback,
            qos)
        self.scan_sub = self.create_subscription(
            LaserScan,
            'scan',
            self.scan_callback,
            qos_profile=qos_profile_sensor_data)

        """************************************************************
        ** Initialise ROS publishers and subscribers
        ************************************************************"""
        # Initialise client
        self.delete_entity_client = self.create_client(DeleteEntity, 'delete_entity')
        self.spawn_entity_client = self.create_client(SpawnEntity, 'spawn_entity')
        self.reset_simulation_client = self.create_client(Empty, 'reset_simulation')
        self.reset_world_client = self.create_client(Empty, 'reset_world')
        self.pause_physics_client = self.create_client(Empty, 'pause_physics')
        self.unpause_physics_client = self.create_client(Empty, 'unpause_physics')

        # Initialise servers
        self.get_logger().info("Creating get_state service...")
        self.get_state_server = self.create_service(State, 'get_state', self.get_state_callback)
        self.get_logger().info("Creating new_episode service...")
        self.new_episode_server = self.create_service(Reset, 'new_episode', self.new_episode_callback)
        #self.get_logger().info("Creating send_twist service...")
        #self.send_twist = self.create_service(Empty, 'send_twist', self.pub_twist_callback)
        #rclpy.logging.initialize()
        #rclpy.logging.get_logger("init_finisd")

        self.get_logger().info("Init finished.")

    """##############################
    Sensors related callbacks
    ##############################"""

    def odom_callback(self, msg):
        #self.get_logger().debug("Receiving odom ...")
        self.odom = msg

    def odom_callback_tb3(self, msg):
        self.last_pose_x = msg.pose.pose.position.x
        self.last_pose_y = msg.pose.pose.position.y
        _, _, self.last_pose_theta = self.euler_from_quaternion(msg.pose.pose.orientation)

        goal_distance = math.sqrt(
            (self.goal_pose_x-self.last_pose_x)**2
            + (self.goal_pose_y-self.last_pose_y)**2)

        path_theta = math.atan2(
            self.goal_pose_y-self.last_pose_y,
            self.goal_pose_x-self.last_pose_x)

        goal_angle = path_theta - self.last_pose_theta
        if goal_angle > math.pi:
            goal_angle -= 2 * math.pi

        elif goal_angle < -math.pi:
            goal_angle += 2 * math.pi

        self.goal_distance = goal_distance
        self.goal_angle = goal_angle

    def euler_from_quaternion(self, quat):
        """
        Converts quaternion (w in last place) to euler roll, pitch, yaw
        quat = [x, y, z, w]
        """
        x = quat.x
        y = quat.y
        z = quat.z
        w = quat.w

        sinr_cosp = 2 * (w*x + y*z)
        cosr_cosp = 1 - 2*(x*x + y*y)
        roll = numpy.arctan2(sinr_cosp, cosr_cosp)

        sinp = 2 * (w*y - z*x)
        pitch = numpy.arcsin(sinp)

        siny_cosp = 2 * (w*z + x*y)
        cosy_cosp = 1 - 2 * (y*y + z*z)
        yaw = numpy.arctan2(siny_cosp, cosy_cosp)

        return roll, pitch, yaw

    def scan_callback(self, msg):
        #self.get_logger().debug("Receiving scan ...")
        self.scan = msg

    """##############################
    Communication with environment callbacks
    ##############################"""

    def get_state_callback(self,request,response):
        self.get_logger().debug("get state callback request received...")
        response.data_received = True

        try:
            response.scan = self.scan
        except Exception as e:
            self.get_logger().debug("Scan data not available yet")
            response.data_received = False
        try:
            response.odom = self.odom
        except Exception as e:
            self.get_logger().debug("Odom data not available yet")
            response.data_received = False

        self.get_logger().debug("sending state...")
        return response

    def new_episode_callback(self,request, response):
        self.get_logger().debug("Reset request received ...")

        self.get_logger().debug("Clearing variables ...")
        self.clear_variables()

        #self.get_logger().debug("Respawing robot ...")
        #self.respawn_robot()

        #self.get_logger().debug("Resetting simulation ...")
        #self.reset_simulation()

        self.get_logger().debug("Resetting world ...")
        self.reset_world()

        self.get_logger().debug("Respawing goal ...")
        self.respawn_entity(request.goal_pos_x, request.goal_pos_y)

        #self.get_logger().debug("Resetting variables ...")
        #self.reset_variables()

        self.get_logger().debug("Environment reset performed ...")
        response.success = True
        return response

    """##############################
    Auxiliar functions
    ##############################"""
    def clear_variables(self):
        try:
            del self.odom
        except:
            pass
        try:
            del self.scan
        except:
            pass

    def reset_variables(self):
        self.get_logger().debug("Un pausing for resetting variables ...")
        self.unpause()
        while True:
            try:
                #self.get_logger().debug("Checking variables ...")
                self.odom
                self.get_logger().debug("Odom is ok ...")
                self.scan
                self.get_logger().debug("Scan is ok ...")
                break
            except:
                rclpy.spin_once(self)
                self.get_logger().debug("Spinning once.")
                time.sleep(0.05)
        self.pause()

    """##############################
    Gazebo services
    ##############################"""

    # currently not used
    def respawn_robot(self):
        #to be fixed. It gives error when deleting the model for the eaerly  destrucion of nodes.s
        #print("pausing...")
        #self.pause()
        self.get_logger().debug("deleting entity...")
        self.delete_entity('omnirob')
        self.get_logger().debug("respawning entity...")
        entity_path='/home/enricosutera/ros_2_workspace/src/omnirob/omnirob_simulation/models/omnirob/model.sdf'
        initial_pose = Pose()
        #initial_pose.position.x = random.uniform(-3,3)
        #initial_pose.position.y = random.uniform(-3,3)
        self.spawn_entity(initial_pose,'omnirob',entity_path)
        #print("unpausing...")
        #self.unpause()
        #print("unpaused.")
        #time.sleep(1)

    def respawn_entity(self, goal_pose_x, goal_pose_y):

        #Goal initialization
        # Entity 'goal'
        self.entity_dir_path = os.path.dirname(os.path.realpath(__file__))
        #print(self.entity_dir_path)
        #self.entity_dir_path = self.entity_dir_path.replace(
        #    'omnirob_rl/omnirob_rl',
        #    'omnirob_simulation/models/goal_box')
        self.entity_dir_path = self.entity_dir_path.replace(
            'pic4rl/pic4rl/pic4rl',
            'pic4rl/pic4rl/models/goal_box')
        self.entity_path = os.path.join(self.entity_dir_path, 'model.sdf')
        self.entity = open(self.entity_path, 'r').read()
        self.entity_name = 'goal'

        self.get_logger().debug("deleting entity...")
        try:
            self.delete_entity('goal')
        except:
            pass
        self.get_logger().debug("respawning entity...")
        entity_path=self.entity_path
        initial_pose = Pose()
        initial_pose.position.x = goal_pose_x
        initial_pose.position.y = goal_pose_y
        self.spawn_entity(initial_pose,self.entity_name,entity_path)
        #print("unpausing...")
        #self.unpause()
        #print("unpaused.")
        #time.sleep(1)

    def reset_world(self):
        req = Empty.Request()
        while not self.reset_world_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.reset_world_client.call_async(req)
        #time.sleep(1)

    # currently not used
    def reset_simulation(self):
        #self.pause()
        req = Empty.Request()
        while not self.reset_simulation_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.reset_simulation_client.call_async(req)
        #self.unpause()

    def delete_entity(self, entity_name):
        req = DeleteEntity.Request()
        #req.name = self.entity_name
        req.name = entity_name
        while not self.delete_entity_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')

        self.delete_entity_client.call_async(req)
        self.get_logger().debug('Entity deleting request sent ...')

    def spawn_entity(self,pose = None, name = None, entity_path = None, entity = None):
        if not pose:
            pose = Pose()
        req = SpawnEntity.Request()
        req.name = name
        if entity_path:
            entity = open(entity_path, 'r').read()
        req.xml = entity
        req.initial_pose = pose
        while not self.spawn_entity_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.spawn_entity_client.call_async(req)

    # currently not used
    def pub_twist_callback(self, twist=Twist()):
        """self.get_logger().info("pub")
        self.unpause
        self.cmd_vel_pub.publish(twist)
        time.sleep(0.1)
        self.pause
        return
        """
        pass

    def pause(self):
        req = Empty.Request()
        while not self.pause_physics_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.pause_physics_client.call_async(req)

    def unpause(self):
        req = Empty.Request()
        while not self.unpause_physics_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.unpause_physics_client.call_async(req)

def main(args=None):
    rclpy.init()
    pic4rl_gazebo = Pic4rlGazebo()
    #executor = SingleThreadedExecutor()
    #executor.add_node(omnirob_gazebo)
    #executor.add_node(omnirob_gazebo.lidar_node)
    #executor.add_node(omnirob_gazebo.odom_node)

    #executor.spin()
    pic4rl_gazebo.get_logger().info("Spinning ...")
    rclpy.spin(pic4rl_gazebo)

    pic4rl_gazebo.get_logger().info("Stop spinning ...")
    pic4rl_gazebo.destroy()
    rclpy.shutdown()

if __name__ == '__main__':
    main()


text:
import numpy as np
from scipy import signal
from golem import DataSet
from markers import resample_markers

def ewma_filter(alpha):
  '''
  Filter coefficients for a recursive exponentially weighted moving average
  '''
  alpha = float(alpha)
  assert 0 <= alpha <= 1
  b, a = [alpha], [1, -(1.-alpha)]
  return b, a

def ewma(x, alpha, v0=0):
  '''
  Causal exponential moving average implemented using scipy.signal.lfilter.
  With alpha as the forgetting factor close to one, x the signal to filter.
  Optionally, an initial estimate can be provided with the float v0.
  '''
  b, a = ewma_filter(alpha)
  x = np.atleast_1d(x).flatten()
  v0 = float(v0)

  zi = signal.lfiltic(b, a, [v0])
  return signal.lfilter(b, a, x, zi=zi)[0]

def ma(x, n):
  '''Causal moving average filter, with signal x, and window-length n.'''
  n = int(n)
  return np.convolve(x, np.ones(n)/n)[:x.size]

def filtfilt_rec(d, (b, a)):
  '''
  Apply a filter defined by the filter coefficients (b, a) to a
  DataSet, *forwards and backwards*.
  d.xs is interpreted as [frames x channels].
  '''
  return DataSet(xs=np.apply_along_axis(lambda x: signal.filtfilt(b, a, x), 0,
    d.xs), default=d)

def resample_rec(d, factor, max_marker_delay=0):
  '''Resample a recording to length d.ninstances * factor'''
  new_len = int(d.ninstances * factor)
  ys = resample_markers(d.ys.flatten(), new_len,
    max_delay=max_marker_delay).reshape(-1, 1)

  # calculate xs and ids
  xs, ids = signal.resample(d.xs, new_len, t=d.ids)
  xs = xs.astype(d.xs.dtype) # keep old dtype

  # construct new DataSet
  extra = d.extra.copy()
  return DataSet(xs=xs, ys=ys, ids=ids.reshape(-1, 1), extra=extra, default=d)

def decimate_rec(d, factor, max_marker_delay=0):
  '''Decimate a recording using an anti-aliasing filter.'''
  assert isinstance(factor, int), 'Decimation factor should be an int'

  # anti-aliasing filter
  (b, a) = signal.iirfilter(8, .8 / factor, btype='lowpass', rp=0.05,
    ftype='cheby1')
  xs = d.xs.copy()
  for i in range(d.nfeatures):
    xs[:,i] = signal.filtfilt(b, a, xs[:, i])

  xs = np.ascontiguousarray(xs[::factor,:]).astype(d.xs.dtype)
  ys = resample_markers(d.ys.flatten(), xs.shape[0],
    max_delay=max_marker_delay).reshape(-1, 1)
  ids = np.ascontiguousarray(d.ids[::factor,:]).astype(d.ids.dtype)

  # construct new DataSet
  return DataSet(xs=xs, ys=ys, ids=ids.reshape(-1, 1), default=d)

max_stars_repo_name: breuderink/psychic

max_issues_repo_name: breuderink/psychic

max_forks_repo_name: breuderink/psychic


text:
#! /usr/bin/env python3

import numpy as np
import hello_helpers.hello_misc as hm
from hello_helpers.simple_command_group import SimpleCommandGroup
from hello_helpers.gripper_conversion import GripperConversion

class HeadPanCommandGroup(SimpleCommandGroup):
    def __init__(self, range_rad=None, calibrated_offset_rad=None, calibrated_looked_left_offset_rad=None, node=None):
        SimpleCommandGroup.__init__(self, 'joint_head_pan', range_rad, acceptable_joint_error=0.15, node=node)
        if calibrated_offset_rad is None:
            calibrated_offset_rad = node.controller_parameters['pan_angle_offset']
        self.calibrated_offset_rad = calibrated_offset_rad
        if calibrated_looked_left_offset_rad is None:
            calibrated_looked_left_offset_rad = node.controller_parameters['pan_looked_left_offset']
        self.looked_left_offset_rad = calibrated_looked_left_offset_rad
        self.looked_left = False

    def update_joint_range(self, joint_range, node=None):
        if joint_range is not None:
            self.range = joint_range
            return

        if node is None:
            return # cannot calculate range without Stretch Body handle
        range_ticks = node.robot.head.motors['head_pan'].params['range_t']
        range_rad = (node.robot.head.motors['head_pan'].ticks_to_world_rad(range_ticks[1]),
                     node.robot.head.motors['head_pan'].ticks_to_world_rad(range_ticks[0]))
        self.range = range_rad

    def init_execution(self, robot, robot_status, **kwargs):
        if self.active:
            _, pan_error = self.update_execution(robot_status)
            robot.head.move_by('head_pan', pan_error, v_r=self.goal['velocity'], a_r=self.goal['acceleration'])
            self.looked_left = pan_error > 0.0

    def update_execution(self, robot_status, **kwargs):
        self.error = None
        if self.active:
            pan_backlash_correction = self.looked_left_offset_rad if self.looked_left else 0.0
            pan_current = robot_status['head']['head_pan']['pos'] + self.calibrated_offset_rad + pan_backlash_correction
            self.error = self.goal['position'] - pan_current
            return self.name, self.error

        return None

    def joint_state(self, robot_status, **kwargs):
        pan_status = robot_status['head']['head_pan']
        pan_backlash_correction = self.looked_left_offset_rad if self.looked_left else 0.0
        pos = pan_status['pos'] + self.calibrated_offset_rad + pan_backlash_correction
        return (pos, pan_status['vel'], pan_status['effort'])

class HeadTiltCommandGroup(SimpleCommandGroup):
    def __init__(self, range_rad=None, calibrated_offset_rad=None, calibrated_looking_up_offset_rad=None, backlash_transition_angle_rad=None, node=None):
        SimpleCommandGroup.__init__(self, 'joint_head_tilt', range_rad, acceptable_joint_error=0.52, node=node)
        if calibrated_offset_rad is None:
            calibrated_offset_rad = node.controller_parameters['tilt_angle_offset']
        self.calibrated_offset_rad = calibrated_offset_rad
        if calibrated_looking_up_offset_rad is None:
            calibrated_looking_up_offset_rad = node.controller_parameters['tilt_looking_up_offset']
        self.looking_up_offset_rad = calibrated_looking_up_offset_rad
        if backlash_transition_angle_rad is None:
            backlash_transition_angle_rad = node.controller_parameters['tilt_angle_backlash_transition']
        self.backlash_transition_angle_rad = backlash_transition_angle_rad
        self.looking_up = False

    def update_joint_range(self, joint_range, node=None):
        if joint_range is not None:
            self.range = joint_range
            return

        if node is None:
            return # cannot calculate range without Stretch Body handle
        range_ticks = node.robot.head.motors['head_tilt'].params['range_t']
        range_rad = (node.robot.head.motors['head_tilt'].ticks_to_world_rad(range_ticks[1]),
                     node.robot.head.motors['head_tilt'].ticks_to_world_rad(range_ticks[0]))
        self.range = range_rad

    def init_execution(self, robot, robot_status, **kwargs):
        if self.active:
            _, tilt_error = self.update_execution(robot_status)
            robot.head.move_by('head_tilt', tilt_error, v_r=self.goal['velocity'], a_r=self.goal['acceleration'])
            self.looking_up = self.goal['position'] > (self.backlash_transition_angle_rad + self.calibrated_offset_rad)

    def update_execution(self, robot_status, **kwargs):
        self.error = None
        if self.active:
            tilt_backlash_correction = self.looking_up_offset_rad if self.looking_up else 0.0
            tilt_current = robot_status['head']['head_tilt']['pos'] + self.calibrated_offset_rad + tilt_backlash_correction
            self.error = self.goal['position'] - tilt_current
            return self.name, self.error

        return None

    def joint_state(self, robot_status, **kwargs):
        tilt_status = robot_status['head']['head_tilt']
        tilt_backlash_correction = self.looking_up_offset_rad if self.looking_up else 0.0
        pos = tilt_status['pos'] + self.calibrated_offset_rad + tilt_backlash_correction
        return (pos, tilt_status['vel'], tilt_status['effort'])

class WristYawCommandGroup(SimpleCommandGroup):
    def __init__(self, range_rad=None, node=None):
        SimpleCommandGroup.__init__(self, 'joint_wrist_yaw', range_rad, node=node)

    def update_joint_range(self, joint_range, node=None):
        if joint_range is not None:
            self.range = joint_range
            return

        if node is None:
            return # cannot calculate range without Stretch Body handle
        range_ticks = node.robot.end_of_arm.motors['wrist_yaw'].params['range_t']
        range_rad = (node.robot.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad(range_ticks[1]),
                     node.robot.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad(range_ticks[0]))
        self.range = range_rad

    def init_execution(self, robot, robot_status, **kwargs):
        if self.active:
            robot.end_of_arm.move_by('wrist_yaw',
                                     self.update_execution(robot_status)[1],
                                     v_r=self.goal['velocity'],
                                     a_r=self.goal['acceleration'])

    def update_execution(self, robot_status, **kwargs):
        self.error = None
        if self.active:
            self.error = self.goal['position'] - robot_status['end_of_arm']['wrist_yaw']['pos']
            return self.name, self.error

        return None

    def joint_state(self, robot_status, **kwargs):
        yaw_status = robot_status['end_of_arm']['wrist_yaw']
        return (yaw_status['pos'], yaw_status['vel'], yaw_status['effort'])

class GripperCommandGroup(SimpleCommandGroup):
    def __init__(self, range_robotis=None, node=None):
        self.gripper_conversion = GripperConversion()
        SimpleCommandGroup.__init__(self, 'joint_gripper_finger_left', range_robotis, acceptable_joint_error=1.0, node=node)
        self.gripper_joint_names = ['joint_gripper_finger_left', 'joint_gripper_finger_right', 'gripper_aperture']
        self.update_joint_range(range_robotis)

    def update_joint_range(self, joint_range, node=None):
        if joint_range is not None:
            self.range = joint_range
            self.range_aperture_m = (self.gripper_conversion.robotis_to_aperture(joint_range[0]),
                                    self.gripper_conversion.robotis_to_aperture(joint_range[1]))
            self.range_finger_rad = (self.gripper_conversion.robotis_to_finger(joint_range[0]),
                                    self.gripper_conversion.robotis_to_finger(joint_range[1]))
            return

        if node is None:
            return # cannot calculate range without Stretch Body handle
        range_ticks = node.robot.end_of_arm.motors['stretch_gripper'].params['range_t']
        range_rad = (node.robot.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad(range_ticks[0]),
                     node.robot.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad(range_ticks[1]))
        range_robotis = (node.robot.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(range_rad[0]),
                         node.robot.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(range_rad[1]))
        self.range = range_robotis
        self.range_aperture_m = (self.gripper_conversion.robotis_to_aperture(range_robotis[0]),
                                 self.gripper_conversion.robotis_to_aperture(range_robotis[1]))
        self.range_finger_rad = (self.gripper_conversion.robotis_to_finger(range_robotis[0]),
                                 self.gripper_conversion.robotis_to_finger(range_robotis[1]))

    def update(self, commanded_joint_names, invalid_joints_callback, **kwargs):
        self.active = False
        self.index = None
        active_gripper_joint_names = list(set(commanded_joint_names) & set(self.gripper_joint_names))
        if len(active_gripper_joint_names) > 1:
            err_str = ("Received a command for the gripper that includes more than one gripper joint name: {0}. "
                       "Only one joint name is allowed to be used for a gripper command to avoid conflicts "
                       "and confusion. The gripper only has a single degree of freedom that can be "
                       "controlled using the following three mutually exclusive joint names: "
                       "{1}.").format(active_gripper_joint_names, self.gripper_joint_names)
            invalid_joints_callback(err_str)
            return False
        elif len(active_gripper_joint_names) == 1:
            self.name = active_gripper_joint_names[0]
            self.index = commanded_joint_names.index(self.name)
            self.active = True

        return True

    def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal, **kwargs):
        self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
        if self.active:
            goal_pos = point.positions[self.index] if len(point.positions) > self.index else None
            if goal_pos is None:
                err_str = ("Received goal point with positions array length={0}. "
                           "This joint ({1})'s index is {2}. Length of array must cover all joints listed "
                           "in commanded_joint_names.").format(len(point.positions), self.name, self.index)
                invalid_goal_callback(err_str)
                return False

            joint_range = self.range_aperture_m if (self.name == 'gripper_aperture') else self.range_finger_rad
            self.goal['position'] = hm.bound_ros_command(joint_range, goal_pos, fail_out_of_range_goal)
            self.goal['velocity'] = point.velocities[self.index] if len(point.velocities) > self.index else None
            self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None
            self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None
            if self.goal['position'] is None:
                err_str = ("Received {0} goal point that is out of bounds. "
                            "Range = {1}, but goal point = {2}.").format(self.name, joint_range, goal_pos)
                invalid_goal_callback(err_str)
                return False

        return True

    def init_execution(self, robot, robot_status, **kwargs):
        if self.active:
            _, gripper_error = self.update_execution(robot_status)
            if (self.name == 'gripper_aperture'):
                gripper_robotis_error = self.gripper_conversion.aperture_to_robotis(gripper_error)
            elif (self.name == 'joint_gripper_finger_left') or (self.name == 'joint_gripper_finger_right'):
                gripper_robotis_error = self.gripper_conversion.finger_to_robotis(gripper_error)
            robot.end_of_arm.move_by('stretch_gripper',
                                     gripper_robotis_error,
                                     v_r=self.goal['velocity'],
                                     a_r=self.goal['acceleration'])

    def update_execution(self, robot_status, **kwargs):
        self.error = None
        if self.active:
            robotis_pct = robot_status['end_of_arm']['stretch_gripper']['pos_pct']
            if (self.name == 'gripper_aperture'):
                gripper_current = self.gripper_conversion.robotis_to_aperture(robotis_pct)
            elif (self.name == 'joint_gripper_finger_left') or (self.name == 'joint_gripper_finger_right'):
                gripper_current = self.gripper_conversion.robotis_to_finger(robotis_pct)

            self.error = self.goal['position'] - gripper_current
            return self.name, self.error

        return None

    def joint_state(self, robot_status, **kwargs):
        joint_name = kwargs['joint_name'] if 'joint_name' in kwargs.keys() else self.name
        gripper_status = robot_status['end_of_arm']['stretch_gripper']
        pos_aperture_m, pos_rad, effort, vel = self.gripper_conversion.status_to_all(gripper_status)
        if (joint_name == 'gripper_aperture'):
            return (pos_aperture_m, vel, effort)
        elif (joint_name == 'joint_gripper_finger_left') or (joint_name == 'joint_gripper_finger_right'):
            return (pos_rad, vel, effort)

class ArmCommandGroup(SimpleCommandGroup):
    def __init__(self, range_m=None, calibrated_retracted_offset_m=None, node=None):
        SimpleCommandGroup.__init__(self, 'wrist_extension', range_m, acceptable_joint_error=0.008, node=node)
        self.telescoping_joints = ['joint_arm_l3', 'joint_arm_l2', 'joint_arm_l1', 'joint_arm_l0']
        self.is_telescoping = False
        if calibrated_retracted_offset_m is None:
            calibrated_retracted_offset_m = node.controller_parameters['arm_retracted_offset']
        self.retracted_offset_m = calibrated_retracted_offset_m
        self.retracted = False

    def update_joint_range(self, joint_range, node=None):
        if joint_range is not None:
            self.range = joint_range
            return

        if node is None:
            return # cannot calculate range without Stretch Body handle
        range_m = tuple(node.robot.arm.params['range_m'])
        self.range = range_m

    def get_num_valid_commands(self):
        if self.active and self.is_telescoping:
            return len(self.telescoping_joints)
        elif self.active:
            return 1

        return 0

    def update(self, commanded_joint_names, invalid_joints_callback, **kwargs):
        self.active = False
        self.is_telescoping = False
        self.index = None
        active_telescoping_joint_names = list(set(commanded_joint_names) & set(self.telescoping_joints))
        if self.name in commanded_joint_names:
            if len(active_telescoping_joint_names) == 0:
                self.index = commanded_joint_names.index(self.name)
                self.active = True
            else:
                err_str = ("Received a command for the wrist_extension joint and one or more telescoping_joints. "
                           "These are mutually exclusive options. The joint names in the received command = "
                           "{0}").format(commanded_joint_names)
                invalid_joints_callback(err_str)
                return False
        elif len(active_telescoping_joint_names) != 0:
            if len(active_telescoping_joint_names) == len(self.telescoping_joints):
                self.active = True
                self.is_telescoping = True
                self.index = [commanded_joint_names.index(i) for i in self.telescoping_joints]
            else:
                err_str = ("Commands with telescoping joints requires all telescoping joints to be present. "
                           "Only received {0} of {1} telescoping joints. They are = "
                           "{2}").format(len(active_telescoping_joint_names), len(self.telescoping_joints),
                                         active_telescoping_joint_names)
                invalid_joints_callback(err_str)
                return False

        return True

    def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal, **kwargs):
        self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
        if self.active:
            if self.is_telescoping:
                goal_pos = sum([point.positions[i] for i in self.index]) \
                           if len(point.positions) > max(self.index) else None
                self.goal['velocity'] = point.velocities[self.index[0]] \
                                        if len(point.velocities) > self.index[0] else None
                self.goal['acceleration'] = point.accelerations[self.index[0]] \
                                            if len(point.accelerations) > self.index[0] else None
                self.goal['contact_threshold'] = point.effort[self.index[0]] \
                                                 if len(point.effort) > self.index[0] else None
            else:
                goal_pos = point.positions[self.index] \
                           if len(point.positions) > self.index else None
                self.goal['velocity'] = point.velocities[self.index] \
                                        if len(point.velocities) > self.index else None
                self.goal['acceleration'] = point.accelerations[self.index] \
                                            if len(point.accelerations) > self.index else None
                self.goal['contact_threshold'] = point.effort[self.index] \
                                                 if len(point.effort) > self.index else None

            if goal_pos is None:
                err_str = ("Received goal point with positions array length={0}. "
                           "This joint ({1})'s index is {2}. Length of array must cover all joints listed "
                           "in commanded_joint_names.").format(len(point.positions), self.name, self.index)
                invalid_goal_callback(err_str)
                return False

            self.goal['position'] = hm.bound_ros_command(self.range, goal_pos, fail_out_of_range_goal)
            if self.goal['position'] is None:
                err_str = ("Received {0} goal point that is out of bounds. "
                            "Range = {1}, but goal point = {2}.").format(self.name, self.range, goal_pos)
                invalid_goal_callback(err_str)
                return False

        return True

    def init_execution(self, robot, robot_status, **kwargs):
        if self.active:
            _, extension_error_m = self.update_execution(robot_status)
            robot.arm.move_by(extension_error_m,
                              v_m=self.goal['velocity'],
                              a_m=self.goal['acceleration'],
                              contact_thresh_pos_N=self.goal['contact_threshold'],
                              contact_thresh_neg_N=-self.goal['contact_threshold'] \
                                                   if self.goal['contact_threshold'] is not None else None)
            self.retracted = extension_error_m < 0.0

    def update_execution(self, robot_status, **kwargs):
        success_callback = kwargs['success_callback'] if 'success_callback' in kwargs.keys() else None
        self.error = None
        if self.active:
            if success_callback and robot_status['arm']['motor']['in_guarded_event']:
                success_callback("{0} contact detected.".format(self.name))
                return True
            arm_backlash_correction = self.retracted_offset_m if self.retracted else 0.0
            extension_current = robot_status['arm']['pos'] + arm_backlash_correction
            self.error = self.goal['position'] - extension_current
            return (self.telescoping_joints, self.error) if self.is_telescoping else (self.name, self.error)

        return None

    def joint_state(self, robot_status, **kwargs):
        arm_status = robot_status['arm']
        arm_backlash_correction = self.retracted_offset_m if self.retracted else 0.0
        pos = arm_status['pos'] + arm_backlash_correction
        return (pos, arm_status['vel'], arm_status['motor']['effort'])

class LiftCommandGroup(SimpleCommandGroup):
    def __init__(self, range_m=None, node=None):
        SimpleCommandGroup.__init__(self, 'joint_lift', range_m, node=node)

    def update_joint_range(self, joint_range, node=None):
        if joint_range is not None:
            self.range = joint_range
            return

        if node is None:
            return # cannot calculate range without Stretch Body handle
        range_m = tuple(node.robot.lift.params['range_m'])
        self.range = range_m

    def init_execution(self, robot, robot_status, **kwargs):
        if self.active:
            robot.lift.move_by(self.update_execution(robot_status)[1],
                               v_m=self.goal['velocity'],
                               a_m=self.goal['acceleration'],
                               contact_thresh_pos_N=self.goal['contact_threshold'],
                               contact_thresh_neg_N=-self.goal['contact_threshold'] \
                                                    if self.goal['contact_threshold'] is not None else None)

    def update_execution(self, robot_status, **kwargs):
        success_callback = kwargs['success_callback'] if 'success_callback' in kwargs.keys() else None
        self.error = None
        if self.active:
            if success_callback and robot_status['lift']['motor']['in_guarded_event']:
                success_callback("{0} contact detected.".format(self.name))
                return True
            self.error = self.goal['position'] - robot_status['lift']['pos']
            return self.name, self.error

        return None

    def joint_state(self, robot_status, **kwargs):
        lift_status = robot_status['lift']
        return (lift_status['pos'], lift_status['vel'], lift_status['motor']['effort'])

class MobileBaseCommandGroup(SimpleCommandGroup):
    def __init__(self, virtual_range_m=(-0.5, 0.5), node=None):
        SimpleCommandGroup.__init__(self, 'joint_mobile_base_translation', virtual_range_m,
                                    acceptable_joint_error=0.005, node=node)
        self.incrementing_joint_names = ['translate_mobile_base', 'rotate_mobile_base']
        self.active_translate_mobile_base = False
        self.active_rotate_mobile_base = False
        self.acceptable_mobile_base_error_m = 0.005
        self.excellent_mobile_base_error_m = 0.005
        self.acceptable_mobile_base_error_rad = (np.pi/180.0) * 6.0
        self.excellent_mobile_base_error_rad = (np.pi/180.0) * 0.6
        self.min_m_per_s = 0.002
        self.min_rad_per_s = np.radians(1.0)

    def update_joint_range(self, virtual_joint_range, node=None):
        if virtual_joint_range is not None:
            self.range = virtual_joint_range
            return

        virtual_range_m = (-0.5, 0.5)
        self.range = virtual_range_m

    def get_num_valid_commands(self):
        if self.active:
            num_inc = self.active_translate_mobile_base + self.active_rotate_mobile_base
            return num_inc if num_inc > 0 else 1

        return 0

    def update(self, commanded_joint_names, invalid_joints_callback, **kwargs):
        #
        # desired behavior:
        #    + If both rotation and translation commands received, then return goal failure.
        #    + If only one received, check that the array length works with the index.
        #    + Can both commands be provided and one have a null goal of some sort? If so, should consider handling this case.

        robot_mode = kwargs['robot_mode']
        self.active = False
        self.active_translate_mobile_base = False
        self.active_rotate_mobile_base = False
        self.index = None
        self.index_translate_mobile_base = None
        self.index_rotate_mobile_base = None
        active_incrementing_joint_names = list(set(commanded_joint_names) & set(self.incrementing_joint_names))

        if robot_mode == 'manipulation':
            err_str = ("MobileBaseCommandGroup.update: Manipulation mode is no longer allowed.")
            invalid_joints_callback(err_str)
            return False

        if 'joint_mobile_base_translation' in commanded_joint_names:
            err_str = ("MobileBaseCommandGroup.update: Received a command for the mobile base virtual joint (joint_mobile_base_translation). This joint is no longer allowed.")
            invalid_joints_callback(err_str)
            return False

        if (('translate_mobile_base' in active_incrementing_joint_names) and
            ('rotate_mobile_base' in active_incrementing_joint_names)):
            err_str = ("MobileBaseCommandGroup.update: Both translate_mobile_base and rotate_mobile_base commands received. Only one can be given at a time. They are mutually exclusive.")
            invalid_joints_callback(err_str)
            return False

        if len(active_incrementing_joint_names) != 0:
            if robot_mode == 'position':
                self.active = True
                if 'translate_mobile_base' in active_incrementing_joint_names:
                    self.active_translate_mobile_base = True
                    self.index_translate_mobile_base = commanded_joint_names.index('translate_mobile_base')
                if 'rotate_mobile_base' in active_incrementing_joint_names:
                    self.active_rotate_mobile_base = True
                    self.index_rotate_mobile_base = commanded_joint_names.index('rotate_mobile_base')
            else:
                err_str = ("MobileBaseCommandGroup.update: Must be in position mode to receive a command for the {0} joint(s). "
                           "Current mode = {1}.").format(active_positioning_joint_names, robot_mode)
                invalid_joints_callback(err_str)
                return False

        return True

    def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal, **kwargs):
        #
        # desired behavior:
        #    + If both rotation and translation commands received, then return goal failure.
        #    + If only one received, check that the array length works with the index.
        #    + Can both commands be provided and one have a null goal of some sort? If so, should consider handling this case.

        self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
        self.goal_translate_mobile_base = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
        self.goal_rotate_mobile_base = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
        if self.active:
            if self.active_translate_mobile_base and self.active_rotate_mobile_base:
                err_str = ("MobileBaseCommandGroup.set_goal: Both self.active_translate_mobile_base and self.active_rotate_mobile_base are True, which should not happen since only translation or rotation can be commanded at a time. They are mutually exclusive.")
                invalid_goal_callback(err_str)
                return False

            if self.active_translate_mobile_base:
                if (len(point.positions) <= self.index_translate_mobile_base):
                    err_str = ("MobileBaseCommandGroup.set_goal: Received goal point with positions array "
                               "length {0} and names ({1}), but the index for the mobile base translation "
                               "joint is {2}.").format(len(point.positions),
                                                       self.incrementing_joint_names,
                                                       self.index_translate_mobile_base)
                    invalid_goal_callback(err_str)
                    return False

                if not np.isclose(point.positions[self.index_translate_mobile_base], 0.0, rtol=1e-5, atol=1e-8, equal_nan=False):
                    self.goal_translate_mobile_base['position'] = point.positions[self.index_translate_mobile_base]
                    self.goal_translate_mobile_base['velocity'] = point.velocities[self.index_translate_mobile_base] if len(point.velocities) > self.index_translate_mobile_base else None
                    self.goal_translate_mobile_base['acceleration'] = point.accelerations[self.index_translate_mobile_base] if len(point.accelerations) > self.index_translate_mobile_base else None
                    self.goal_translate_mobile_base['contact_threshold'] = point.effort[self.index_translate_mobile_base] if len(point.effort) > self.index_translate_mobile_base else None

            if self.active_rotate_mobile_base:
                if (len(point.positions) <= self.index_rotate_mobile_base):
                    err_str = ("MobileBaseCommandGroup.set_goal: Received goal point with positions array "
                               "length {0} and names ({1}), but the index for the mobile base rotation "
                               "joint is {2}.").format(len(point.positions),
                                                       self.incrementing_joint_names,
                                                       self.index_rotate_mobile_base)
                    invalid_goal_callback(err_str)
                    return False

                if not np.isclose(point.positions[self.index_rotate_mobile_base], 0.0, rtol=1e-5, atol=1e-8, equal_nan=False):
                    self.goal_rotate_mobile_base['position'] = point.positions[self.index_rotate_mobile_base]
                    self.goal_rotate_mobile_base['velocity'] = point.velocities[self.index_rotate_mobile_base] if len(point.velocities) > self.index_rotate_mobile_base else None
                    self.goal_rotate_mobile_base['acceleration'] = point.accelerations[self.index_rotate_mobile_base] if len(point.accelerations) > self.index_rotate_mobile_base else None
                    self.goal_rotate_mobile_base['contact_threshold'] = point.effort[self.index_rotate_mobile_base] if len(point.effort) > self.index_rotate_mobile_base else None

        return True

    def init_execution(self, robot, robot_status, **kwargs):
        self.startx = robot_status['base']['x']
        self.starty = robot_status['base']['y']
        self.starttheta = robot_status['base']['theta']
        self.base_status = robot_status['base']

        if self.active:
            if self.active_translate_mobile_base or self.active_rotate_mobile_base:
                (_, mobile_base_error_m), (_, mobile_base_error_rad) = self.update_execution(robot_status)
                if mobile_base_error_m is not None:
                    robot.base.translate_by(mobile_base_error_m,
                                            v_m=self.goal_translate_mobile_base['velocity'],
                                            a_m=self.goal_translate_mobile_base['acceleration'],
                                            contact_thresh_N=self.goal_translate_mobile_base['contact_threshold'])
                elif mobile_base_error_rad is not None:
                    robot.base.rotate_by(mobile_base_error_rad,
                                         v_r=self.goal_rotate_mobile_base['velocity'],
                                         a_r=self.goal_rotate_mobile_base['acceleration'],
                                         contact_thresh_N=self.goal_rotate_mobile_base['contact_threshold'])
            else:
                robot.base.translate_by(self.update_execution(robot_status)[1],
                                        v_m=self.goal['velocity'],
                                        a_m=self.goal['acceleration'],
                                        contact_thresh_N=self.goal['contact_threshold'])

    def update_execution(self, robot_status, **kwargs):
        success_callback = kwargs['success_callback'] if 'success_callback' in kwargs.keys() else None
        currx = robot_status['base']['x']
        curry = robot_status['base']['y']
        currtheta = robot_status['base']['theta']
        self.base_status = robot_status['base']

        self.error = None
        self.error_translate_mobile_base_m = None
        self.error_rotate_mobile_base_rad = None
        if self.active:
            if self.active_translate_mobile_base or self.active_rotate_mobile_base:
                if self.goal_translate_mobile_base['position'] is not None:
                    if (robot_status['base']['left_wheel']['in_guarded_event'] or \
                        robot_status['base']['right_wheel']['in_guarded_event']) and \
                       success_callback:
                        success_callback("translate_mobile_base contact detected.")
                        return True
                    dist = np.sqrt(np.square(currx - self.startx) + np.square(curry - self.starty))
                    self.error_translate_mobile_base_m = self.goal_translate_mobile_base['position'] - (dist * np.sign(self.goal_translate_mobile_base['position']))
                    return [('translate_mobile_base', self.error_translate_mobile_base_m), ('rotate_mobile_base', self.error_rotate_mobile_base_rad)]
                elif self.goal_rotate_mobile_base['position'] is not None:
                    if (robot_status['base']['left_wheel']['in_guarded_event'] or \
                        robot_status['base']['right_wheel']['in_guarded_event']) and \
                       success_callback:
                        success_callback("rotate_mobile_base contact detected.")
                        return True
                    rot = hm.angle_diff_rad(currtheta, self.starttheta)
                    self.error_rotate_mobile_base_rad = hm.angle_diff_rad(self.goal_rotate_mobile_base['position'], rot)
                    return [('translate_mobile_base', self.error_translate_mobile_base_m), ('rotate_mobile_base', self.error_rotate_mobile_base_rad)]
            else:
                if (robot_status['base']['left_wheel']['in_guarded_event'] or \
                    robot_status['base']['right_wheel']['in_guarded_event']) and \
                   success_callback:
                    success_callback("{0} contact detected.".format(self.name))
                    return True
                self.error = self.goal['position'] - currx
                return self.name, self.error

        return None

    def goal_reached(self):
        if self.active:
            if self.active_translate_mobile_base or self.active_rotate_mobile_base:
                if self.active_translate_mobile_base:
                    reached = (abs(self.error_translate_mobile_base_m) < self.acceptable_mobile_base_error_m)
                    if not (abs(self.error_translate_mobile_base_m) < self.excellent_mobile_base_error_m):
                        # Use velocity to help decide when the low-level command has been finished
                        speed = np.sqrt(np.square(self.base_status['x_vel']) + np.square(self.base_status['y_vel']))
                        reached = reached and (speed < self.min_m_per_s)
                    return reached
                elif self.active_rotate_mobile_base:
                    reached = (abs(self.error_rotate_mobile_base_rad) < self.acceptable_mobile_base_error_rad)
                    if not (abs(self.error_rotate_mobile_base_rad) < self.excellent_mobile_base_error_rad):
                        # Use velocity to help decide when the low-level command has been finished
                        speed = self.base_status['theta_vel']
                        reached = reached and (abs(speed) < self.min_rad_per_s)
                    return reached
            else:
                return (abs(self.error) < self.acceptable_joint_error)

        return True


text:
import numpy as np

import torch
import torch.nn as nn

def update_state_dict(net, pretrained_dict):
    model_dict = net.state_dict()
    pretrained_dict = {k: v for k, v in pretrained_dict.items()
                       if k in model_dict}
    model_dict.update(pretrained_dict)
    net.load_state_dict(model_dict)

max_stars_repo_name: Qinka/revolver

max_issues_repo_name: Qinka/revolver

max_forks_repo_name: Qinka/revolver