Python math.pi() Examples

The following are 30 code examples of math.pi(). You can vote up the ones you like or vote down the ones you don't like, and go to the original project or source file by following the links above each example. You may also want to check out all available functions/classes of the module math , or try the search function .
Example #1
Source File: Wave.py    From BiblioPixelAnimations with MIT License 7 votes vote down vote up
def step(self, amt=1):
        for i in range(self._size):
            y = math.sin(
                math.pi *
                float(self.cycles) *
                float(self._step * i) /
                float(self._size))

            if y >= 0.0:
                # Peaks of sine wave are white
                y = 1.0 - y  # Translate Y to 0.0 (top) to 1.0 (center)
                r, g, b = self.palette(0)
                c2 = (int(255 - float(255 - r) * y),
                      int(255 - float(255 - g) * y),
                      int(255 - float(255 - b) * y))
            else:
                # Troughs of sine wave are black
                y += 1.0  # Translate Y to 0.0 (bottom) to 1.0 (center)
                r, g, b = self.palette(0)
                c2 = (int(float(r) * y),
                      int(float(g) * y),
                      int(float(b) * y))
            self.layout.set(self._start + i, c2)

        self._step += amt 
Example #2
Source File: transforms.py    From audio with BSD 2-Clause "Simplified" License 6 votes vote down vote up
def _fade_in(self, waveform_length: int) -> Tensor:
        fade = torch.linspace(0, 1, self.fade_in_len)
        ones = torch.ones(waveform_length - self.fade_in_len)

        if self.fade_shape == "linear":
            fade = fade

        if self.fade_shape == "exponential":
            fade = torch.pow(2, (fade - 1)) * fade

        if self.fade_shape == "logarithmic":
            fade = torch.log10(.1 + fade) + 1

        if self.fade_shape == "quarter_sine":
            fade = torch.sin(fade * math.pi / 2)

        if self.fade_shape == "half_sine":
            fade = torch.sin(fade * math.pi - math.pi / 2) / 2 + 0.5

        return torch.cat((fade, ones)).clamp_(0, 1) 
Example #3
Source File: minitaur_ball_gym_env.py    From soccer-matlab with BSD 2-Clause "Simplified" License 6 votes vote down vote up
def _get_observation(self):
    world_translation_minitaur, world_rotation_minitaur = (
        self._pybullet_client.getBasePositionAndOrientation(
            self.minitaur.quadruped))
    world_translation_ball, world_rotation_ball = (
        self._pybullet_client.getBasePositionAndOrientation(self._ball_id))
    minitaur_translation_world, minitaur_rotation_world = (
        self._pybullet_client.invertTransform(world_translation_minitaur,
                                              world_rotation_minitaur))
    minitaur_translation_ball, _ = (
        self._pybullet_client.multiplyTransforms(minitaur_translation_world,
                                                 minitaur_rotation_world,
                                                 world_translation_ball,
                                                 world_rotation_ball))
    distance = math.sqrt(minitaur_translation_ball[0]**2 +
                         minitaur_translation_ball[1]**2)
    angle = math.atan2(minitaur_translation_ball[0],
                       minitaur_translation_ball[1])
    self._observation = [angle - math.pi / 2, distance]
    return self._observation 
Example #4
Source File: minitaur_randomize_terrain_gym_env_example.py    From soccer-matlab with BSD 2-Clause "Simplified" License 6 votes vote down vote up
def ResetTerrainExample():
  """An example showing resetting random terrain env."""
  num_reset = 10
  steps = 100
  env = minitaur_randomize_terrain_gym_env.MinitaurRandomizeTerrainGymEnv(
      render=True,
      leg_model_enabled=False,
      motor_velocity_limit=np.inf,
      pd_control_enabled=True)
  action = [math.pi / 2] * 8
  for _ in xrange(num_reset):
    env.reset()
    for _ in xrange(steps):
      _, _, done, _ = env.step(action)
      if done:
        break 
