Error importing URDF for cuRobo

  1. cuRobo installation mode: docker via isaac_ros_common/docker/Dockerfile.ros2_humble - branch: 855d408
  2. python version: 3.10.12
  3. curobo: isaac_ros_cumotion via branch: 3bfed9d
  4. start via: ros2 run isaac_ros_cumotion cumotion_planner_node --ros-args -p robot:=kr1.xrdf -p urdf_path:=kr1.urdf

There is an error in the _build_kinematics_with_lock_joints() function when you load a robot with joints that are not fixed and also not in the robot arm.

  File "/home/bren/ws_humanoid/build/curobo_core/curobo/src/curobo/cuda_robot_model/cuda_robot_generator.py", line 527, in _build_kinematics_with_lock_joints
    if "mimic" in joint_data[k]:
KeyError: 'left_wheel_joint'

I can solve this with this modified code:

        for k in lock_joint_names:
            if k in joint_data:
                    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

This then leads to some other problem that I managed to solve, but as I don’t 100% know what this code is doing I haven’t created a pull request.

With a print statement this is the joints in the 2 dicts:

self.joint_names:  ['arm_joint_1', 'arm_joint_2', 'arm_joint_3', 'arm_joint_4', 'arm_joint_5', 'arm_joint_6', 'arm_joint_7']
joint_data: {}
lock_joint_names:  ['left_wheel_joint', 'right_wheel_joint', 'torso_joint_1', 'torso_joint_2', 'gripper_motor_joint']

My kr1.xrdf file:

format: xrdf
format_version: 1.0

modifiers:
  - set_base_frame: "base_link"

default_joint_positions:
  arm_joint_1: 0.8508
  arm_joint_2: -1.0055
  arm_joint_3: -1.4077
  arm_joint_4: -2.1193
  arm_joint_5: 2.8
  arm_joint_6: 0.8199
  arm_joint_7: 0.0

cspace:
  joint_names:
    - "arm_joint_1"
    - "arm_joint_2"
    - "arm_joint_3"
    - "arm_joint_4"
    - "arm_joint_5"
    - "arm_joint_6"
    - "arm_joint_7"
  acceleration_limits: [12.0, 12.0, 12.0, 12.0, 12.0, 12.0, 12.0]
  jerk_limits: [500.0, 500.0, 500.0, 500.0, 500.0, 500.0, 500.0]

tool_frames: ["arm_link_7"]

Hi @bren1

Thank you for opening the topic. I tried to replicate the error running this demo, but I didn’t encounter any errors. isaac_ros_cumotion_moveit — isaac_ros_docs documentation

To better support you, which demo have you run?

Best,
Raffaello

Thank you for your feedback. And yes the demo works, but If I modify the URDF for the UR5 to add a new link then it fails with the same error that I get with my custom URDF. This is the simple link that I added:

  <!-- Drive wheels-->
  <link name="left_wheel_link"/>

  <joint name="left_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="left_wheel_link"/>
    <origin rpy="-1.57079632679 0 0" xyz="0.0 0.2211 0.0"/>
    <axis xyz="0 0 1"/>
  </joint>

With the same error:

[INFO]: Loaded robot file name: ur10e.xrdf
[INFO]: warming up cuMotion, wait until ready
Traceback (most recent call last):
  File "/home/bren/ws_humanoid/install/isaac_ros_cumotion/lib/isaac_ros_cumotion/cumotion_planner_node", line 33, in <module>
    sys.exit(load_entry_point('isaac-ros-cumotion', 'console_scripts', 'cumotion_planner_node')())
  File "/home/bren/ws_humanoid/build/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py", line 711, in main
    cumotion_action_server = CumotionActionServer()
  File "/home/bren/ws_humanoid/build/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py", line 159, in __init__
    self.warmup()
  File "/home/bren/ws_humanoid/build/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py", line 226, in warmup
    motion_gen_config = MotionGenConfig.load_from_robot_config(
  File "/home/bren/ws_humanoid/build/curobo_core/curobo/src/curobo/wrap/reacher/motion_gen.py", line 595, in load_from_robot_config
    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
  File "/home/bren/ws_humanoid/build/curobo_core/curobo/src/curobo/types/robot.py", line 50, in from_dict
    data_dict["kinematics"] = CudaRobotModelConfig.from_config(
  File "/home/bren/ws_humanoid/build/curobo_core/curobo/src/curobo/cuda_robot_model/cuda_robot_model.py", line 126, in from_config
    generator = CudaRobotGenerator(config)
  File "/home/bren/ws_humanoid/build/curobo_core/curobo/src/curobo/cuda_robot_model/cuda_robot_generator.py", line 225, in __init__
    self.initialize_tensors()
  File "/usr/lib/python3.10/contextlib.py", line 79, in inner
    return func(*args, **kwds)
  File "/home/bren/ws_humanoid/build/curobo_core/curobo/src/curobo/cuda_robot_model/cuda_robot_generator.py", line 298, in initialize_tensors
    self._build_kinematics_with_lock_joints(
  File "/usr/lib/python3.10/contextlib.py", line 79, in inner
    return func(*args, **kwds)
  File "/home/bren/ws_humanoid/build/curobo_core/curobo/src/curobo/cuda_robot_model/cuda_robot_generator.py", line 525, in _build_kinematics_with_lock_joints
    if "mimic" in joint_data[k]:
KeyError: 'left_wheel_joint'
[ros2run]: Process exited with failure 1

Maybe you can test on your end, I’ve included the modified urdf?

ur10e.urdf.txt (23.3 KB)

So I got it working:

  1. created a new URDF with just the arm with out the body of the robot and also the gripper.
  2. create a simple tool0 link to replace the gripper URDF
  3. Then use this new simplified URDF For the controller and have the more complete URDF for the main “robot description”

Bren

I encountered the same problem, have you solved it