kinematics refactor with mimic joint support

CHANGELOG.md

@@ -10,6 +10,23 @@ its affiliates is strictly prohibited.
 -->
 # Changelog
 
+## Latest Commit
+
+### New Features
+- Add mimic joint parsing and optimization support. Check `ur5e_robotiq_2f_140.yml`.
+
+### BugFixes & Misc.
+- Fix bug in `WorldVoxelCollision` where `env_query_idx` was being overwritten.
+- Fix bug in `WorldVoxelCollision` where parent collision types were not getting called in some cases.
+- Change voxelization dimensions to include 1 extra voxel per dim.
+- Added `seed` parameter to `IKSolverConfig`.
+- Added `sampler_seed` parameter `RolloutConfig`.
+- Fixed bug in `links_goal_pose` where tensor could be non contiguous.
+- Improved `ik_solver` success by removing gaussian projection of seed samples.
+- Added flag to sample from ik seeder instead of `rollout_fn` sampler.
+- Added ik startup profiler to `benchmark/curobo_python_profile.py`.
+- Reduced branching in Kinematics kernels and added mimic joint computations.
+
 ## Version 0.7.0
 ### Changes in default behavior
 - Increased default collision cache to 50 in RobotWorld.

benchmark/curobo_benchmark.py

@@ -154,7 +154,7 @@ def load_curobo(
 
     # del robot_cfg["kinematics"]
     if ik_seeds is None:
-        ik_seeds = 24
+        ik_seeds = 32
 
     if graph_mode:
         trajopt_seeds = 4
@@ -231,7 +231,7 @@ def benchmark_mb(
     # load dataset:
     force_graph = False
 
-    file_paths = [motion_benchmaker_raw, mpinets_raw]
+    file_paths = [motion_benchmaker_raw, mpinets_raw][:]
     if args.demo:
         file_paths = [demo_raw]
 
@@ -500,8 +500,8 @@ def benchmark_mb(
                             write_robot_usd_path="benchmark/log/usd/assets/",
                             robot_usd_local_reference="assets/",
                             base_frame="/world_" + problem_name,
-                            visualize_robot_spheres=False,
-                            flatten_usd=False,
+                            visualize_robot_spheres=True,
+                            flatten_usd=True,
                         )
 
                     if write_plot:  # and result.optimized_dt.item() > 0.06:
@@ -554,12 +554,12 @@ def benchmark_mb(
                         start_state,
                         Pose.from_list(pose),
                         join_path("benchmark/log/usd/", problem_name) + "_debug",
-                        write_ik=False,
+                        write_ik=True,
                         write_trajopt=True,
-                        visualize_robot_spheres=False,
+                        visualize_robot_spheres=True,
                         grid_space=2,
                         write_robot_usd_path="benchmark/log/usd/assets/",
-                        # flatten_usd=True,
+                        flatten_usd=True,
                     )
                     print(result.status)
 

benchmark/curobo_python_profile.py

@@ -24,39 +24,52 @@
 from curobo.types.base import TensorDeviceType
 from curobo.types.math import Pose
 from curobo.types.robot import JointState, RobotConfig
+from curobo.util.logger import setup_curobo_logger
 from curobo.util_file import get_robot_configs_path, get_robot_path, join_path, load_yaml
+from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
 from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
 
 
-def demo_motion_gen():
-    # Standard Library
-
+def demo_motion_gen(robot_file, motion_gen=None):
     st_time = time.time()
-    tensor_args = TensorDeviceType()
-    world_file = "collision_table.yml"
-    robot_file = "franka.yml"
-    motion_gen_config = MotionGenConfig.load_from_robot_config(
-        robot_file,
-        world_file,
-        tensor_args,
-        trajopt_tsteps=44,
-        collision_checker_type=CollisionCheckerType.PRIMITIVE,
-        use_cuda_graph=False,
-        num_trajopt_seeds=24,
-        num_graph_seeds=24,
-        evaluate_interpolated_trajectory=True,
-        interpolation_dt=0.02,
-    )
-    motion_gen = MotionGen(motion_gen_config)
 
-    # st_time = time.time()
-    motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False)
-    print("motion gen time:", time.time() - st_time)
+    if motion_gen is None:
+        setup_curobo_logger("warn")
+        # Standard Library
+
+        tensor_args = TensorDeviceType()
+        world_file = "collision_table.yml"
+        robot_file = "ur5e.yml"
+        motion_gen_config = MotionGenConfig.load_from_robot_config(
+            robot_file,
+            world_file,
+            tensor_args,
+            trajopt_tsteps=32,
+            collision_checker_type=CollisionCheckerType.PRIMITIVE,
+            use_cuda_graph=True,
+            num_trajopt_seeds=4,
+            num_graph_seeds=4,
+            evaluate_interpolated_trajectory=True,
+            interpolation_dt=0.02,
+        )
+        motion_gen = MotionGen(motion_gen_config)
+
+        # st_time = time.time()
+        torch.cuda.synchronize()
+        print("LOAD TIME: ", time.time() - st_time)
+        st_time = time.time()
+
+        motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False)
+
+        torch.cuda.synchronize()
+
+        print("warmup TIME: ", time.time() - st_time)
+
+        return motion_gen
 
     # print(time.time() - st_time)
     # return
     retract_cfg = motion_gen.get_retract_config()