Example #5
Source File: loss.py    From torch-toolbox with BSD 3-Clause "New" or "Revised" License 6 votes vote down vote up
def __init__(
            self,
            classes,
            m=0.5,
            s=64,
            easy_margin=True,
            weight=None,
            size_average=None,
            ignore_index=-100,
            reduce=None,
            reduction='mean'):
        super(ArcLoss, self).__init__(weight, size_average, reduce, reduction)
        self.ignore_index = ignore_index
        assert s > 0.
        assert 0 <= m <= (math.pi / 2)
        self.s = s
        self.m = m
        self.cos_m = math.cos(m)
        self.sin_m = math.sin(m)
        self.mm = math.sin(math.pi - m) * m
        self.threshold = math.cos(math.pi - m)
        self.classes = classes
        self.easy_margin = easy_margin 
Example #6
Source File: minitaur.py    From soccer-matlab with BSD 2-Clause "Simplified" License 6 votes vote down vote up
def MapToMinusPiToPi(angles):
  """Maps a list of angles to [-pi, pi].

  Args:
    angles: A list of angles in rad.
  Returns:
    A list of angle mapped to [-pi, pi].
  """
  mapped_angles = copy.deepcopy(angles)
  for i in range(len(angles)):
    mapped_angles[i] = math.fmod(angles[i], TWO_PI)
    if mapped_angles[i] >= math.pi:
      mapped_angles[i] -= TWO_PI
    elif mapped_angles[i] < -math.pi:
      mapped_angles[i] += TWO_PI
  return mapped_angles 
Example #7
Source File: minitaur_gym_env.py    From soccer-matlab with BSD 2-Clause "Simplified" License 6 votes vote down vote up
def _get_observation_upper_bound(self):
    """Get the upper bound of the observation.

    Returns:
      The upper bound of an observation. See GetObservation() for the details
        of each element of an observation.
    """
    upper_bound = np.zeros(self._get_observation_dimension())
    num_motors = self.minitaur.num_motors
    upper_bound[0:num_motors] = math.pi  # Joint angle.
    upper_bound[num_motors:2 * num_motors] = (
        motor.MOTOR_SPEED_LIMIT)  # Joint velocity.
    upper_bound[2 * num_motors:3 * num_motors] = (
        motor.OBSERVED_TORQUE_LIMIT)  # Joint torque.
    upper_bound[3 * num_motors:] = 1.0  # Quaternion of base orientation.
    return upper_bound 
Example #8
Source File: loss.py    From torch-toolbox with BSD 3-Clause "New" or "Revised" License 6 votes vote down vote up
def _get_body(self, x, target):
        cos_t = torch.gather(x, 1, target.unsqueeze(1))  # cos(theta_yi)
        if self.easy_margin:
            cond = torch.relu(cos_t)
        else:
            cond_v = cos_t - self.threshold
            cond = torch.relu(cond_v)
        cond = cond.bool()
        # Apex would convert FP16 to FP32 here
        # cos(theta_yi + m)
        new_zy = torch.cos(torch.acos(cos_t) + self.m).type(cos_t.dtype)
        if self.easy_margin:
            zy_keep = cos_t
        else:
            zy_keep = cos_t - self.mm  # (cos(theta_yi) - sin(pi - m)*m)
        new_zy = torch.where(cond, new_zy, zy_keep)
        diff = new_zy - cos_t  # cos(theta_yi + m) - cos(theta_yi)
        gt_one_hot = F.one_hot(target, num_classes=self.classes)
        body = gt_one_hot * diff
        return body 
Example #9
Source File: transform_utils.py    From robosuite with MIT License 6 votes vote down vote up
def random_quat(rand=None):
    """Return uniform random unit quaternion.
    rand: array like or None
        Three independent random variables that are uniformly distributed
        between 0 and 1.
    >>> q = random_quat()
    >>> np.allclose(1.0, vector_norm(q))
    True
    >>> q = random_quat(np.random.random(3))
    >>> q.shape
    (4,)
    """
    if rand is None:
        rand = np.random.rand(3)
    else:
        assert len(rand) == 3
    r1 = np.sqrt(1.0 - rand[0])
    r2 = np.sqrt(rand[0])
    pi2 = math.pi * 2.0
    t1 = pi2 * rand[1]
    t2 = pi2 * rand[2]
    return np.array(
        (np.sin(t1) * r1, np.cos(t1) * r1, np.sin(t2) * r2, np.cos(t2) * r2),
        dtype=np.float32,
    ) 
