decoupling tendons to subcomponents and fixing tendon scaling issue

python/mujoco/usd/shapes.py

@@ -27,14 +27,28 @@ def create_hemisphere(
 
     return mesh
 
+def decouple_config(config: dict):
+  decoupled_config = []
+  for key, value in config.items():
+    if key == "name":
+      continue
+    decoupled_config.append({
+      "parent_name": config["name"],
+      "name": config["name"] + "_" + key,
+      key: value.copy()
+    })
+
+  return decoupled_config
+
 def mesh_config_generator(
     name: str,
     geom_type: mujoco.mjtGeom,
-    size: np.ndarray
+    size: np.ndarray,
+    decouple: bool = False
 ):
 
     if geom_type == mujoco.mjtGeom.mjGEOM_PLANE:
-      return {
+      config = {
         "name": name,
         "box": {
           "width": size[0] * 2 if size[0] > 0 else 100,
@@ -44,7 +58,7 @@ def mesh_config_generator(
         }
       }
     elif geom_type == mujoco.mjtGeom.mjGEOM_SPHERE:
-      return {
+      config = {
         "name": name,
         "sphere": {
           "radius": float(size[0])
@@ -52,7 +66,7 @@ def mesh_config_generator(
       }
     elif geom_type == mujoco.mjtGeom.mjGEOM_CAPSULE:
       cylinder = mesh_config_generator(name, mujoco.mjtGeom.mjGEOM_CYLINDER, size)
-      return {
+      config = {
         "name": name,
         "cylinder": cylinder["cylinder"],
         "left_hemisphere": {
@@ -74,12 +88,12 @@ def mesh_config_generator(
       sphere["sphere"]["transform"] = {
         "scale": tuple(size)
       }
-      return {
+      config = {
         "name": name,
         "sphere": sphere["sphere"],
       }
     elif geom_type == mujoco.mjtGeom.mjGEOM_CYLINDER:
-      return {
+      config = {
         "name": name,
         "cylinder": {
           "radius": size[0],
@@ -87,7 +101,7 @@ def mesh_config_generator(
         }
       }
     elif geom_type == mujoco.mjtGeom.mjGEOM_BOX:
-      return {
+      config = {
         "name": name,
         "box": {
           "width": size[0] * 2,
@@ -98,6 +112,10 @@ def mesh_config_generator(
     else:
       raise NotImplemented(f"{geom_type} primitive geom type not implemented with USD integration")
 
+    if decouple:
+      config = decouple_config(config)
+
+    return config
 
 def mesh_generator(
     mesh_config: dict,
@@ -110,7 +128,7 @@ def mesh_generator(
 
     for shape, config in mesh_config.items():
 
-      if shape == "name":
+      if "name" in shape:
         continue
 
       if "box" in shape:
@@ -140,9 +158,9 @@ def mesh_generator(
         )
 
       if "transform" in config:
-
         if "rotate" in config["transform"]:
-          R = mesh.get_rotation_matrix_from_xyz(config["transform"]["rotate"])
+          tmp = o3d.geometry.TriangleMesh.create_coordinate_frame() # TODO: add rotation matrix code gen to utils.py
+          R = tmp.get_rotation_matrix_from_xyz(config["transform"]["rotate"])
           prim_mesh.rotate(R, center=(0, 0, 0))
         if "scale" in config["transform"]:
           prim_mesh.vertices = o3d.utility.Vector3dVector(