-    print(retract_cfg)
     state = motion_gen.rollout_fn.compute_kinematics(
         JointState.from_position(retract_cfg.view(1, -1))
     )
@@ -66,15 +79,58 @@ def demo_motion_gen():
     result = motion_gen.plan(
         start_state,
         retract_pose,
-        enable_graph=True,
+        enable_graph=False,
         enable_opt=True,
         max_attempts=1,
-        need_graph_success=True,
+        # need_graph_success=True,
     )
-    print(result.status)
-    print(result.optimized_plan.position.shape)
     traj = result.get_interpolated_plan()  # $.position.view(-1, 7)  # optimized plan
-    print("Trajectory Generated: ", result.success, result.optimized_dt.item())
+    print("Trajectory Generated: ", result.success, time.time() - st_time)
+    return motion_gen
+
+
+def demo_basic_ik(config_file="ur10e.yml"):
+    st_time = time.time()
+
+    tensor_args = TensorDeviceType()
+
+    config_file = load_yaml(join_path(get_robot_configs_path(), config_file))
+    urdf_file = config_file["kinematics"]["urdf_path"]  # Send global path starting with "/"
+    base_link = config_file["kinematics"]["base_link"]
+    ee_link = config_file["kinematics"]["ee_link"]
+    robot_cfg = RobotConfig.from_basic(urdf_file, base_link, ee_link, tensor_args)
+
+    ik_config = IKSolverConfig.load_from_robot_config(
+        robot_cfg,
+        None,
+        rotation_threshold=0.05,
+        position_threshold=0.005,
+        num_seeds=20,
+        self_collision_check=False,
+        self_collision_opt=False,
+        tensor_args=tensor_args,
+        use_cuda_graph=False,
+    )
+    ik_solver = IKSolver(ik_config)
+    torch.cuda.synchronize()
+    print("IK load time:", time.time() - st_time)
+    st_time = time.time()
+    # print(kin_state)
+
+    q_sample = ik_solver.sample_configs(100)
+    kin_state = ik_solver.fk(q_sample)
+    goal = Pose(kin_state.ee_position, kin_state.ee_quaternion)
+
+    torch.cuda.synchronize()
+    print("FK time:", time.time() - st_time)
+
+    st_time = time.time()
+    result = ik_solver.solve_batch(goal)
+    torch.cuda.synchronize()
+    print(
+        "Cold Start Solve Time(s) ",
+        result.solve_time,
+    )
 
 
 def demo_basic_robot():
@@ -122,7 +178,7 @@ def demo_full_config_robot(config_file):
     parser.add_argument(
         "--save_path",
         type=str,
-        default="log/trace",
+        default="benchmark/log/trace",
         help="path to save file",
     )
     parser.add_argument(
@@ -137,12 +193,24 @@ def demo_full_config_robot(config_file):
         help="When True, runs startup for motion generation",
         default=False,
     )
+    parser.add_argument(
+        "--motion_gen_plan",
+        action="store_true",
+        help="When True, runs startup for motion generation",
+        default=False,
+    )
     parser.add_argument(
         "--kinematics",
         action="store_true",
         help="When True, runs startup for kinematics",
         default=False,
     )