Example #10
Source File: minitaur_reactive_env.py    From soccer-matlab with BSD 2-Clause "Simplified" License 6 votes vote down vote up
def _get_observation_upper_bound(self):
    """Get the upper bound of the observation.

    Returns:
      The upper bound of an observation. See _get_true_observation() for the
      details of each element of an observation.
    """
    upper_bound_roll = 2 * math.pi
    upper_bound_pitch = 2 * math.pi
    upper_bound_roll_dot = 2 * math.pi / self._time_step
    upper_bound_pitch_dot = 2 * math.pi / self._time_step
    upper_bound_motor_angle = 2 * math.pi
    upper_bound = [
        upper_bound_roll, upper_bound_pitch, upper_bound_roll_dot,
        upper_bound_pitch_dot
    ]

    if self._use_angle_in_observation:
      upper_bound.extend([upper_bound_motor_angle] * NUM_MOTORS)
    return np.array(upper_bound) 
Example #11
Source File: kaldi.py    From audio with BSD 2-Clause "Simplified" License 6 votes vote down vote up
def _feature_window_function(window_type: str,
                             window_size: int,
                             blackman_coeff: float,
                             device: torch.device,
                             dtype: int,
                             ) -> Tensor:
    r"""Returns a window function with the given type and size
    """
    if window_type == HANNING:
        return torch.hann_window(window_size, periodic=False, device=device, dtype=dtype)
    elif window_type == HAMMING:
        return torch.hamming_window(window_size, periodic=False, alpha=0.54, beta=0.46, device=device, dtype=dtype)
    elif window_type == POVEY:
        # like hanning but goes to zero at edges
        return torch.hann_window(window_size, periodic=False, device=device, dtype=dtype).pow(0.85)
    elif window_type == RECTANGULAR:
        return torch.ones(window_size, device=device, dtype=dtype)
    elif window_type == BLACKMAN:
        a = 2 * math.pi / (window_size - 1)
        window_function = torch.arange(window_size, device=device, dtype=dtype)
        # can't use torch.blackman_window as they use different coefficients
        return (blackman_coeff - 0.5 * torch.cos(a * window_function) +
                (0.5 - blackman_coeff) * torch.cos(2 * a * window_function)).to(device=device, dtype=dtype)
    else:
        raise Exception('Invalid window type ' + window_type) 
Example #12
Source File: minitaur_trotting_env.py    From soccer-matlab with BSD 2-Clause "Simplified" License 6 votes vote down vote up
def _gen_signal(self, t, phase):
    """Generates a sinusoidal reference leg trajectory.

    The foot (leg tip) will move in a ellipse specified by extension and swing
    amplitude.

    Args:
      t: Current time in simulation.
      phase: The phase offset for the periodic trajectory.

    Returns:
      The desired leg extension and swing angle at the current time.
    """
    period = 1 / self._step_frequency
    extension = self._extension_amplitude * math.cos(
        2 * math.pi / period * t + phase)
    swing = self._swing_amplitude * math.sin(2 * math.pi / period * t + phase)
    return extension, swing 
Example #13
Source File: minitaur_gym_env_example.py    From soccer-matlab with BSD 2-Clause "Simplified" License 6 votes vote down vote up
def ResetPoseExample(log_path=None):
  """An example that the minitaur stands still using the reset pose."""
  steps = 10000
  environment = minitaur_gym_env.MinitaurGymEnv(
      urdf_version=minitaur_gym_env.DERPY_V0_URDF_VERSION,
      render=True,
      leg_model_enabled=False,
      motor_velocity_limit=np.inf,
      pd_control_enabled=True,
      accurate_motor_model_enabled=True,
      motor_overheat_protection=True,
      hard_reset=False,
      log_path=log_path)
  action = [math.pi / 2] * 8
  for _ in range(steps):
    _, _, done, _ = environment.step(action)
    time.sleep(1./100.)
    if done:
      break 
