Untitled
import numpy as np
import os
import torch
from isaacgym import gymtorch
from isaacgym import gymapi
import math
from isaacgymenvs.utils.torch_jit_utils import (
to_torch,
get_axis_params,
torch_rand_float,
quat_rotate_inverse,
)
from isaacgymenvs.tasks.base.vec_task import VecTask
from isaacgymenvs.utils.torch_jit_utils import to_torch, get_axis_params,
torch_rand_float, quat_rotate, quat_rotate_inverse
from typing import Tuple, Dict
# PERMUTATION_MATRIX = torch.tensor([
# [0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0.],
# [0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0.],
# [0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0.],
# [0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0.],
# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0.],
# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1.],
# [1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
# [0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
# [0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0.],
# [0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
# [0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0.],
# [0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.]]
# , dtype=torch.float32)
# KP = torch.tensor(2 * [2.0, 2.0, 34.0, 34.0, 0.75, 0.75]) * torch.square(
# torch.tensor(2 * [100, 122, 24.42, 24.42, 102, 125]))
# ) # from PACT_dimensioning_beta.pdf
KP = torch.tensor([15000.0, 15000.0, 15000.0, 15000.0, 5000.0, 5000.0] * 2)
#KD = torch.tensor(2 * [0.000638, 0.0029, 0.000638, 0.001, 0.0007, 0.0007])
KD = torch.tensor([0.01, 0.01, 0.01, 0.01, 0.01, 0.01] * 2)
ACTIONS_MIRROR_MAT = torch.tensor([
[ 0., 0., 0., 0., 0., 0., -1., 0., 0., 0., 0., 0.],
[ 0., 0., 0., 0., 0., 0., 0., -1., 0., 0., 0., 0.],
[ 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0.],
[ 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0.],
[ 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0.],
[ 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -1.],
[-1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],