+    parser.add_argument(
+        "--inverse_kinematics",
+        action="store_true",
+        help="When True, runs startup for kinematics",
+        default=False,
+    )
     parser.add_argument(
         "--motion_gen_once",
         action="store_true",
@@ -172,21 +240,59 @@ def demo_full_config_robot(config_file):
         filename = join_path(args.save_path, args.file_name) + "_kinematics_trace.json"
         prof.export_chrome_trace(filename)
 
+    if args.inverse_kinematics:
+        with profile(activities=[ProfilerActivity.CPU, ProfilerActivity.CUDA]) as prof:
+            demo_basic_ik(config_file)
+        filename = join_path(args.save_path, args.file_name) + "_inverse_kinematics_trace.json"
+        prof.export_chrome_trace(filename)
+
+        pr = cProfile.Profile()
+        pr.enable()
+        demo_basic_ik(config_file)
+        pr.disable()
+        filename = join_path(args.save_path, args.file_name) + "_inverse_kinematics_cprofile.prof"
+        pr.dump_stats(filename)
+
     if args.motion_gen_once:
-        demo_motion_gen()
+        pr = cProfile.Profile()
+        pr.enable()
+        demo_motion_gen(config_file)
+        pr.disable()
+        filename = join_path(args.save_path, args.file_name) + "_motion_gen_cprofile.prof"
+        pr.dump_stats(filename)
+
+        with profile(activities=[ProfilerActivity.CPU, ProfilerActivity.CUDA]) as prof:
+            demo_motion_gen(config_file)
+        filename = join_path(args.save_path, args.file_name) + "_motion_gen_trace.json"
+        prof.export_chrome_trace(filename)
+
+    if args.motion_gen_plan:
+        motion_gen = demo_motion_gen(config_file)
+
+        pr = cProfile.Profile()
+        pr.enable()
+        demo_motion_gen(config_file, motion_gen)
+        pr.disable()
+        filename = join_path(args.save_path, args.file_name) + "_motion_gen_plan_cprofile.prof"
+        pr.dump_stats(filename)
+
+        with profile(activities=[ProfilerActivity.CPU, ProfilerActivity.CUDA]) as prof:
+            demo_motion_gen(config_file, motion_gen)
+        filename = join_path(args.save_path, args.file_name) + "_motion_gen_plan_trace.json"
+        prof.export_chrome_trace(filename)
 
     if args.motion_gen:
         for _ in range(5):
-            demo_motion_gen()
+            demo_motion_gen(config_file)
 
         pr = cProfile.Profile()
         pr.enable()
-        demo_motion_gen()
+        demo_motion_gen(config_file)
         pr.disable()
         filename = join_path(args.save_path, args.file_name) + "_motion_gen_cprofile.prof"
         pr.dump_stats(filename)
 
         with profile(activities=[ProfilerActivity.CPU, ProfilerActivity.CUDA]) as prof:
-            demo_motion_gen()
+            demo_motion_gen(config_file)
         filename = join_path(args.save_path, args.file_name) + "_motion_gen_trace.json"
         prof.export_chrome_trace(filename)

benchmark/ik_benchmark.py

@@ -67,7 +67,7 @@ def run_full_config_collision_free_ik(
         robot_cfg,
         world_cfg,
         position_threshold=position_threshold,
-        num_seeds=20,
+        num_seeds=30,
         self_collision_check=collision_free,
         self_collision_opt=collision_free,
         tensor_args=tensor_args,

src/curobo/content/assets/robot/franka_description/franka_panda_mobile_xy_tz.urdf

@@ -324,7 +324,6 @@
     <axis xyz="0 -1 0"/>
     <dynamics damping="10.0"/>
     <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
-    <mimic joint="panda_finger_joint1"/>
   </joint>
 
 </robot>

src/curobo/content/assets/robot/kinova/kortex_description/grippers/robotiq_2f_140/LICENSE

@@ -0,0 +1,23 @@
+Copyright (c) 2013, ROS-Industrial
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+  Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+  Redistributions in binary form must reproduce the above copyright notice, this
+  list of conditions and the following disclaimer in the documentation and/or
+  other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

src/curobo/content/assets/robot/kinova/kortex_description/grippers/robotiq_2f_140/README.md

@@ -0,0 +1,12 @@
+This is the README file from Robotiq's package for the 2F-140 gripper. The package's license file can also be found in this folder.
+We used commit `4e3196c1c6a83f54acfa3dd8cf02b575ebba3e53` from [Robotiq's ROS Industrial Github repository](https://github.com/ros-industrial/robotiq)
+
+# Robotiq 140mm 2-Finger-Adaptive-Gripper 
+
+This package contains the URDF files describing the 140mm stroke gripper from robotiq, also known as series **C3**.
+
+## Robot Visual
+![140](https://user-images.githubusercontent.com/8356912/49428409-463f8580-f7a6-11e8-8278-5246acdc5c14.png)
+
+## Robot Collision 
+![1402](https://user-images.githubusercontent.com/8356912/49428407-463f8580-f7a6-11e8-9c4e-df69e478f107.png)