Example #14
Source File: Wave.py    From BiblioPixelAnimations with MIT License 6 votes vote down vote up
def step(self, amt=1):
        for i in range(self._size):
            y = math.sin((math.pi * float(self.cycles) * float(i) / float(self._size)) + self._moveStep)

            if y >= 0.0:
                # Peaks of sine wave are white
                y = 1.0 - y  # Translate Y to 0.0 (top) to 1.0 (center)
                r, g, b = self.palette(0)
                c2 = (int(255 - float(255 - r) * y), int(255 - float(255 - g) * y), int(255 - float(255 - b) * y))
            else:
                # Troughs of sine wave are black
                y += 1.0  # Translate Y to 0.0 (bottom) to 1.0 (center)
                r, g, b = self.palette(0)
                c2 = (int(float(r) * y),
                      int(float(g) * y),
                      int(float(b) * y))
            self.layout.set(self._start + i, c2)

        self._moveStep += amt
        self._moveStep += 1
        if(self._moveStep >= self._size):
            self._moveStep = 0 
Example #15
Source File: common.py    From PyOptiX with MIT License 6 votes vote down vote up
def calculate_camera_variables(eye, lookat, up, fov, aspect_ratio, fov_is_vertical=False):
    import numpy as np
    import math

    W = np.array(lookat) - np.array(eye)
    wlen = np.linalg.norm(W)
    U = np.cross(W, np.array(up))
    U /= np.linalg.norm(U)
    V = np.cross(U, W)
    V /= np.linalg.norm(V)

    if fov_is_vertical:
        vlen = wlen * math.tan(0.5 * fov * math.pi / 180.0)
        V *= vlen
        ulen = vlen * aspect_ratio
        U *= ulen
    else:
        ulen = wlen * math.tan(0.5 * fov * math.pi / 180.0)
        U *= ulen
        vlen = ulen * aspect_ratio
        V *= vlen

    return U, V, W 
Example #16
Source File: minitaur_trotting_env.py    From soccer-matlab with BSD 2-Clause "Simplified" License 6 votes vote down vote up
def _signal(self, t):
    """Generates the trotting gait for the robot.

    Args:
      t: Current time in simulation.

    Returns:
      A numpy array of the reference leg positions.
    """
    # Generates the leg trajectories for the two digonal pair of legs.
    ext_first_pair, sw_first_pair = self._gen_signal(t, 0)
    ext_second_pair, sw_second_pair = self._gen_signal(t, math.pi)

    trotting_signal = np.array([
        sw_first_pair, sw_second_pair, sw_second_pair, sw_first_pair,
        ext_first_pair, ext_second_pair, ext_second_pair, ext_first_pair
    ])
    signal = np.array(self._init_pose) + trotting_signal
    return signal 
Example #17
Source File: minitaur_terrain_randomizer.py    From soccer-matlab with BSD 2-Clause "Simplified" License 6 votes vote down vote up
def sample(self):
    """Samples new points around some existing point.

    Removes the sampling base point and also stores the new jksampled points if
    they are far enough from all existing points.
    """
    active_point = self._active_list.pop()
    for _ in xrange(self._max_sample_size):
      # Generate random points near the current active_point between the radius
      random_radius = np.random.uniform(self._min_radius, 2 * self._min_radius)
      random_angle = np.random.uniform(0, 2 * math.pi)

      # The sampled 2D points near the active point
      sample = random_radius * np.array(
          [np.cos(random_angle), np.sin(random_angle)]) + active_point

      if not self._is_in_grid(sample):
        continue

      if self._is_close_to_existing_points(sample):
        continue

      self._active_list.append(sample)
      self._grid[self._point_to_index_1d(sample)] = sample 
