Skip to content

Commit

Permalink
Merge pull request #2 from ZhengyiLuo/dev
Browse files Browse the repository at this point in the history 
Migrating to full-featured SMPLSim
  • Loading branch information
@ZhengyiLuo
ZhengyiLuo authored Mar 13, 2024
2 parents 0ec11c8 + 440a591 commit 3984c90
Show file tree
Hide file tree
Showing 15 changed files with 1,733 additions and 1,097 deletions.
3 changes: 3 additions & 0 deletions MUJOCO_LOG.TXT
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,3 +4,6 @@ ERROR: could not initialize GLFW
Mon Feb 19 20:01:16 2024
WARNING: mju_openResource: unknown file 'tmp/smpl_humanoid.xml'

Sat Mar 2 18:16:01 2024
ERROR: could not initialize GLFW

26 changes: 13 additions & 13 deletions smpl_sim/smpllib/np_smpl_humanoid_batch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,20 +5,20 @@
import sys
import pdb
import os.path as osp
from copycat.utils.torch_ext import dict_to_torch
from smpl_sim.utils.torch_ext import dict_to_torch

sys.path.append(os.getcwd())

from copycat.utils.torch_utils import *
from copycat.utils.transform_utils import *
from smpl_sim.utils.torch_utils import *
from smpl_sim.utils.np_transform_utils import *
from scipy.spatial.transform import Rotation as sRot
import joblib
from mujoco_py import load_model_from_path
from copycat.smpllib.smpl_mujoco import SMPLConverter, smpl_to_qpose, smpl_to_qpose_torch, SMPL_BONE_ORDER_NAMES
from copycat.smpllib.smpl_parser import SMPL_EE_NAMES
from copycat.utils.tools import get_expert, get_expert_master
from smpl_sim.smpllib.smpl_joint_names import SMPLConverter, smpl_to_qpose, smpl_to_qpose_torch, SMPL_BONE_ORDER_NAMES
from smpl_sim.smpllib.smpl_parser import SMPL_EE_NAMES
from smpl_sim.utils.tools import get_expert, get_expert_master
import pytorch3d.transforms as tR
from copycat.smpllib.smpl_parser import (
from smpl_sim.smpllib.smpl_parser import (
SMPL_Parser,
SMPLH_Parser,
SMPLX_Parser,
Expand Down Expand Up @@ -406,12 +406,12 @@ def forward_kinematics_batch(self, rotations, root_rotations,

if __name__ == "__main__":
import mujoco_py
from copycat.smpllib.smpl_robot import Robot
from copycat.smpllib.torch_smpl_humanoid import Humanoid
from copycat.utils.config_utils.copycat_config import Config
from copycat.data_loaders.dataset_amass_single import DatasetAMASSSingle
from copycat.utils.torch_ext import dict_to_torch
from copycat.smpllib.smpl_mujoco import smpl_to_qpose_torch, smplh_to_smpl
from smpl_sim.smpllib.smpl_robot import Robot
from smpl_sim.smpllib.torch_smpl_humanoid import Humanoid
from smpl_sim.utils.config_utils.copycat_config import Config
from smpl_sim.data_loaders.dataset_amass_single import DatasetAMASSSingle
from smpl_sim.utils.torch_ext import dict_to_torch
from smpl_sim.smpllib.smpl_mujoco import smpl_to_qpose_torch, smplh_to_smpl
torch.manual_seed(0)

cfg = Config(
Expand Down
115 changes: 73 additions & 42 deletions smpl_sim/smpllib/skeleton_local.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,31 +105,62 @@
# }

# PHC's gains
# GAINS = {
# "L_Hip": [800, 80, 1, 500],
# "L_Knee": [800, 80, 1, 500],
# "L_Ankle": [800, 80, 1, 500],
# "L_Toe": [500, 50, 1, 500],
# "R_Hip": [800, 80, 1, 500],
# "R_Knee": [800, 80, 1, 500],
# "R_Ankle": [800, 80, 1, 500],
# "R_Toe": [500, 50, 1, 500],
# "Torso": [1000, 100, 1, 500],
# "Spine": [1000, 100, 1, 500],
# "Chest": [1000, 100, 1, 500],
# "Neck": [500, 50, 1, 250],
# "Head": [500, 50, 1, 250],
# "L_Thorax": [500, 50, 1, 500],
# "L_Shoulder": [500, 50, 1, 500],
# "L_Elbow": [500, 50, 1, 150],
# "L_Wrist": [300, 30, 1, 150],
# "L_Hand": [300, 30, 1, 150],
# "R_Thorax": [500, 50, 1, 150],
# "R_Shoulder": [500, 50, 1, 250],
# "R_Elbow": [500, 50, 1, 150],
# "R_Wrist": [300, 30, 1, 150],
# "R_Hand": [300, 30, 1, 150],
# }
GAINS_PHC = {
"L_Hip": [800, 80, 1, 500],
"L_Knee": [800, 80, 1, 500],
"L_Ankle": [800, 80, 1, 500],
"L_Toe": [500, 50, 1, 500],
"R_Hip": [800, 80, 1, 500],
"R_Knee": [800, 80, 1, 500],
"R_Ankle": [800, 80, 1, 500],
"R_Toe": [500, 50, 1, 500],
"Torso": [1000, 100, 1, 500],
"Spine": [1000, 100, 1, 500],
"Chest": [1000, 100, 1, 500],
"Neck": [500, 50, 1, 250],
"Head": [500, 50, 1, 250],
"L_Thorax": [500, 50, 1, 500],
"L_Shoulder": [500, 50, 1, 500],
"L_Elbow": [500, 50, 1, 150],
"L_Wrist": [300, 30, 1, 150],
"L_Hand": [300, 30, 1, 150],
"R_Thorax": [500, 50, 1, 150],
"R_Shoulder": [500, 50, 1, 250],
"R_Elbow": [500, 50, 1, 150],
"R_Wrist": [300, 30, 1, 150],
"R_Hand": [300, 30, 1, 150],

"L_Index1": [100, 10, 1, 150],
"L_Index2": [100, 10, 1, 150],
"L_Index3": [100, 10, 1, 150],
"L_Middle1": [100, 10, 1, 150],
"L_Middle2": [100, 10, 1, 150],
"L_Middle3": [100, 10, 1, 150],
"L_Pinky1": [100, 10, 1, 150],
"L_Pinky2": [100, 10, 1, 150],
"L_Pinky3": [100, 10, 1, 150],
"L_Ring1": [100, 10, 1, 150],
"L_Ring2": [100, 10, 1, 150],
"L_Ring3": [100, 10, 1, 150],
"L_Thumb1": [100, 10, 1, 150],
"L_Thumb2": [100, 10, 1, 150],
"L_Thumb3": [100, 10, 1, 150],
"R_Index1": [100, 10, 1, 150],
"R_Index2": [100, 10, 1, 150],
"R_Index3": [100, 10, 1, 150],
"R_Middle1": [100, 10, 1, 150],
"R_Middle2": [100, 10, 1, 150],
"R_Middle3": [100, 10, 1, 150],
"R_Pinky1": [100, 10, 1, 150],
"R_Pinky2": [100, 10, 1, 150],
"R_Pinky3": [100, 10, 1, 150],
"R_Ring1": [100, 10, 1, 150],
"R_Ring2": [100, 10, 1, 150],
"R_Ring3": [100, 10, 1, 150],
"R_Thumb1": [100, 10, 1, 150],
"R_Thumb2": [100, 10, 1, 150],
"R_Thumb3": [100, 10, 1, 150],
}

### UHC Phd
# GAINS = {
Expand Down Expand Up @@ -158,7 +189,7 @@
# "R_Hand": [100, 10, 1, 150, 1, 1],
# }

GAINS = {
GAINS_MJ = {
"L_Hip": [250, 2.5, 1, 500, 10, 2],
"L_Knee": [250, 2.5, 1, 500, 10, 2],
"L_Ankle": [150, 2.5, 1, 500, 10, 2],
Expand Down Expand Up @@ -276,6 +307,7 @@ def load_from_offsets(
real_weight=False,
big_ankle=False,
box_body = False,
sim='mujoco',
ball_joints = False,
create_vel_sensors = False,
exclude_contacts = []
Expand All @@ -297,6 +329,7 @@ def load_from_offsets(
self.freeze_hand = freeze_hand
self.box_body = box_body
self.ball_joints = ball_joints
self.sim = sim
joint_names = list(filter(lambda x: all([t not in x for t in exclude_bones]), offsets.keys()))
dof_ind = {"x": 0, "y": 1, "z": 2}
self.len_scale = scale
Expand Down Expand Up @@ -369,7 +402,10 @@ def construct_tree(self,
attr = dict()
attr["name"] = name
attr["joint"] = name
attr["gear"] = str(GAINS[name[:-2]][2])
if self.sim in ["mujoco"]:
attr["gear"] = str(GAINS_MJ[name[:-2]][2])
elif self.sim in ["isaacgym"]:
attr["gear"] = "500"
SubElement(actuators, "motor", attr)


Expand Down Expand Up @@ -440,7 +476,7 @@ def write_xml_bodynode(self, bone, parent_node, offset, ref_angles):
j_attr["name"] = bone.name
j_attr["type"] = "ball"
j_attr["pos"] = "{0:.4f} {1:.4f} {2:.4f}".format(*(bone.pos + offset))
j_attr["user"] = " ".join([ str(s) for s in GAINS[bone.name]]) # using user to set the max torque
j_attr["user"] = " ".join([ str(s) for s in GAINS_MJ[bone.name]]) # using user to set the max torque

if j_attr["name"] in ref_angles.keys():
j_attr["ref"] = f"{ref_angles[j_attr['name']]:.1f}"
Expand All @@ -456,16 +492,14 @@ def write_xml_bodynode(self, bone, parent_node, offset, ref_angles):
j_attr["pos"] = "{0:.4f} {1:.4f} {2:.4f}".format(*(bone.pos + offset))
j_attr["axis"] = "{0:.4f} {1:.4f} {2:.4f}".format(*axis)

################################################################################
# j_attr["stiffness"] = str(GAINS[bone.name][-2])
# j_attr["damping"] = str(GAINS[bone.name][-1])
################################################################################

# j_attr["stiffness"] = str(1)
# j_attr["damping"] = str(1)

j_attr["user"] = " ".join([ str(s) for s in GAINS[bone.name]]) # using user to set the max torque
j_attr["armature"] = "0.01"
if self.sim in ["mujoco"]:
j_attr["user"] = " ".join([ str(s) for s in GAINS_MJ[bone.name]]) # using user to set the max torque
j_attr["armature"] = "0.01"
elif self.sim in ["isaacgym"]:
j_attr["stiffness"] = str(GAINS_PHC[bone.name][0])
j_attr["damping"] = str(GAINS_PHC[bone.name][1])
j_attr["armature"] = "0.02"


if i < len(bone.lb):
j_attr["range"] = "{0:.4f} {1:.4f}".format(bone.lb[i], bone.ub[i])
Expand Down Expand Up @@ -601,9 +635,6 @@ def write_xml_bodynode(self, bone, parent_node, offset, ref_angles):
rot = np.array([1, 0, 0, 0])

g_attr["type"] = "box"
g_attr["pos"] = "{0:.4f} {1:.4f} {2:.4f}".format(*pos)
g_attr["size"] = "{0:.4f} {1:.4f} {2:.4f}".format(*size)
g_attr["quat"] = "{0:.4f} {1:.4f} {2:.4f} {3:.4f}".format(*rot)

if bone.name == "Pelvis":
size /= 1.75 # ZL Hack: shrinkage
Expand All @@ -628,7 +659,7 @@ def write_xml_bodynode(self, bone, parent_node, offset, ref_angles):

if self.real_weight_porpotion_boxes:
g_attr["density"] = str((hull_params['volume'] / (size[0] * size[1] * size[2] * 8).item()) * base_density)

g_attr["pos"] = "{0:.4f} {1:.4f} {2:.4f}".format(*pos)
g_attr["size"] = "{0:.4f} {1:.4f} {2:.4f}".format(*size)
g_attr["quat"] = "{0:.4f} {1:.4f} {2:.4f} {3:.4f}".format(*rot)
Expand Down
Loading

0 comments on commit 3984c90

Please sign in to comment.