Isaac Sim 4.0 support, Kinematics API doc, Windows support
This commit is contained in:
Balakumar Sundaralingam
170
src/curobo/util/xrdf_utils.py
Normal file
170
src/curobo/util/xrdf_utils.py
Normal file
@@ -0,0 +1,170 @@
|
||||
# Copyright (c) 2024 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.
|
||||
|
||||
# Standard Library
|
||||
from typing import Any, Dict, Optional
|
||||
|
||||
# CuRobo
|
||||
from curobo.cuda_robot_model.urdf_kinematics_parser import UrdfKinematicsParser
|
||||
from curobo.types.file_path import ContentPath
|
||||
from curobo.util.logger import log_error, log_warn
|
||||
from curobo.util_file import load_yaml
|
||||
|
||||
|
||||
def return_value_if_exists(input_dict: Dict, key: str, suffix: str = "xrdf") -> Any:
|
||||
if key not in input_dict:
|
||||
log_error(key + " key not found in " + suffix)
|
||||
return input_dict[key]
|
||||
|
||||
|
||||
def convert_xrdf_to_curobo(
|
||||
content_path: ContentPath = ContentPath(),
|
||||
input_xrdf_dict: Optional[Dict] = None,
|
||||
) -> Dict:
|
||||
|
||||
if content_path.robot_urdf_absolute_path is None:
|
||||
log_error(
|
||||
"content_path.robot_urdf_absolute_path or content_path.robot_urdf_file \
|
||||
is required."
|
||||
)
|
||||
urdf_path = content_path.robot_urdf_absolute_path
|
||||
if input_xrdf_dict is None:
|
||||
input_xrdf_dict = load_yaml(content_path.robot_xrdf_absolute_path)
|
||||
|
||||
if isinstance(content_path, str):
|
||||
log_error("content_path should be of type ContentPath")
|
||||
|
||||
if return_value_if_exists(input_xrdf_dict, "format") != "xrdf":
|
||||
log_error("format is not xrdf")
|
||||
raise ValueError("format is not xrdf")
|
||||
|
||||
if return_value_if_exists(input_xrdf_dict, "format_version") > 1.0:
|
||||
log_warn("format_version is greater than 1.0")
|
||||
# Also get base link as root of urdf
|
||||
kinematics_parser = UrdfKinematicsParser(
|
||||
urdf_path, mesh_root=content_path.robot_asset_absolute_path, build_scene_graph=True
|
||||
)
|
||||
joint_names = kinematics_parser.get_controlled_joint_names()
|
||||
base_link = kinematics_parser.root_link
|
||||
|
||||
output_dict = {}
|
||||
if "collision" in input_xrdf_dict:
|
||||
|
||||
coll_name = return_value_if_exists(input_xrdf_dict["collision"], "geometry")
|
||||
|
||||
if "spheres" not in input_xrdf_dict["geometry"][coll_name]:
|
||||
log_error("spheres key not found in xrdf")
|
||||
coll_spheres = return_value_if_exists(input_xrdf_dict["geometry"][coll_name], "spheres")
|
||||
output_dict["collision_spheres"] = coll_spheres
|
||||
|
||||
buffer_distance = return_value_if_exists(input_xrdf_dict["collision"], "buffer_distance")
|
||||
output_dict["collision_sphere_buffer"] = buffer_distance
|
||||
output_dict["collision_link_names"] = list(coll_spheres.keys())
|
||||
|
||||
if "self_collision" in input_xrdf_dict:
|
||||
if (
|
||||
input_xrdf_dict["self_collision"]["geometry"]
|
||||
!= input_xrdf_dict["collision"]["geometry"]
|
||||
):
|
||||
log_error("self_collision geometry does not match collision geometry")
|
||||
|
||||
self_collision_ignore = return_value_if_exists(
|
||||
input_xrdf_dict["self_collision"],
|
||||
"ignore",
|
||||
)
|
||||
|
||||
self_collision_buffer = return_value_if_exists(
|
||||
input_xrdf_dict["self_collision"],
|
||||
"buffer_distance",
|
||||
)
|
||||
|
||||
output_dict["self_collision_ignore"] = self_collision_ignore
|
||||
output_dict["self_collision_buffer"] = self_collision_buffer
|
||||
else:
|
||||
log_error("self_collision key not found in xrdf")
|
||||
else:
|
||||
log_warn("collision key not found in xrdf, collision avoidance is disabled")
|
||||
|
||||
tool_frames = return_value_if_exists(input_xrdf_dict, "tool_frames")
|
||||
output_dict["ee_link"] = tool_frames[0]
|
||||
output_dict["link_names"] = None
|
||||
if len(tool_frames) > 1:
|
||||
output_dict["link_names"] = input_xrdf_dict["tool_frames"]
|
||||
|
||||
# cspace:
|
||||
cspace_dict = return_value_if_exists(input_xrdf_dict, "cspace")
|
||||
|
||||
active_joints = return_value_if_exists(cspace_dict, "joint_names")
|
||||
|
||||
default_joint_positions = return_value_if_exists(input_xrdf_dict, "default_joint_positions")
|
||||
active_config = []
|
||||
locked_joints = {}
|
||||
|
||||
for j in joint_names:
|
||||
if j in active_joints:
|
||||
if j in default_joint_positions:
|
||||
active_config.append(default_joint_positions[j])
|
||||
else:
|
||||
active_config.append(0.0)
|
||||
else:
|
||||
locked_joints[j] = 0.0
|
||||
if j in default_joint_positions:
|
||||
locked_joints[j] = default_joint_positions[j]
|
||||
|
||||
acceleration_limits = return_value_if_exists(cspace_dict, "acceleration_limits")
|
||||
jerk_limits = return_value_if_exists(cspace_dict, "jerk_limits")
|
||||
|
||||
max_acc = max(acceleration_limits)
|
||||
max_jerk = max(jerk_limits)
|
||||
output_dict["lock_joints"] = locked_joints
|
||||
all_joint_names = active_joints + list(locked_joints.keys())
|
||||
output_cspace = {
|
||||
"joint_names": all_joint_names,
|
||||
"retract_config": active_config + list(locked_joints.values()),
|
||||
"null_space_weight": [1.0 for _ in range(len(all_joint_names))],
|
||||
"cspace_distance_weight": [1.0 for _ in range(len(all_joint_names))],
|
||||
"max_acceleration": acceleration_limits
|
||||
+ [max_acc for _ in range(len(all_joint_names) - len(active_joints))],
|
||||
"max_jerk": jerk_limits
|
||||
+ [max_jerk for _ in range(len(all_joint_names) - len(active_joints))],
|
||||
}
|
||||
|
||||
output_dict["cspace"] = output_cspace
|
||||
|
||||
extra_links = {}
|
||||
if "modifiers" in input_xrdf_dict:
|
||||
for k in range(len(input_xrdf_dict["modifiers"])):
|
||||
mod_list = list(input_xrdf_dict["modifiers"][k].keys())
|
||||
if len(mod_list) > 1:
|
||||
log_error("Each modifier should have only one key")
|
||||
raise ValueError("Each modifier should have only one key")
|
||||
mod_type = mod_list[0]
|
||||
if mod_type == "set_base_frame":
|
||||
base_link = input_xrdf_dict["modifiers"][k]["set_base_frame"]
|
||||
elif mod_type == "add_frame":
|
||||
frame_data = input_xrdf_dict["modifiers"][k]["add_frame"]
|
||||
extra_links[frame_data["frame_name"]] = {
|
||||
"parent_link_name": frame_data["parent_frame_name"],
|
||||
"link_name": frame_data["frame_name"],
|
||||
"joint_name": frame_data["joint_name"],
|
||||
"joint_type": frame_data["joint_type"],
|
||||
"fixed_transform": frame_data["fixed_transform"]["position"]
|
||||
+ [frame_data["fixed_transform"]["orientation"]["w"]]
|
||||
+ frame_data["fixed_transform"]["orientation"]["xyz"],
|
||||
}
|
||||
else:
|
||||
log_warn('XRDF modifier "' + mod_type + '" not recognized')
|
||||
output_dict["extra_links"] = extra_links
|
||||
|
||||
output_dict["base_link"] = base_link
|
||||
|
||||
output_dict["urdf_path"] = urdf_path
|
||||
|
||||
output_dict = {"robot_cfg": {"kinematics": output_dict}}
|
||||
return output_dict
|
||||
Reference in New Issue
Block a user