Example #18
Source File: test_pybullet_sim_gym_env.py    From soccer-matlab with BSD 2-Clause "Simplified" License 6 votes vote down vote up
def ResetPoseExample(steps):
  """An example that the minitaur stands still using the reset pose."""
  
  
  environment = pybullet_sim_gym_env.PyBulletSimGymEnv(
      pybullet_sim_factory=boxstack_pybullet_sim,
      debug_visualization=False,
      render=True, action_repeat=30)
  action = [math.pi / 2] * 8
  
  vid = video_recorder.VideoRecorder(env=environment,path="vid.mp4")
  
  for _ in range(steps):
    print(_)
    startsim = time.time()
    _, _, done, _ = environment.step(action)
    stopsim = time.time()
    startrender = time.time()
    #environment.render(mode='rgb_array')
    vid.capture_frame()
    stoprender = time.time()
    print ("env.step " , (stopsim - startsim))
    print ("env.render " , (stoprender - startrender))
    if done:
    	environment.reset() 
Example #19
Source File: kuka.py    From soccer-matlab with BSD 2-Clause "Simplified" License 6 votes vote down vote up
def __init__(self, urdfRootPath=pybullet_data.getDataPath(), timeStep=0.01):
    self.urdfRootPath = urdfRootPath
    self.timeStep = timeStep
    self.maxVelocity = .35
    self.maxForce = 200.
    self.fingerAForce = 2 
    self.fingerBForce = 2.5
    self.fingerTipForce = 2
    self.useInverseKinematics = 1
    self.useSimulation = 1
    self.useNullSpace =21
    self.useOrientation = 1
    self.kukaEndEffectorIndex = 6
    self.kukaGripperIndex = 7
    #lower limits for null space
    self.ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05]
    #upper limits for null space
    self.ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05]
    #joint ranges for null space
    self.jr=[5.8,4,5.8,4,5.8,4,6]
    #restposes for null space
    self.rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0]
    #joint damping coefficents
    self.jd=[0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001]
    self.reset() 
Example #20
Source File: minitaur_gym_env_example.py    From soccer-matlab with BSD 2-Clause "Simplified" License 6 votes vote down vote up
def ResetPoseExample():
  """An example that the minitaur stands still using the reset pose."""
  steps = 1000
  randomizer = (minitaur_env_randomizer.MinitaurEnvRandomizer())
  environment = minitaur_gym_env.MinitaurBulletEnv(
      render=True,
      leg_model_enabled=False,
      motor_velocity_limit=np.inf,
      pd_control_enabled=True,
      accurate_motor_model_enabled=True,
      motor_overheat_protection=True,
      env_randomizer =  randomizer,
      hard_reset=False)
  action = [math.pi / 2] * 8
  for _ in range(steps):
    _, _, done, _ = environment.step(action)
    if done:
      break
  environment.reset() 
Example #21
Source File: minitaur.py    From soccer-matlab with BSD 2-Clause "Simplified" License 6 votes vote down vote up
def ConvertFromLegModel(self, actions):
    """Convert the actions that use leg model to the real motor actions.

    Args:
      actions: The theta, phi of the leg model.
    Returns:
      The eight desired motor angles that can be used in ApplyActions().
    """

    motor_angle = copy.deepcopy(actions)
    scale_for_singularity = 1
    offset_for_singularity = 1.5
    half_num_motors = int(self.num_motors / 2)
    quater_pi = math.pi / 4
    for i in range(self.num_motors):
      action_idx = i // 2
      forward_backward_component = (-scale_for_singularity * quater_pi * (
          actions[action_idx + half_num_motors] + offset_for_singularity))
      extension_component = (-1)**i * quater_pi * actions[action_idx]
      if i >= half_num_motors:
        extension_component = -extension_component
      motor_angle[i] = (
          math.pi + forward_backward_component + extension_component)
    return motor_angle 
