kinematics refactor with mimic joint support

src/curobo/cuda_robot_model/cuda_robot_generator.py

@@ -314,6 +314,7 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
             link_map=self._link_map,
             joint_map=self._joint_map,
             joint_map_type=self._joint_map_type,
+            joint_offset_map=self._joint_offset_map,
             store_link_map=self._store_link_map,
             link_chain_map=self._link_chain_map,
             link_names=self.link_names,
@@ -332,6 +333,7 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
             base_link=self.base_link,
             ee_link=self.ee_link,
             lock_jointstate=self.lock_jointstate,
+            mimic_joints=self._mimic_joint_data,
         )
         if self.asset_root_path != "":
             self._kinematics_parser.add_absolute_path_to_link_meshes(self.asset_root_path)
@@ -368,9 +370,8 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
     @profiler.record_function("robot_generator/build_chain")
     def _build_chain(self, base_link, ee_link, other_links, link_names):
         self._n_dofs = 0
-        self._controlled_joints = []
+        self._controlled_links = []
         self._bodies = []
-
         self._name_to_idx_map = dict()
         self.base_link = base_link
         self.ee_link = ee_link
@@ -403,37 +404,61 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
         self.non_fixed_joint_names = self.joint_names.copy()
         return chain_link_names
 
+    def _get_mimic_joint_data(self):
+        # get joint types:
+        mimic_joint_data = {}
+        for i in range(1, len(self._bodies)):
+            body = self._bodies[i]
+            if i in self._controlled_links:
+                if body.mimic_joint_name is not None:
+                    if body.joint_name not in mimic_joint_data:
+                        mimic_joint_data[body.joint_name] = []
+                    mimic_joint_data[body.joint_name].append(i)
+        return mimic_joint_data
+
     @profiler.record_function("robot_generator/build_kinematics_tensors")
     def _build_kinematics_tensors(self, base_link, ee_link, link_names, chain_link_names):
+        self._active_joints = []
+        self._mimic_joint_data = {}
         link_map = [0 for i in range(len(self._bodies))]
         store_link_map = []  # [-1 for i in range(len(self._bodies))]
 
         joint_map = [
-            -1 if i not in self._controlled_joints else i for i in range(len(self._bodies))
+            -1 if i not in self._controlled_links else i for i in range(len(self._bodies))
         ]  #
         joint_map_type = [
-            -1 if i not in self._controlled_joints else i for i in range(len(self._bodies))
+            -1 if i not in self._controlled_links else i for i in range(len(self._bodies))
         ]
         all_joint_names = []
-        j_count = 0
         ordered_link_names = []
+        joint_offset_map = [[1.0, 0.0]]
         # add body 0 details:
         if self._bodies[0].link_name in link_names:
             store_link_map.append(chain_link_names.index(self._bodies[0].link_name))
             ordered_link_names.append(self._bodies[0].link_name)
+            joint_offset_map.append(self._bodies[0].joint_offset)
         # get joint types:
         for i in range(1, len(self._bodies)):
             body = self._bodies[i]
             parent_name = body.parent_link_name
             link_map[i] = self._name_to_idx_map[parent_name]
-            all_joint_names.append(body.joint_name)
+            joint_offset_map.append(body.joint_offset)
+            joint_map_type[i] = body.joint_type.value
             if body.link_name in link_names:
                 store_link_map.append(chain_link_names.index(body.link_name))
                 ordered_link_names.append(body.link_name)
