# Path: ravens/utils/utils.py
# def get_heightmap(points, colors, bounds, pixel_size):
# def get_pointcloud(depth, intrinsics):
# def transform_pointcloud(points, transform):
# def reconstruct_heightmaps(color, depth, configs, bounds, pixel_size):
# def pix_to_xyz(pixel, height, bounds, pixel_size, skip_height=False):
# def xyz_to_pix(position, bounds, pixel_size):
# def unproject_vectorized(uv_coordinates, depth_values,
#                          intrinsic,
#                          distortion):
# def unproject_depth_vectorized(im_depth, depth_dist,
#                                camera_mtx,
#                                camera_dist):
# def sample_distribution(prob, n_samples=1):
# def invert(pose):
# def multiply(pose0, pose1):
# def apply(pose, position):
# def eulerXYZ_to_quatXYZW(rotation):  # pylint: disable=invalid-name
# def quatXYZW_to_eulerXYZ(quaternion_xyzw):  # pylint: disable=invalid-name
# def apply_transform(transform_to_from, points_from):
# def preprocess(img):
# def get_fused_heightmap(obs, configs, bounds, pix_size):
# def get_image_transform(theta, trans, pivot=(0, 0)):
# def check_transform(image, pixel, transform):
# def get_se3_from_image_transform(theta, trans, pivot, heightmap, bounds,
#                                  pixel_size):
# def get_random_image_transform_params(image_size):
# def perturb(input_image, pixels, set_theta_zero=False):
# def plot(fname,  # pylint: disable=dangerous-default-value
#          title,
#          ylabel,
#          xlabel,
#          data,
#          xlim=[-np.inf, 0],
#          xticks=None,
#          ylim=[np.inf, -np.inf],
#          show_std=True):
# def create_visualizer(clear=True):
# def make_frame(vis, name, h, radius, o=1.0):
# def meshcat_visualize(vis, obs, act, info):
# COLORS = {
#     'blue': [078.0 / 255.0, 121.0 / 255.0, 167.0 / 255.0],
#     'red': [255.0 / 255.0, 087.0 / 255.0, 089.0 / 255.0],
#     'green': [089.0 / 255.0, 169.0 / 255.0, 079.0 / 255.0],
#     'orange': [242.0 / 255.0, 142.0 / 255.0, 043.0 / 255.0],
#     'yellow': [237.0 / 255.0, 201.0 / 255.0, 072.0 / 255.0],
#     'purple': [176.0 / 255.0, 122.0 / 255.0, 161.0 / 255.0],
#     'pink': [255.0 / 255.0, 157.0 / 255.0, 167.0 / 255.0],
#     'cyan': [118.0 / 255.0, 183.0 / 255.0, 178.0 / 255.0],
#     'brown': [156.0 / 255.0, 117.0 / 255.0, 095.0 / 255.0],
#     'gray': [186.0 / 255.0, 176.0 / 255.0, 172.0 / 255.0]
# }


import numpy as np
from ravens.utils import utils

"""Motion primitives."""



class PickPlace():
  """Pick and place primitive."""

  def __init__(self, height=0.32, speed=0.01):
    self.height, self.speed = height, speed

  def __call__(self, movej, movep, ee, pose0, pose1):
    """Execute pick and place primitive.

    Args:
      movej: function to move robot joints.
      movep: function to move robot end effector pose.
      ee: robot end effector.
      pose0: SE(3) picking pose.
      pose1: SE(3) placing pose.

    Returns:
      timeout: robot movement timed out if True.
    """

    pick_pose, place_pose = pose0, pose1

    # Execute picking primitive.
    prepick_to_pick = ((0, 0, 0.32), (0, 0, 0, 1))
    postpick_to_pick = ((0, 0, self.height), (0, 0, 0, 1))

# Path: ravens/utils/utils.py
# def get_heightmap(points, colors, bounds, pixel_size):
# def get_pointcloud(depth, intrinsics):
# def transform_pointcloud(points, transform):
# def reconstruct_heightmaps(color, depth, configs, bounds, pixel_size):
# def pix_to_xyz(pixel, height, bounds, pixel_size, skip_height=False):
# def xyz_to_pix(position, bounds, pixel_size):
# def unproject_vectorized(uv_coordinates, depth_values,
#                          intrinsic,
#                          distortion):
# def unproject_depth_vectorized(im_depth, depth_dist,
#                                camera_mtx,
#                                camera_dist):
# def sample_distribution(prob, n_samples=1):
# def invert(pose):
# def multiply(pose0, pose1):
# def apply(pose, position):
# def eulerXYZ_to_quatXYZW(rotation):  # pylint: disable=invalid-name
# def quatXYZW_to_eulerXYZ(quaternion_xyzw):  # pylint: disable=invalid-name
# def apply_transform(transform_to_from, points_from):
# def preprocess(img):
# def get_fused_heightmap(obs, configs, bounds, pix_size):
# def get_image_transform(theta, trans, pivot=(0, 0)):
# def check_transform(image, pixel, transform):
# def get_se3_from_image_transform(theta, trans, pivot, heightmap, bounds,
#                                  pixel_size):
# def get_random_image_transform_params(image_size):
# def perturb(input_image, pixels, set_theta_zero=False):
# def plot(fname,  # pylint: disable=dangerous-default-value
#          title,
#          ylabel,
#          xlabel,
#          data,
#          xlim=[-np.inf, 0],
#          xticks=None,
#          ylim=[np.inf, -np.inf],
#          show_std=True):
# def create_visualizer(clear=True):
# def make_frame(vis, name, h, radius, o=1.0):
# def meshcat_visualize(vis, obs, act, info):
# COLORS = {
#     'blue': [078.0 / 255.0, 121.0 / 255.0, 167.0 / 255.0],
#     'red': [255.0 / 255.0, 087.0 / 255.0, 089.0 / 255.0],
#     'green': [089.0 / 255.0, 169.0 / 255.0, 079.0 / 255.0],
#     'orange': [242.0 / 255.0, 142.0 / 255.0, 043.0 / 255.0],
#     'yellow': [237.0 / 255.0, 201.0 / 255.0, 072.0 / 255.0],
#     'purple': [176.0 / 255.0, 122.0 / 255.0, 161.0 / 255.0],
#     'pink': [255.0 / 255.0, 157.0 / 255.0, 167.0 / 255.0],
#     'cyan': [118.0 / 255.0, 183.0 / 255.0, 178.0 / 255.0],
#     'brown': [156.0 / 255.0, 117.0 / 255.0, 095.0 / 255.0],
#     'gray': [186.0 / 255.0, 176.0 / 255.0, 172.0 / 255.0]
# }


# Path: ravens/tasks/primitives.py
import numpy as np
from ravens.utils import utils


"""Motion primitives."""



class PickPlace():
  """Pick and place primitive."""

  def __init__(self, height=0.32, speed=0.01):
    self.height, self.speed = height, speed

  def __call__(self, movej, movep, ee, pose0, pose1):
    """Execute pick and place primitive.

    Args:
      movej: function to move robot joints.
      movep: function to move robot end effector pose.
      ee: robot end effector.
      pose0: SE(3) picking pose.
      pose1: SE(3) placing pose.

    Returns:
      timeout: robot movement timed out if True.
    """

    pick_pose, place_pose = pose0, pose1

    # Execute picking primitive.
    prepick_to_pick = ((0, 0, 0.32), (0, 0, 0, 1))
    postpick_to_pick = ((0, 0, self.height), (0, 0, 0, 1))

    prepick_pose = utils.multiply(pick_pose, prepick_to_pick)