Example #22
Source File: functional.py    From audio with BSD 2-Clause "Simplified" License 5 votes vote down vote up
def bandpass_biquad(
        waveform: Tensor,
        sample_rate: int,
        central_freq: float,
        Q: float = 0.707,
        const_skirt_gain: bool = False
) -> Tensor:
    r"""Design two-pole band-pass filter.  Similar to SoX implementation.

    Args:
        waveform (Tensor): audio waveform of dimension of `(..., time)`
        sample_rate (int): sampling rate of the waveform, e.g. 44100 (Hz)
        central_freq (float): central frequency (in Hz)
        Q (float, optional): https://en.wikipedia.org/wiki/Q_factor (Default: ``0.707``)
        const_skirt_gain (bool, optional) : If ``True``, uses a constant skirt gain (peak gain = Q).
            If ``False``, uses a constant 0dB peak gain. (Default: ``False``)

    Returns:
        Tensor: Waveform of dimension of `(..., time)`

    References:
        http://sox.sourceforge.net/sox.html
        https://www.w3.org/2011/audio/audio-eq-cookbook.html#APF
    """
    w0 = 2 * math.pi * central_freq / sample_rate
    alpha = math.sin(w0) / 2 / Q

    temp = math.sin(w0) / 2 if const_skirt_gain else alpha
    b0 = temp
    b1 = 0.
    b2 = -temp
    a0 = 1 + alpha
    a1 = -2 * math.cos(w0)
    a2 = 1 - alpha
    return biquad(waveform, b0, b1, b2, a0, a1, a2) 
Example #23
Source File: functional.py    From audio with BSD 2-Clause "Simplified" License 5 votes vote down vote up
def allpass_biquad(
        waveform: Tensor,
        sample_rate: int,
        central_freq: float,
        Q: float = 0.707
) -> Tensor:
    r"""Design two-pole all-pass filter.  Similar to SoX implementation.

    Args:
        waveform(torch.Tensor): audio waveform of dimension of `(..., time)`
        sample_rate (int): sampling rate of the waveform, e.g. 44100 (Hz)
        central_freq (float): central frequency (in Hz)
        Q (float, optional): https://en.wikipedia.org/wiki/Q_factor (Default: ``0.707``)

    Returns:
        Tensor: Waveform of dimension of `(..., time)`

    References:
        http://sox.sourceforge.net/sox.html
        https://www.w3.org/2011/audio/audio-eq-cookbook.html#APF
    """
    w0 = 2 * math.pi * central_freq / sample_rate
    alpha = math.sin(w0) / 2 / Q

    b0 = 1 - alpha
    b1 = -2 * math.cos(w0)
    b2 = 1 + alpha
    a0 = 1 + alpha
    a1 = -2 * math.cos(w0)
    a2 = 1 - alpha
    return biquad(waveform, b0, b1, b2, a0, a1, a2) 
Example #24
Source File: losses.py    From ACAN with MIT License 5 votes vote down vote up
def NormalDist(x, sigma):
    f = torch.exp(-x**2/(2*sigma**2)) / sqrt(2*pi*sigma**2)
    return f 
Example #25
Source File: intra_alignment.py    From transferlearning with MIT License 5 votes vote down vote up
def getAngle(Ps, Pt, DD):
    
    Q = np.hstack((Ps, scipy.linalg.null_space(Ps.T)))
    dim = Pt.shape[1]
    QPt = Q.T @ Pt
    A, B = QPt[:dim, :], QPt[dim:, :]
    U,V,X,C,S = gsvd(A, B)
    alpha = np.zeros([1, DD])
    for i in range(DD):
        alpha[0][i] = math.sin(np.real(math.acos(C[i][i]*math.pi/180)))
    
    return alpha 