-            if i in self._controlled_joints:
-                joint_map[i] = j_count
-                joint_map_type[i] = body.joint_type.value
-                j_count += 1
+            if body.joint_name not in all_joint_names:
+                all_joint_names.append(body.joint_name)
+            if i in self._controlled_links:
+                joint_map[i] = self.joint_names.index(body.joint_name)
+                if body.mimic_joint_name is not None:
+                    if body.joint_name not in self._mimic_joint_data:
+                        self._mimic_joint_data[body.joint_name] = []
+                    self._mimic_joint_data[body.joint_name].append(
+                        {"joint_offset": body.joint_offset, "joint_name": body.mimic_joint_name}
+                    )
+                else:
+                    self._active_joints.append(i)
         self.link_names = ordered_link_names
         # do a for loop to get link matrix:
         link_chain_map = torch.eye(
@@ -458,6 +483,10 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
         self._store_link_map = torch.as_tensor(
             store_link_map, device=self.tensor_args.device, dtype=torch.int16
         )
+        self._joint_offset_map = torch.as_tensor(
+            joint_offset_map, device=self.tensor_args.device, dtype=torch.float32
+        )
+        self._joint_offset_map = self._joint_offset_map.view(-1).contiguous()
         self._link_chain_map = link_chain_map.to(device=self.tensor_args.device)
         self._fixed_transform = torch.cat((self._fixed_transform), dim=0).to(
             device=self.tensor_args.device
@@ -489,12 +518,18 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
         joint_data = self._get_joint_links(lock_joint_names)
 
         lock_links = list(
-            set(
-                [joint_data[j]["parent"] for j in joint_data.keys()]
-                + [joint_data[j]["child"] for j in joint_data.keys()]
-            )
+            [joint_data[j]["parent"] for j in joint_data.keys()]
+            + [joint_data[j]["child"] for j in joint_data.keys()]
         )
-        new_link_names = link_names + lock_links
+
+        for k in lock_joint_names:
+            if "mimic" in joint_data[k]:
+                mimic_link_names = [[x["parent"], x["child"]] for x in joint_data[k]["mimic"]]
+                mimic_link_names = [x for xs in mimic_link_names for x in xs]
+                lock_links += mimic_link_names
+        lock_links = list(set(lock_links))
+
+        new_link_names = list(set(link_names + lock_links))
 
         # rebuild kinematic tree with link names added to link pose computation:
         self._build_kinematics_tensors(base_link, ee_link, new_link_names, chain_link_names)
@@ -520,6 +555,7 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
             link_map=self._link_map,
             joint_map=self._joint_map,
             joint_map_type=self._joint_map_type,
+            joint_offset_map=self._joint_offset_map,
             store_link_map=self._store_link_map,
             link_chain_map=self._link_chain_map,
             link_names=self.link_names,
@@ -531,14 +567,12 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
             total_spheres=self.total_spheres,
         )
         link_poses = self._get_link_poses(q, lock_links, kinematics_config)
-
         # remove lock links from store map:
         store_link_map = [chain_link_names.index(l) for l in link_names]
         self._store_link_map = torch.as_tensor(
             store_link_map, device=self.tensor_args.device, dtype=torch.int16
         )
         self.link_names = link_names
-
         # compute a fixed transform for fixing joints:
         with profiler.record_function("cuda_robot_generator/fix_locked_joints"):
             # convert tensors to cpu:
@@ -554,14 +588,33 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
                     .multiply(link_poses.get_index(0, w_child))
                 )
                 # Make this joint as fixed
-                i = self._all_joint_names.index(j) + 1
+                i = joint_data[j]["link_index"]
+                self._fixed_transform[i] = parent_t_child.get_matrix()
+
+                if "mimic" in joint_data[j]:
+                    for mimic_joint in joint_data[j]["mimic"]:
+                        w_parent = lock_links.index(mimic_joint["parent"])
+                        w_child = lock_links.index(mimic_joint["child"])
+                        parent_t_child = (
+                            link_poses.get_index(0, w_parent)
+                            .inverse()
+                            .multiply(link_poses.get_index(0, w_child))
+                        )
+                        i_q = mimic_joint["link_index"]
+                        self._fixed_transform[i_q] = parent_t_child.get_matrix()
+                        self._controlled_links.remove(i_q)
+                        self._joint_map_type[i_q] = -1
+                        self._joint_map[i_q] = -1
+
+                i = joint_data[j]["link_index"]
                 self._joint_map_type[i] = -1
                 self._joint_map[i:] -= 1
                 self._joint_map[i] = -1
