hofee/gen_data_curobo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Isaac Sim 4.0 support, Kinematics API doc, Windows support

Browse Source
This commit is contained in:
Balakumar Sundaralingam
2024-07-20 14:51:43 -07:00
parent 2ae381f328
commit 3690d28c54
83 changed files with 2818 additions and 497 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
tests/conftest.py
View File

@@ -14,3 +14,4 @@ import os
os.environ["CUROBO_TORCH_COMPILE_DISABLE"] = str(1)
os.environ["CUROBO_USE_LRU_CACHE"] = str(1)
os.environ["CUROBO_TORCH_CUDA_GRAPH_RESET"] = str(0)

75
tests/cuda_graph_test.py Normal file
View File

@@ -0,0 +1,75 @@
#
# 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.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState, RobotConfig
from curobo.util_file import get_robot_configs_path, join_path, load_yaml
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig
from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
def test_motion_gen_mpc():
tensor_args = TensorDeviceType()
world_file = "collision_test.yml"
robot_file = "ur5e.yml"
motion_gen_config = MotionGenConfig.load_from_robot_config(
robot_file,
world_file,
tensor_args,
use_cuda_graph=True,
)
motion_gen = MotionGen(motion_gen_config)
robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
retract_cfg = motion_gen.get_retract_config()
state = motion_gen.rollout_fn.compute_kinematics(
JointState.from_position(retract_cfg.view(1, -1))
)
motion_gen.warmup(warmup_js_trajopt=False)
retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze())
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.5)
tensor_args = TensorDeviceType()
robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
world_file = "collision_test.yml"
if True:
new_motion_gen_config = MotionGenConfig.load_from_robot_config(
robot_file,
world_file,
tensor_args,
use_cuda_graph=True,
)
new_motion_gen = MotionGen(new_motion_gen_config)
new_motion_gen.warmup()
if True:
mpc_config = MpcSolverConfig.load_from_robot_config(
robot_file,
world_file,
use_cuda_graph=True,
use_cuda_graph_metrics=False,
use_cuda_graph_full_step=False,
)
mpc = MpcSolver(mpc_config)
retract_cfg = robot_cfg.cspace.retract_config.view(1, -1)
result = motion_gen.plan(start_state, retract_pose, enable_graph=False)
assert result.success
result = new_motion_gen.plan(start_state, retract_pose, enable_graph=False)
assert result.success

46
tests/kinematics_test.py
View File

@@ -18,14 +18,18 @@ from curobo.cuda_robot_model.cuda_robot_generator import CudaRobotGeneratorConfi
from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel, CudaRobotModelConfig
from curobo.cuda_robot_model.types import CSpaceConfig
from curobo.geom.transform import matrix_to_quaternion, quaternion_to_matrix
from curobo.geom.types import Cuboid
from curobo.types.base import TensorDeviceType
from curobo.types.math import quat_multiply
from curobo.types.state import JointState
from curobo.util_file import get_robot_configs_path, join_path, load_yaml
@pytest.fixture(scope="module")
def cfg():
cfg = CudaRobotModelConfig.from_robot_yaml_file("franka.yml", "panda_hand")
robot_data = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))
robot_data["robot_cfg"]["kinematics"]["extra_collision_spheres"] = {"attached_object": 100}
cfg = CudaRobotModelConfig.from_robot_yaml_file(robot_data, "panda_hand")
return cfg
@@ -121,11 +125,11 @@ def test_franka_attach_object_kinematics(cfg):
attached_spheres = state.link_spheres_tensor[:, sph_idx, :]
# check if all values are equal to position and quaternion
assert torch.norm(attached_spheres[:, :, 0] - 0.0829) < 0.1
assert torch.norm(attached_spheres[:, :, 0] - attached_spheres[0, 0, 0]) < 0.1
assert torch.norm(attached_spheres[:, :, 1]) < 0.0001
# attach an object:
new_object = torch.zeros((4, 4), **(tensor_args.as_torch_dict()))
new_object = torch.zeros((100, 4), **(tensor_args.as_torch_dict()))
new_object[:2, 3] = 0.01
new_object[:2, 1] = 0.1
# print(attached_spheres[:, sph_idx,:].shape)
@@ -145,6 +149,42 @@ def test_franka_attach_object_kinematics(cfg):
robot_model.kinematics_config.detach_object()
def test_franka_attach_external_object_kinematics(cfg):
tensor_args = TensorDeviceType()
robot_model = CudaRobotModel(cfg)
q_test = torch.as_tensor(
[0.0, -1.2, 0.0, -2.0, 0.0, 1.0, 0.0], **(tensor_args.as_torch_dict())
).view(1, -1)
sph_idx = robot_model.kinematics_config.get_sphere_index_from_link_name("attached_object")
state = robot_model.get_state(q_test.clone())
attached_spheres = state.link_spheres_tensor[:, sph_idx, :]
# check if all values are equal to position and quaternion
sphere = attached_spheres[0, 0, 0].item()
assert torch.norm(attached_spheres[:, :, 0] - sphere) < 0.01
external_object = Cuboid(
name="external_object", dims=[0.1, 0.1, 0.1], pose=[0, 0, 0, 1, 0, 0, 0]
)
robot_model.attach_external_objects_to_robot(
JointState.from_position(q_test.clone()), [external_object]
)
state = robot_model.get_state(q_test.clone())
attached_spheres = state.link_spheres_tensor[:, sph_idx, :]
assert torch.norm(attached_spheres[:, :, 0] - sphere) > 0.1
robot_model.kinematics_config.detach_object()
state = robot_model.get_state(q_test.clone())
attached_spheres = state.link_spheres_tensor[:, sph_idx, :]
assert torch.norm(attached_spheres[:, :, 0] - sphere) < 0.01
assert torch.norm(attached_spheres[:, :, 1]) < 0.0001
robot_model.kinematics_config.detach_object()
def test_locked_joints_kinematics():
tensor_args = TensorDeviceType()