Example #26
Source File: metrics.py    From dogTorch with MIT License 5 votes vote down vote up
def record_output(self, output, output_indices, target, prev_absolute_imu,
                      next_absolute_imu, batch_size=1):

        if self.args.no_angle_metric:
            return []

        prev_absolute_imu = prev_absolute_imu[:, output_indices]
        next_absolute_imu = next_absolute_imu[:, output_indices]

        assert output.dim() == 4
        if self.regression:
            # Output is the vectors itself.
            relative_imus = output
        else:
            output = torch.max(output, -1)[1]  #get labels from vectors
            relative_imus = self.get_diff_from_initial(output)
        if self.absolute_regress:
            resulting_imus = output
        else:
            resulting_imus = self.inverse_subtract(prev_absolute_imu,
                                                   relative_imus)

        angle_diff = self.get_angle_diff(next_absolute_imu, resulting_imus)
        whole_diff = self.get_angle_diff(next_absolute_imu[:, 0:1, :, :],
                                         next_absolute_imu[:, -1:, :, :])

        self.stats.append([whole_diff, angle_diff.mean()])
        self.outputs.append(output)
        self.targets.append(target)

        degree = angle_diff * 180 / math.pi
        self.meter.update(degree, batch_size)

        return resulting_imus 
Example #27
Source File: transform_utils.py    From robosuite with MIT License 5 votes vote down vote up
def quat_slerp(quat0, quat1, fraction, spin=0, shortestpath=True):
    """Return spherical linear interpolation between two quaternions.
    >>> q0 = random_quat()
    >>> q1 = random_quat()
    >>> q = quat_slerp(q0, q1, 0.0)
    >>> np.allclose(q, q0)
    True
    >>> q = quat_slerp(q0, q1, 1.0, 1)
    >>> np.allclose(q, q1)
    True
    >>> q = quat_slerp(q0, q1, 0.5)
    >>> angle = math.acos(np.dot(q0, q))
    >>> np.allclose(2.0, math.acos(np.dot(q0, q1)) / angle) or \
        np.allclose(2.0, math.acos(-np.dot(q0, q1)) / angle)
    True
    """
    q0 = unit_vector(quat0[:4])
    q1 = unit_vector(quat1[:4])
    if fraction == 0.0:
        return q0
    elif fraction == 1.0:
        return q1
    d = np.dot(q0, q1)
    if abs(abs(d) - 1.0) < _EPS:
        return q0
    if shortestpath and d < 0.0:
        # invert rotation
        d = -d
        q1 *= -1.0
    angle = math.acos(d) + spin * math.pi
    if abs(angle) < _EPS:
        return q0
    isin = 1.0 / math.sin(angle)
    q0 *= math.sin((1.0 - fraction) * angle) * isin
    q1 *= math.sin(fraction * angle) * isin
    q0 += q1
    return q0 
Example #28
Source File: utility.py    From soccer-matlab with BSD 2-Clause "Simplified" License 5 votes vote down vote up
def diag_normal_entropy(mean, logstd):
  """Empirical entropy of a normal with diagonal covariance."""
  constant = mean.shape[-1].value * math.log(2 * math.pi * math.e)
  return (constant + tf.reduce_sum(2 * logstd, 1)) / 2 
Example #29
Source File: utility.py    From soccer-matlab with BSD 2-Clause "Simplified" License 5 votes vote down vote up
def diag_normal_logpdf(mean, logstd, loc):
  """Log density of a normal with diagonal covariance."""
  constant = -0.5 * math.log(2 * math.pi) - logstd
  value = -0.5 * ((loc - mean) / tf.exp(logstd)) ** 2
  return tf.reduce_sum(constant + value, -1) 
Example #30
Source File: minitaur.py    From soccer-matlab with BSD 2-Clause "Simplified" License 5 votes vote down vote up
def GetObservationUpperBound(self):
    """Get the upper bound of the observation.

    Returns:
      The upper bound of an observation. See GetObservation() for the details
        of each element of an observation.
    """
    upper_bound = np.array([0.0] * self.GetObservationDimension())
    upper_bound[0:self.num_motors] = math.pi  # Joint angle.
    upper_bound[self.num_motors:2 * self.num_motors] = (
        motor.MOTOR_SPEED_LIMIT)  # Joint velocity.
    upper_bound[2 * self.num_motors:3 * self.num_motors] = (
        motor.OBSERVED_TORQUE_LIMIT)  # Joint torque.
    upper_bound[3 * self.num_motors:] = 1.0  # Quaternion of base orientation.
    return upper_bound