-                self._n_dofs -= 1
-                self._fixed_transform[i] = parent_t_child.get_matrix()
-                self._controlled_joints.remove(i)
+                self._controlled_links.remove(i)
                 self.joint_names.remove(j)
+                self._n_dofs -= 1
+                self._active_joints.remove(i)
+
             self._joint_map[self._joint_map < -1] = -1
             self._joint_map = self._joint_map.to(device=self.tensor_args.device)
             self._joint_map_type = self._joint_map_type.to(device=self.tensor_args.device)
@@ -715,7 +768,7 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
             if not valid_data:
                 break
             if torch.max(coll_cpu[i]) == -torch.inf:
-                print("skip", i)
+                log_info("skip" + str(i))
             for j in range(i + 1, n_spheres):
                 if sl_idx > thread_loc.shape[0] - 1:
                     valid_data = False
@@ -776,9 +829,10 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
         rigid_body_params = self._kinematics_parser.get_link_parameters(link_name, base=base)
         self._bodies.append(rigid_body_params)
         if rigid_body_params.joint_type != JointType.FIXED:
-            self._controlled_joints.append(body_idx)
-            self.joint_names.append(rigid_body_params.joint_name)
-            self._n_dofs = self._n_dofs + 1
+            self._controlled_links.append(body_idx)
+            if rigid_body_params.joint_name not in self.joint_names:
+                self.joint_names.append(rigid_body_params.joint_name)
+                self._n_dofs = self._n_dofs + 1
         self._fixed_transform.append(
             torch.as_tensor(
                 rigid_body_params.fixed_transform,
@@ -790,21 +844,34 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
 
     def _get_joint_links(self, joint_names: List[str]):
         j_data = {}
+
         for j in joint_names:
-            for b in self._bodies:
+            for bi, b in enumerate(self._bodies):
                 if b.joint_name == j:
-                    j_data[j] = {"parent": b.parent_link_name, "child": b.link_name}
+                    if j not in j_data:
+                        j_data[j] = {}
+                    if b.mimic_joint_name is None:
+                        j_data[j]["parent"] = b.parent_link_name
+                        j_data[j]["child"] = b.link_name
+                        j_data[j]["link_index"] = bi
+                    else:
+                        if "mimic" not in j_data[j]:
+                            j_data[j]["mimic"] = []
+                        j_data[j]["mimic"].append(
+                            {
+                                "parent": b.parent_link_name,
+                                "child": b.link_name,
+                                "link_index": bi,
+                                "joint_offset": b.joint_offset,
+                            }
+                        )
+
         return j_data
 
     @profiler.record_function("robot_generator/get_link_poses")
     def _get_link_poses(
         self, q: torch.Tensor, link_names: List[str], kinematics_config: KinematicsTensorConfig
     ) -> Pose:
-        if q.is_contiguous():
-            q_in = q.view(-1)
-        else:
-            q_in = q.reshape(-1)  # .reshape(-1)
-        # q_in = q.reshape(-1)
         link_pos_seq = torch.zeros(
             (1, len(self.link_names), 3),
             device=self.tensor_args.device,
@@ -831,12 +898,11 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
             dtype=self.tensor_args.dtype,
         )
         link_pos_seq, link_quat_seq, _ = get_cuda_kinematics(
-            # self._link_mat_seq,  # data will be stored here
             link_pos_seq,
             link_quat_seq,
             batch_robot_spheres,
             global_cumul_mat,
-            q_in,
+            q,
             kinematics_config.fixed_transforms,
             kinematics_config.link_spheres,
             kinematics_config.link_map,  # tells which link is attached to which link i
@@ -845,6 +911,7 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
             kinematics_config.store_link_map,
             kinematics_config.link_sphere_idx_map,  # sphere idx map
             kinematics_config.link_chain_map,
+            kinematics_config.joint_offset_map,
             grad_out_q,
             self.use_global_cumul,
         )
@@ -873,7 +940,7 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
     def _get_joint_position_velocity_limits(self):
         joint_limits = {"position": [[], []], "velocity": [[], []]}
 
-        for idx in self._controlled_joints:
+        for idx in self._active_joints:
             joint_limits["position"][0].append(self._bodies[idx].joint_limits[0])
             joint_limits["position"][1].append(self._bodies[idx].joint_limits[1])
             joint_limits["velocity"][0].append(self._bodies[idx].joint_velocity_limits[0])
@@ -897,7 +964,7 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
             ]
         )
         # clip joint position:
-        # TODO: change this to be per joint
+        # NOTE: change this to be per joint
         joint_limits["position"][0] += self.cspace.position_limit_clip
         joint_limits["position"][1] -= self.cspace.position_limit_clip
         joint_limits["velocity"][0] *= self.cspace.velocity_scale

src/curobo/cuda_robot_model/cuda_robot_model.py

@@ -33,6 +33,7 @@ from curobo.curobolib.kinematics import get_cuda_kinematics
 from curobo.geom.types import Mesh, Sphere
 from curobo.types.base import TensorDeviceType
 from curobo.types.math import Pose
+from curobo.types.state import JointState
 from curobo.util.logger import log_error
 from curobo.util_file import get_robot_path, join_path, load_yaml
 
@@ -322,7 +323,6 @@ class CudaRobotModel(CudaRobotModelConfig):
 
     def _cuda_forward(self, q):
         link_pos, link_quat, robot_spheres = get_cuda_kinematics(
-            # self._link_mat_seq,  # data will be stored here
             self._link_pos_seq,
             self._link_quat_seq,
             self._batch_robot_spheres,
@@ -336,11 +336,10 @@ class CudaRobotModel(CudaRobotModelConfig):
             self.kinematics_config.store_link_map,
             self.kinematics_config.link_sphere_idx_map,  # sphere idx map
             self.kinematics_config.link_chain_map,
+            self.kinematics_config.joint_offset_map,
             self._grad_out_q,
             self.use_global_cumul,
         )
-        # if(robot_spheres.shape[0]<10):
-        #    print(robot_spheres)
         return link_pos, link_quat, robot_spheres
 
     @property
@@ -381,6 +380,31 @@ class CudaRobotModel(CudaRobotModelConfig):
     def lock_jointstate(self):
         return self.kinematics_config.lock_jointstate
 
+    def get_full_js(self, js: JointState):
+        all_joint_names = self.all_articulated_joint_names
+        lock_joint_state = self.lock_jointstate
+
+        new_js = js.get_augmented_joint_state(all_joint_names, lock_joint_state)
+        return new_js
+
+    def get_mimic_js(self, js: JointState):
+        if self.kinematics_config.mimic_joints is None:
+            return None
+        extra_joints = {"position": [], "joint_names": []}
+        # for every joint in mimic_joints, get active joint name
+        for j in self.kinematics_config.mimic_joints:
+            active_q = js.position[..., js.joint_names.index(j)]
+            for k in self.kinematics_config.mimic_joints[j]:
+                extra_joints["joint_names"].append(k["joint_name"])
+                extra_joints["position"].append(
+                    k["joint_offset"][0] * active_q + k["joint_offset"][1]
+                )
+        extra_js = JointState.from_position(
+            position=torch.stack(extra_joints["position"]), joint_names=extra_joints["joint_names"]
+        )
+        new_js = js.get_augmented_joint_state(js.joint_names + extra_js.joint_names, extra_js)
+        return new_js
+
     @property
     def ee_link(self):
         return self.kinematics_config.ee_link

src/curobo/cuda_robot_model/kinematics_parser.py

@@ -11,7 +11,7 @@
 
 # Standard Library
 from dataclasses import dataclass, field
-from typing import Dict, List, Optional, Union
+from typing import Dict, List, Optional
 
 # Third Party
 import numpy as np
@@ -21,7 +21,6 @@ from curobo.cuda_robot_model.types import JointType
 from curobo.geom.types import Mesh
 from curobo.types.base import TensorDeviceType
 from curobo.types.math import Pose
-from curobo.util.logger import log_error
 
 
 @dataclass