5
tests/motion_gen_constrained_test.py
View File

@@ -21,11 +21,12 @@ from curobo.wrap.reacher.motion_gen import (
MotionGen,
MotionGenConfig,
MotionGenPlanConfig,
MotionGenStatus,
PoseCostMetric,
)
@pytest.fixture(scope="module")
@pytest.fixture(scope="function")
def motion_gen(request):
tensor_args = TensorDeviceType()
world_file = "collision_table.yml"
@@ -108,7 +109,7 @@ def test_reach_only_position(motion_gen):
)
result = motion_gen.plan_single(start_state, goal_pose, m_config.clone())
assert result.status != MotionGenStatus.INVALID_PARTIAL_POSE_COST_METRIC
assert torch.count_nonzero(result.success) == 1
reached_state = result.optimized_plan[-1]

96
tests/motion_gen_module_test.py
View File

@@ -18,6 +18,7 @@ from curobo.geom.types import WorldConfig
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState, RobotConfig
from curobo.util.torch_utils import is_cuda_graph_reset_available
from curobo.util.trajectory import InterpolateType
from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
from curobo.wrap.reacher.motion_gen import (
@@ -43,6 +44,21 @@ def motion_gen():
return motion_gen_instance
@pytest.fixture(scope="module")
def motion_gen_cg():
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,
use_cuda_graph=True,
)
motion_gen_instance = MotionGen(motion_gen_config)
return motion_gen_instance
@pytest.fixture(scope="module")
def motion_gen_batch_env():
tensor_args = TensorDeviceType()
@@ -63,13 +79,53 @@ def motion_gen_batch_env():
return motion_gen_instance
@pytest.fixture(scope="module")
def motion_gen_batch_env_cg():
tensor_args = TensorDeviceType()
world_files = ["collision_table.yml", "collision_test.yml"]
world_cfg = [
WorldConfig.from_dict(load_yaml(join_path(get_world_configs_path(), world_file)))
for world_file in world_files
]
robot_file = "franka.yml"
motion_gen_config = MotionGenConfig.load_from_robot_config(
robot_file,
world_cfg,
tensor_args,
use_cuda_graph=True,
)
motion_gen_instance = MotionGen(motion_gen_config)
return motion_gen_instance
@pytest.mark.parametrize(
"motion_gen_str,interpolation",
[
("motion_gen", InterpolateType.LINEAR),
("motion_gen", InterpolateType.CUBIC),
# ("motion_gen", InterpolateType.KUNZ_STILMAN_OPTIMAL),
("motion_gen", InterpolateType.LINEAR_CUDA),
pytest.param(
"motion_gen_cg",
InterpolateType.LINEAR,
marks=pytest.mark.skipif(
not is_cuda_graph_reset_available(), reason="CUDAGraph.reset() not available"
),
),
pytest.param(
"motion_gen_cg",
InterpolateType.CUBIC,
marks=pytest.mark.skipif(
not is_cuda_graph_reset_available(), reason="CUDAGraph.reset() not available"
),
),
pytest.param(
"motion_gen_cg",
InterpolateType.LINEAR_CUDA,
marks=pytest.mark.skipif(
not is_cuda_graph_reset_available(), reason="CUDAGraph.reset() not available"
),
),
],
)
def test_motion_gen_single(motion_gen_str, interpolation, request):
@@ -95,7 +151,21 @@ def test_motion_gen_single(motion_gen_str, interpolation, request):
assert torch.norm(goal_pose.position - reached_state.ee_pos_seq) < 0.005
def test_motion_gen_goalset(motion_gen):
@pytest.mark.parametrize(
"motion_gen_str",
[
("motion_gen"),
pytest.param(
"motion_gen_cg",
marks=pytest.mark.skipif(
not is_cuda_graph_reset_available(), reason="CUDAGraph.reset() not available"
),
),
],
)
def test_motion_gen_goalset(motion_gen_str, request):
motion_gen = request.getfixturevalue(motion_gen_str)
motion_gen.reset()
retract_cfg = motion_gen.get_retract_config()
@@ -203,8 +273,28 @@ def test_motion_gen_batch(motion_gen):
[
("motion_gen", InterpolateType.LINEAR),
("motion_gen", InterpolateType.CUBIC),
# ("motion_gen", InterpolateType.KUNZ_STILMAN_OPTIMAL),
("motion_gen", InterpolateType.LINEAR_CUDA),
pytest.param(
"motion_gen_cg",
InterpolateType.LINEAR,
marks=pytest.mark.skipif(
not is_cuda_graph_reset_available(), reason="CUDAGraph.reset() not available"
),
),
pytest.param(
"motion_gen_cg",
InterpolateType.CUBIC,
marks=pytest.mark.skipif(
not is_cuda_graph_reset_available(), reason="CUDAGraph.reset() not available"
),
),
pytest.param(
"motion_gen_cg",
InterpolateType.LINEAR_CUDA,
marks=pytest.mark.skipif(
not is_cuda_graph_reset_available(), reason="CUDAGraph.reset() not available"
),
),
],
)
def test_motion_gen_batch_graph(motion_gen_str: str, interpolation: InterpolateType, request):

