kinematics refactor with mimic joint support

tests/kinematics_parser_test.py

@@ -73,7 +73,7 @@ def usd_parser(robot_params_all):
 def urdf_parser(robot_params_all):
     robot_params = robot_params_all["kinematics"]
     robot_urdf = join_path(get_assets_path(), robot_params["urdf_path"])
-    kinematics_parser = UrdfKinematicsParser(robot_urdf)
+    kinematics_parser = UrdfKinematicsParser(robot_urdf, build_scene_graph=True)
     return kinematics_parser
 
 
@@ -140,3 +140,8 @@ def test_basic_ee_pose(usd_cuda_robot, urdf_cuda_robot, retract_state):
     p_d, q_d = usd_pose.distance(urdf_pose)
     assert p_d < 1e-5
     assert q_d < 1e-5
+
+
+def test_urdf_parser(urdf_parser):
+    assert urdf_parser.root_link == "base_link"
+    assert len(urdf_parser.get_controlled_joint_names()) == 9

tests/mimic_joint_test.py

@@ -0,0 +1,38 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# CuRobo
+from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel, CudaRobotModelConfig
+from curobo.types.state import JointState
+
+
+def test_mimic_config():
+    cfg = CudaRobotModelConfig.from_robot_yaml_file("simple_mimic_robot.yml", "ee_link")
+
+    # print(cfg.kinematics_config.fixed_transforms)
+    robot_model = CudaRobotModel(cfg)
+    q = JointState.from_position(robot_model.retract_config, joint_names=robot_model.joint_names)
+    q = robot_model.get_full_js(q)
+
+    # print(q)
+    q_mimic = robot_model.get_mimic_js(q)
+    assert len(q_mimic) == 3
+
+
+def test_robotiq_mimic_config():
+    cfg = CudaRobotModelConfig.from_robot_yaml_file("ur5e_robotiq_2f_140.yml", "grasp_frame")
+
+    robot_model = CudaRobotModel(cfg)
+    q = JointState.from_position(robot_model.retract_config, joint_names=robot_model.joint_names)
+    q = robot_model.get_full_js(q)
+
+    q_mimic = robot_model.get_mimic_js(q)
+    assert len(q_mimic.joint_names) == 12

tests/multi_pose_test.py

@@ -10,6 +10,7 @@
 #
 
 # Third Party
+import pytest
 import torch
 
 # CuRobo
@@ -21,7 +22,11 @@ from curobo.util_file import get_robot_configs_path, get_world_configs_path, joi
 from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
 
 
-def test_multi_pose_franka():
+@pytest.mark.parametrize(
+    "b_size",
+    [1, 10, 100],
+)
+def test_multi_pose_franka(b_size: int):
     tensor_args = TensorDeviceType()
     world_file = "collision_cubby.yml"
 
@@ -44,13 +49,14 @@ def test_multi_pose_franka():
         tensor_args=tensor_args,
     )
     ik_solver = IKSolver(ik_config)
-    b_size = 1
 
     q_sample = ik_solver.sample_configs(b_size)
     kin_state = ik_solver.fk(q_sample)
     link_poses = kin_state.link_pose
     goal = Pose(kin_state.ee_position, kin_state.ee_quaternion)
-    result = ik_solver.solve_single(goal, link_poses=link_poses)
+    result = ik_solver.solve_batch(goal, link_poses=link_poses)
 
     success = result.success
-    assert torch.count_nonzero(success).item() >= 1.0  # we check if atleast 90% are successful
+    assert (
+        torch.count_nonzero(success).item() / b_size >= 0.9
+    )  # we check if atleast 90% are successful

tests/voxel_collision_test.py

@@ -184,7 +184,7 @@ def test_primitive_voxel_sphere_distance(world_collision):
 
     error = torch.abs(d_cuboid.view(-1) - d_voxel.view(-1))
 
-    assert torch.max(error) < voxel_grid.voxel_size
+    assert torch.max(error) - voxel_grid.voxel_size < 1e-3
 
 
 @pytest.mark.parametrize(
@@ -243,12 +243,11 @@ def test_primitive_voxel_sphere_gradient(world_collision):
 
     error = torch.abs(d_cuboid.view(-1) - d_voxel.view(-1))
 
-    assert torch.max(error) < voxel_grid.voxel_size
+    assert torch.max(error) - voxel_grid.voxel_size < 1e-3
 
     cuboid_gradient = cuboid_collision_buffer.get_gradient_buffer()
 
     voxel_gradient = voxel_collision_buffer.get_gradient_buffer()
     error = torch.linalg.norm(cuboid_gradient - voxel_gradient, dim=-1)
-    print(cuboid_gradient)
-    print(voxel_gradient)
-    assert torch.max(error) < voxel_grid.voxel_size
+
+    assert torch.max(error) - voxel_grid.voxel_size < 1e-3

tests/voxelization_test.py

@@ -107,12 +107,12 @@ def test_voxels_from_world(world_collision):
     assert voxels.shape[0] > 4
 
 
-@pytest.mark.skip(reason="Not ready yet.")
+# @pytest.mark.skip(reason="Not ready yet.")
 @pytest.mark.parametrize(
     "world_collision",
     [
         (True, True),
-        (False, True),
+        # (False, True),
     ],
     indirect=True,
 )
@@ -156,9 +156,8 @@ def test_esdf_prim_mesh(world_collision, world_collision_primitive):
     voxel_size = 0.1
     esdf = world_collision.get_esdf_in_bounding_box(voxel_size=voxel_size).clone()
     esdf_prim = world_collision_primitive.get_esdf_in_bounding_box(voxel_size=voxel_size).clone()
-    print(esdf.voxel_size, esdf_prim.voxel_size)
-    voxels = esdf.get_occupied_voxels()
-    voxels_prim = esdf_prim.get_occupied_voxels()
+    voxels = esdf.get_occupied_voxels(voxel_size)
+    voxels_prim = esdf_prim.get_occupied_voxels(voxel_size)
     assert voxels.shape == voxels_prim.shape