@@ -35,6 +34,8 @@ class LinkParams:
     joint_axis: Optional[np.ndarray] = None
     joint_id: Optional[int] = None
     joint_velocity_limits: List[float] = field(default_factory=lambda: [-2.0, 2.0])
+    joint_offset: List[float] = field(default_factory=lambda: [1.0, 0.0])
+    mimic_joint_name: Optional[str] = None
 
     @staticmethod
     def from_dict(dict_data):
@@ -56,7 +57,7 @@ class KinematicsParser:
         # add extra links to parent:
         if self.extra_links is not None and len(list(self.extra_links.keys())) > 0:
             for i in self.extra_links:
-                self._parent_map[i] = self.extra_links[i].parent_link_name
+                self._parent_map[i] = {"parent": self.extra_links[i].parent_link_name}
 
     def build_link_parent(self):
         """Build a map of parent links to each link in the kinematic tree.
@@ -78,7 +79,7 @@ class KinematicsParser:
         chain_links = [ee_link]
         link = ee_link
         while link != base_link:
-            link = self._parent_map[link]
+            link = self._parent_map[link]["parent"]
             # add link to chain:
             chain_links.append(link)
         chain_links.reverse()

src/curobo/cuda_robot_model/types.py

@@ -228,28 +228,87 @@ class CSpaceConfig:
 
 @dataclass
 class KinematicsTensorConfig:
+    """Stores robot's kinematics parameters as Tensors to use in Kinematics computations.
+
+    Use :meth:`curobo.cuda_robot_model.cuda_robot_generator.CudaRobotGenerator` to generate this
+    configuration from a urdf or usd.
+
+    """
+
+    #: Static Homogenous Transform from parent link to child link for all links [n_links,4,4].
     fixed_transforms: torch.Tensor
+
+    #: index of fixed_transform given link index [n_links].
     link_map: torch.Tensor
+
+    #: joint index given link index [n_links].
     joint_map: torch.Tensor
+
+    #: type of joint given link index [n_links].
     joint_map_type: torch.Tensor
+
+    joint_offset_map: torch.Tensor
+
+    #: index of link to write out pose [n_store_links].
     store_link_map: torch.Tensor
+
+    #: Mapping between each link to every other link, this is used to check
+    #: if a link is part of a serial chain formed by another link [n_links, n_links].
     link_chain_map: torch.Tensor
+
+    #: Name of links whose pose will be stored [n_store_links].
     link_names: List[str]
+
+    #: Joint limits
     joint_limits: JointLimits
+
+    #: Name of joints which are not fixed.
     non_fixed_joint_names: List[str]
+
+    #: Number of joints that are active. Each joint is only actuated along 1 dimension.
     n_dof: int
+
+    #: Name of links which have a mesh. Currently only used for debugging and rendering.
     mesh_link_names: Optional[List[str]] = None
+
+    #: Name of all actuated joints.
     joint_names: Optional[List[str]] = None
+
+    #:
     lock_jointstate: Optional[JointState] = None
+
+    #:
+    mimic_joints: Optional[dict] = None
+
+    #:
     link_spheres: Optional[torch.Tensor] = None
+
+    #:
     link_sphere_idx_map: Optional[torch.Tensor] = None
+
+    #:
     link_name_to_idx_map: Optional[Dict[str, int]] = None
+
+    #: total number of spheres that represent the robot's geometry.
     total_spheres: int = 0
+
+    #: Additional debug parameters.
     debug: Optional[Any] = None
+
+    #: index of end-effector in stored link poses.
     ee_idx: int = 0
+
+    #: Cspace configuration
     cspace: Optional[CSpaceConfig] = None
+
+    #: Name of base link. This is the root link from which all kinematic parameters were computed.
     base_link: str = "base_link"
+
+    #: Name of end-effector link for which the Cartesian pose will be computed.
     ee_link: str = "ee_link"
+
+    #: A copy of link spheres that is used as reference, in case the link_spheres get modified at
+    #: runtime.
     reference_link_spheres: Optional[torch.Tensor] = None
 
     def __post_init__(self):
@@ -260,7 +319,7 @@ class KinematicsTensorConfig:
         if self.link_spheres is not None and self.reference_link_spheres is None:
             self.reference_link_spheres = self.link_spheres.clone()
 
-    def copy_(self, new_config: KinematicsTensorConfig):
+    def copy_(self, new_config: KinematicsTensorConfig) -> KinematicsTensorConfig:
         self.fixed_transforms.copy_(new_config.fixed_transforms)
         self.link_map.copy_(new_config.link_map)
         self.joint_map.copy_(new_config.joint_map)
@@ -268,6 +327,7 @@ class KinematicsTensorConfig:
         self.store_link_map.copy_(new_config.store_link_map)
         self.link_chain_map.copy_(new_config.link_chain_map)
         self.joint_limits.copy_(new_config.joint_limits)
+        self.joint_offset_map.copy_(new_config.joint_offset_map)
         if new_config.link_spheres is not None and self.link_spheres is not None:
             self.link_spheres.copy_(new_config.link_spheres)
         if new_config.link_sphere_idx_map is not None and self.link_sphere_idx_map is not None:
@@ -321,7 +381,8 @@ class KinematicsTensorConfig:
     ):
         """Update sphere parameters
 