25
tests/motion_gen_speed_test.py
View File

@@ -14,12 +14,9 @@ import pytest
import torch
# CuRobo
from curobo.geom.types import WorldConfig
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState, RobotConfig
from curobo.util.trajectory import InterpolateType
from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
from curobo.types.robot import JointState
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
@@ -58,6 +55,24 @@ def motion_gen_ur5e():
return motion_gen_instance
@pytest.fixture(scope="module")
def motion_gen_ur5e_small_interpolation():
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,
interpolation_steps=10,
interpolation_dt=0.05,
)
motion_gen_instance = MotionGen(motion_gen_config)
motion_gen_instance.warmup(warmup_js_trajopt=False, enable_graph=False)
return motion_gen_instance
@pytest.mark.parametrize(
"motion_gen",
[
@@ -164,6 +179,7 @@ def test_pose_sequence_speed_ur5e_scale(velocity_scale, acceleration_scale):
("motion_gen_ur5e", 0.15),
("motion_gen_ur5e", 0.1),
("motion_gen_ur5e", 0.001),
("motion_gen_ur5e_small_interpolation", 0.01),
],
)
def test_pose_sequence_speed_ur5e_time_dilation(motion_gen_str, time_dilation_factor, request):
@@ -201,6 +217,7 @@ def test_pose_sequence_speed_ur5e_time_dilation(motion_gen_str, time_dilation_fa
)
if result.success.item():
plan = result.get_interpolated_plan()
augmented_js = motion_gen.get_full_js(plan)
trajectory = trajectory.stack(plan.clone())
motion_time += result.motion_time
else:

6
tests/self_collision_test.py
View File

@@ -35,7 +35,7 @@ def test_self_collision_experimental(batch_size, horizon):
tensor_args = TensorDeviceType()
robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
robot_cfg["kinematics"]["debug"] = {"self_collision_experimental": True}
robot_cfg["kinematics"]["debug"] = {"self_collision_experimental": False}
robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
kinematics = CudaRobotModel(robot_cfg.kinematics)
self_collision_data = kinematics.get_self_collision_config()
@@ -44,6 +44,7 @@ def test_self_collision_experimental(batch_size, horizon):
tensor_args=tensor_args
)
cost_fn = SelfCollisionCost(self_collision_config)
cost_fn.self_collision_kin_config.experimental_kernel = True
b = batch_size
h = horizon
@@ -74,6 +75,8 @@ def test_self_collision_franka():
tensor_args = TensorDeviceType()
robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
robot_cfg["kinematics"]["debug"] = {"self_collision_experimental": False}
robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
robot_cfg.kinematics.self_collision_config.experimental_kernel = True
kinematics = CudaRobotModel(robot_cfg.kinematics)
@@ -83,6 +86,7 @@ def test_self_collision_franka():
tensor_args=tensor_args
)
cost_fn = SelfCollisionCost(self_collision_config)
cost_fn.self_collision_kin_config.experimental_kernel = True
b = 10
h = 1

33
tests/xrdf_test.py Normal file
View File

@@ -0,0 +1,33 @@
#
# 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.cuda_robot_model.util import load_robot_yaml
from curobo.types.file_path import ContentPath
def test_xrdf_kinematics():
robot_file = "ur10e.xrdf"
urdf_file = "robot/ur_description/ur10e.urdf"
content_path = ContentPath(robot_xrdf_file=robot_file, robot_urdf_file=urdf_file)
robot_data = load_robot_yaml(content_path)
robot_data["robot_cfg"]["kinematics"]["ee_link"] = "wrist_3_link"
cfg = CudaRobotModelConfig.from_data_dict(robot_data["robot_cfg"]["kinematics"])
robot = CudaRobotModel(cfg)
q_test = robot.tensor_args.to_device([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).view(1, -1)
kin_pose = robot.get_state(q_test)
expected_position = robot.tensor_args.to_device([1.1842, 0.2907, 0.0609]).view(1, -1)
error = kin_pose.ee_pose.position - expected_position
assert error.norm() < 0.01