-        #NOTE: This currently does not update self collision distances.
+        NOTE: This currently does not update self collision distances.
+
         Args:
             link_name: _description_
             sphere_position_radius: _description_

src/curobo/cuda_robot_model/urdf_kinematics_parser.py

@@ -9,7 +9,7 @@
 # its affiliates is strictly prohibited.
 #
 # Standard Library
-from typing import Dict, Optional
+from typing import Dict, List, Optional
 
 # Third Party
 import numpy as np
@@ -33,12 +33,13 @@ class UrdfKinematicsParser(KinematicsParser):
         load_meshes: bool = False,
         mesh_root: str = "",
         extra_links: Optional[Dict[str, LinkParams]] = None,
+        build_scene_graph: bool = False,
     ) -> None:
         # load robot from urdf:
         self._robot = yourdfpy.URDF.load(
             urdf_path,
             load_meshes=load_meshes,
-            build_scene_graph=False,
+            build_scene_graph=build_scene_graph,
             mesh_dir=mesh_root,
             filename_handler=yourdfpy.filename_handler_null,
         )
@@ -46,63 +47,114 @@ class UrdfKinematicsParser(KinematicsParser):
 
     def build_link_parent(self):
         self._parent_map = {}
-        for j in self._robot.joint_map:
-            self._parent_map[self._robot.joint_map[j].child] = self._robot.joint_map[j].parent
-
-    def _find_parent_joint_of_link(self, link_name):
-        for j_idx, j in enumerate(self._robot.joint_map):
-            if self._robot.joint_map[j].child == link_name:
-                return j_idx, j
-        log_error("Link is not attached to any joint")
+        for jid, j in enumerate(self._robot.joint_map):
+            self._parent_map[self._robot.joint_map[j].child] = {
+                "parent": self._robot.joint_map[j].parent,
+                "jid": jid,
+                "joint_name": j,
+            }
 
     def _get_joint_name(self, idx):
         joint = self._robot.joint_names[idx]
         return joint
 
+    def _get_joint_limits(self, joint):
+        joint_type = joint.type
+        if joint_type != "continuous":
+            joint_limits = {
+                "effort": joint.limit.effort,
+                "lower": joint.limit.lower,
+                "upper": joint.limit.upper,
+                "velocity": joint.limit.velocity,
+            }
+        else:
+            log_warn("Converting continuous joint to revolute with limits[-10,10]")
+            joint_type = "revolute"
+            joint_limits = {
+                "effort": joint.limit.effort,
+                "lower": -10,
+                "upper": 10,
+                "velocity": joint.limit.velocity,
+            }
+        return joint_limits, joint_type
+
     def get_link_parameters(self, link_name: str, base=False) -> LinkParams:
         link_params = self._get_from_extra_links(link_name)
         if link_params is not None:
             return link_params
         body_params = {}
         body_params["link_name"] = link_name
-
+        mimic_joint_name = None
         if base:
             body_params["parent_link_name"] = None
             joint_transform = np.eye(4)
             joint_name = "base_joint"
+            active_joint_name = joint_name
             joint_type = "fixed"
             joint_limits = None
             joint_axis = None
             body_params["joint_id"] = 0
-        else:
-            body_params["parent_link_name"] = self._parent_map[link_name]
+            body_params["joint_type"] = JointType.FIXED
 
-            jid, joint_name = self._find_parent_joint_of_link(link_name)
+        else:
+            parent_data = self._parent_map[link_name]
+            body_params["parent_link_name"] = parent_data["parent"]
+
+            jid, joint_name = parent_data["jid"], parent_data["joint_name"]
             body_params["joint_id"] = jid
             joint = self._robot.joint_map[joint_name]
+            active_joint_name = joint_name
             joint_transform = joint.origin
             if joint_transform is None:
                 joint_transform = np.eye(4)
             joint_type = joint.type
             joint_limits = None
             joint_axis = None
+            body_params["joint_type"] = JointType.FIXED
+
             if joint_type != "fixed":
-                if joint_type != "continuous":
-                    joint_limits = {
-                        "effort": joint.limit.effort,
-                        "lower": joint.limit.lower,
-                        "upper": joint.limit.upper,
-                        "velocity": joint.limit.velocity,
-                    }
-                else:
-                    log_warn("Converting continuous joint to revolute")
-                    joint_type = "revolute"
-                    joint_limits = {
-                        "effort": joint.limit.effort,
-                        "lower": -3.14 * 2,
-                        "upper": 3.14 * 2,
-                        "velocity": joint.limit.velocity,
-                    }
+                joint_offset = [1.0, 0.0]
+                joint_limits, joint_type = self._get_joint_limits(joint)
+
+                if joint.mimic is not None:
+                    joint_offset = [joint.mimic.multiplier, joint.mimic.offset]
+                    # read joint limits of active joint:
+                    mimic_joint_name = joint_name
+                    active_joint_name = joint.mimic.joint
+                    active_joint = self._robot.joint_map[active_joint_name]
+                    active_joint_limits, active_joint_type = self._get_joint_limits(active_joint)
+                    # check to make sure mimic joint limits are not larger than active joint:
+                    if (
+                        active_joint_limits["lower"] * joint_offset[0] + joint_offset[1]
+                        < joint_limits["lower"]
+                    ):
+                        log_error(
+                            "mimic joint can go out of it's lower limit as active joint has larger range "
+                            + "FIX: make mimic joint's lower limit even lower "
+                            + active_joint_name
+                            + " "
+                            + mimic_joint_name
+                        )
+                    if (
+                        active_joint_limits["upper"] * joint_offset[0] + joint_offset[1]
+                        > joint_limits["upper"]
+                    ):
+                        log_error(
+                            "mimic joint can go out of it's upper limit as active joint has larger range "
+                            + "FIX: make mimic joint's upper limit higher"
+                            + active_joint_name
+                            + " "
+                            + mimic_joint_name
+                        )
+                    if active_joint_limits["velocity"] * joint_offset[0] > joint_limits["velocity"]:
+                        log_error(
+                            "mimic joint can move at higher velocity than active joint,"
+                            + "increase velocity limit for mimic joint"
+                            + active_joint_name
+                            + " "
+                            + mimic_joint_name
+                        )
+                    joint_limits = active_joint_limits
 
                 joint_axis = joint.axis
 
@@ -111,45 +163,33 @@ class UrdfKinematicsParser(KinematicsParser):
                     -1.0 * joint_limits["velocity"],
                     joint_limits["velocity"],
                 ]
+                if joint_type == "prismatic":
+                    if abs(joint_axis[0]) == 1:
+                        joint_type = JointType.X_PRISM
+                    if abs(joint_axis[1]) == 1:
+                        joint_type = JointType.Y_PRISM
+                    if abs(joint_axis[2]) == 1:
+                        joint_type = JointType.Z_PRISM
+                elif joint_type == "revolute":
+                    if abs(joint_axis[0]) == 1:
+                        joint_type = JointType.X_ROT
+                    if abs(joint_axis[1]) == 1:
+                        joint_type = JointType.Y_ROT
+                    if abs(joint_axis[2]) == 1:
+                        joint_type = JointType.Z_ROT
+                else:
+                    log_error("Joint type not supported")
+                if joint_axis[0] == -1 or joint_axis[1] == -1 or joint_axis[2] == -1:
+                    joint_offset[0] = -1.0 * joint_offset[0]
+                body_params["joint_type"] = joint_type
+                body_params["joint_offset"] = joint_offset
 
         body_params["fixed_transform"] = joint_transform
-        body_params["joint_name"] = joint_name
+        body_params["joint_name"] = active_joint_name
 
         body_params["joint_axis"] = joint_axis
+        body_params["mimic_joint_name"] = mimic_joint_name
 
-        if joint_type == "fixed":
-            joint_type = JointType.FIXED
-        elif joint_type == "prismatic":
-            if joint_axis[0] == 1:
-                joint_type = JointType.X_PRISM
-            if joint_axis[1] == 1:
-                joint_type = JointType.Y_PRISM
-            if joint_axis[2] == 1:
-                joint_type = JointType.Z_PRISM
-            if joint_axis[0] == -1:
-                joint_type = JointType.X_PRISM_NEG
-            if joint_axis[1] == -1:
-                joint_type = JointType.Y_PRISM_NEG
-            if joint_axis[2] == -1:
-                joint_type = JointType.Z_PRISM_NEG
-
-        elif joint_type == "revolute":
-            if joint_axis[0] == 1:
-                joint_type = JointType.X_ROT
-            if joint_axis[1] == 1:
-                joint_type = JointType.Y_ROT
-            if joint_axis[2] == 1:
-                joint_type = JointType.Z_ROT
-            if joint_axis[0] == -1:
-                joint_type = JointType.X_ROT_NEG
-            if joint_axis[1] == -1:
-                joint_type = JointType.Y_ROT_NEG
-            if joint_axis[2] == -1:
-                joint_type = JointType.Z_ROT_NEG
-        else:
-            log_error("Joint type not supported")
-
-        body_params["joint_type"] = joint_type
         link_params = LinkParams(**body_params)
 
         return link_params
@@ -194,3 +234,16 @@ class UrdfKinematicsParser(KinematicsParser):
             scale=m.scale,
             file_path=m.filename,
         )
+
+    @property
+    def root_link(self):
+        return self._robot.base_link
+
+    def get_controlled_joint_names(self) -> List[str]:
+        j_list = []
+        for k in self._parent_map.keys():
+            joint_name = self._parent_map[k]["joint_name"]
+            joint = self._robot.joint_map[joint_name]
+            if joint.type != "fixed" and joint.mimic is None:
+                j_list.append(joint_name)
+        return j_list

src/curobo/cuda_robot_model/usd_kinematics_parser.py

@@ -71,7 +71,7 @@ class UsdKinematicsParser(KinematicsParser):
         for l in all_joints:
             parent, child = get_links_for_joint(l)
             if child is not None and parent is not None:
-                self._parent_map[child.GetName()] = parent.GetName()
+                self._parent_map[child.GetName()] = {"parent": parent.GetName()}
                 self._parent_joint_map[child.GetName()] = l  # store joint prim
 
     def get_link_parameters(self, link_name: str, base: bool = False) -> LinkParams:
@@ -100,7 +100,7 @@ class UsdKinematicsParser(KinematicsParser):
             joint_type = JointType.FIXED
 
         else:
-            parent_link_name = self._parent_map[link_name]
+            parent_link_name = self._parent_map[link_name]["parent"]
             joint_prim = self._parent_joint_map[link_name]  # joint prim connects link
             joint_transform = self._get_joint_transform(joint_prim)
             joint_axis = None