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

constrained planning, robot segmentation

Browse Source
This commit is contained in:
Balakumar Sundaralingam
2024-02-22 21:45:47 -08:00
parent 88eac64edc
commit bafdf80c05
102 changed files with 12440 additions and 8112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
tests/goal_test.py
View File

@@ -47,6 +47,9 @@ def test_repeat_seeds():
out_r_v = torch.zeros((b, 4), device=tensor_args.device, dtype=tensor_args.dtype)
out_idx = torch.zeros((b, 1), device=tensor_args.device, dtype=torch.int32)
vec_weight = torch.ones((6), device=tensor_args.device, dtype=tensor_args.dtype)
offset_waypoint = torch.zeros((6), device=tensor_args.device, dtype=tensor_args.dtype)
offset_tstep_fraction = torch.ones((1), device=tensor_args.device, dtype=tensor_args.dtype)
weight = tensor_args.to_device([1, 1, 1, 1])
vec_convergence = tensor_args.to_device([0, 0])
run_weight = tensor_args.to_device([1])
@@ -66,6 +69,8 @@ def test_repeat_seeds():
vec_convergence,
run_weight,
vec_weight.clone() * 0.0,
offset_waypoint,
offset_tstep_fraction,
g.batch_pose_idx,
start_pose.position.shape[0],
1,
@@ -74,6 +79,7 @@ def test_repeat_seeds():
False,
False,
True,
True,
)
assert torch.sum(r[0]).item() == 0.0
@@ -112,6 +118,9 @@ def test_horizon_repeat_seeds():
out_r_v = torch.zeros((b, h, 4), device=tensor_args.device, dtype=tensor_args.dtype)
out_idx = torch.zeros((b, h, 1), device=tensor_args.device, dtype=torch.int32)
vec_weight = torch.ones((6), device=tensor_args.device, dtype=tensor_args.dtype)
offset_waypoint = torch.zeros((6), device=tensor_args.device, dtype=tensor_args.dtype)
offset_tstep_fraction = torch.ones((1), device=tensor_args.device, dtype=tensor_args.dtype)
weight = tensor_args.to_device([1, 1, 1, 1])
vec_convergence = tensor_args.to_device([0, 0])
run_weight = torch.zeros((1, h), device=tensor_args.device)
@@ -132,6 +141,8 @@ def test_horizon_repeat_seeds():
vec_convergence,
run_weight,
vec_weight.clone() * 0.0,
offset_waypoint,
offset_tstep_fraction,
g.batch_pose_idx,
start_pose.position.shape[0],
h,
@@ -140,5 +151,6 @@ def test_horizon_repeat_seeds():
True,
False,
False,
True,
)
assert torch.sum(r[0]).item() == 0.0
assert torch.sum(r[0]).item() < 1e-6

2
tests/ik_config_test.py
View File

@@ -146,5 +146,5 @@ def test_eval(config, expected):
result = ik_solver.solve_single(goal)
success = result.success
if expected is not -100:
if expected != -100:
assert success.item() == expected

32
tests/kinematics_test.py
View File

@@ -169,3 +169,35 @@ def test_locked_joints_kinematics():
assert torch.linalg.norm(out.ee_position - out_locked.ee_position) < 1e-5
assert torch.linalg.norm(out.ee_quaternion - out_locked.ee_quaternion) < 1e-5
assert torch.linalg.norm(out.link_spheres_tensor - out_locked.link_spheres_tensor) < 1e-5
def test_franka_toggle_link_collision(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], **vars(tensor_args)).view(1, -1)
sph_idx = robot_model.kinematics_config.get_sphere_index_from_link_name("panda_link5")
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
link_radius = attached_spheres[..., 3].clone()
assert torch.count_nonzero(link_radius <= 0.0) == 0
robot_model.kinematics_config.disable_link_spheres("panda_link5")
state = robot_model.get_state(q_test.clone())
attached_spheres = state.link_spheres_tensor[:, sph_idx, :]
sph_radius = attached_spheres[..., 3].clone()
assert torch.count_nonzero(sph_radius < 0.0)
robot_model.kinematics_config.enable_link_spheres("panda_link5")
state = robot_model.get_state(q_test.clone())
radius = link_radius - state.link_spheres_tensor[:, sph_idx, 3]
assert torch.count_nonzero(radius == 0.0)

51
tests/motion_gen_api_test.py
View File

@@ -14,11 +14,10 @@ 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_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
from curobo.types.robot import JointState
from curobo.util_file import get_robot_configs_path, join_path, load_yaml
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
@@ -60,3 +59,49 @@ def test_motion_gen_attach_obstacle_offset(motion_gen):
start_state, [obstacle], world_objects_pose_offset=offset_pose
)
assert True
def test_motion_gen_lock_js_update():
tensor_args = TensorDeviceType()
world_file = "collision_table.yml"
robot_file = "franka_mobile.yml"
robot_config = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
robot_config["kinematics"]["lock_joints"] = {"base_x": 0.0, "base_y": 0.0, "base_z": 0.0}
motion_gen_config = MotionGenConfig.load_from_robot_config(
robot_config,
world_file,
tensor_args,
use_cuda_graph=True,
)
motion_gen_instance = MotionGen(motion_gen_config)
motion_gen_instance.warmup()
retract_cfg = motion_gen_instance.get_retract_config()
start_state = JointState.from_position(retract_cfg.view(1, -1))
kin_state = motion_gen_instance.compute_kinematics(start_state)
ee_pose = kin_state.ee_pose.clone()
# test motion gen:
plan_start = start_state.clone()
plan_start.position[..., :-2] += 0.1
result = motion_gen_instance.plan_single(plan_start, ee_pose)
assert result.success.item()
lock_js = {"base_x": 1.0, "base_y": 0.0, "base_z": 0.0}
motion_gen_instance.update_locked_joints(lock_js, robot_config)
kin_state_new = motion_gen_instance.compute_kinematics(start_state)
ee_pose_shift = kin_state_new.ee_pose.clone()
assert torch.norm(ee_pose.position[..., 0] - ee_pose_shift.position[..., 0]).item() == 1.0
assert torch.norm(ee_pose.position[..., 1:] - ee_pose_shift.position[..., 1:]).item() == 0.0
# test motion gen with new lock state:
result = motion_gen_instance.plan_single(plan_start, ee_pose_shift)
assert result.success.item()
result = motion_gen_instance.plan_single(
plan_start, ee_pose, MotionGenPlanConfig(max_attempts=3)
)
assert result.success.item() == False

314
tests/motion_gen_constrained_test.py Normal file
View File

@@ -0,0 +1,314 @@
#
# 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.
#
# Third Party
import pytest
import torch
# CuRobo
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState
from curobo.wrap.reacher.motion_gen import (
MotionGen,
MotionGenConfig,
MotionGenPlanConfig,
PoseCostMetric,
)
@pytest.fixture(scope="module")
def motion_gen(request):
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,
project_pose_to_goal_frame=request.param,
)
motion_gen_instance = MotionGen(motion_gen_config)
motion_gen_instance.warmup(enable_graph=False, warmup_js_trajopt=False)
return motion_gen_instance
@pytest.mark.parametrize(
"motion_gen",
[
(True),
(False),
],
indirect=True,
)
def test_approach_grasp_pose(motion_gen):
# run full pose planning
retract_cfg = motion_gen.get_retract_config()
state = motion_gen.compute_kinematics(JointState.from_position(retract_cfg.view(1, -1)))
goal_pose = state.ee_pose.clone()
goal_pose.position[0, 0] -= 0.1
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
m_config = MotionGenPlanConfig(max_attempts=1)
result = motion_gen.plan_single(start_state, goal_pose, m_config.clone())
assert torch.count_nonzero(result.success) == 1
# run grasp pose planning:
m_config.pose_cost_metric = PoseCostMetric.create_grasp_approach_metric()
result = motion_gen.plan_single(start_state, goal_pose, m_config.clone())
assert torch.count_nonzero(result.success) == 1
@pytest.mark.parametrize(
"motion_gen",
[
(True),
(False),
],
indirect=True,
)
def test_reach_only_position(motion_gen):
retract_cfg = motion_gen.get_retract_config()
state = motion_gen.compute_kinematics(JointState.from_position(retract_cfg.view(1, -1)))
goal_pose = state.ee_pose.clone()
goal_pose.position[0, 0] -= 0.1
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
m_config = MotionGenPlanConfig(
max_attempts=1,
pose_cost_metric=PoseCostMetric(
reach_partial_pose=True,
reach_vec_weight=motion_gen.tensor_args.to_device([0, 0, 0, 1, 1, 1]),
),
)
result = motion_gen.plan_single(start_state, goal_pose, m_config.clone())
assert torch.count_nonzero(result.success) == 1
reached_state = result.optimized_plan[-1]
reached_pose = motion_gen.compute_kinematics(reached_state).ee_pose.clone()
assert goal_pose.angular_distance(reached_pose) > 0.0
assert goal_pose.linear_distance(reached_pose) <= 0.005
@pytest.mark.parametrize(
"motion_gen",
[
(True),
(False),
],
indirect=True,
)
def test_reach_only_orientation(motion_gen):
retract_cfg = motion_gen.get_retract_config()
state = motion_gen.compute_kinematics(JointState.from_position(retract_cfg.view(1, -1)))
goal_pose = state.ee_pose.clone()
goal_pose.position[0, 0] -= 0.1
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
m_config = MotionGenPlanConfig(
max_attempts=1,
pose_cost_metric=PoseCostMetric(
reach_partial_pose=True,
reach_vec_weight=motion_gen.tensor_args.to_device([1, 1, 1, 0, 0, 0]),
),
)
result = motion_gen.plan_single(start_state, goal_pose, m_config.clone())
assert torch.count_nonzero(result.success) == 1
reached_state = result.optimized_plan[-1]
reached_pose = motion_gen.compute_kinematics(reached_state).ee_pose.clone()
assert goal_pose.linear_distance(reached_pose) > 0.0
assert goal_pose.angular_distance(reached_pose) < 0.05
@pytest.mark.parametrize(
"motion_gen",
[
(True),
(False),
],
indirect=True,
)
def test_hold_orientation(motion_gen):
retract_cfg = motion_gen.get_retract_config()
state = motion_gen.compute_kinematics(JointState.from_position(retract_cfg.view(1, -1)))
goal_pose = state.ee_pose.clone()
goal_pose.position[0, 0] -= 0.1
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
start_pose = motion_gen.compute_kinematics(start_state).ee_pose.clone()
goal_pose.quaternion = start_pose.quaternion.clone()
m_config = MotionGenPlanConfig(
max_attempts=1,
pose_cost_metric=PoseCostMetric(
hold_partial_pose=True,
hold_vec_weight=motion_gen.tensor_args.to_device([1, 1, 1, 0, 0, 0]),
),
)
result = motion_gen.plan_single(start_state, goal_pose, m_config.clone())
assert torch.count_nonzero(result.success) == 1
traj_pose = motion_gen.compute_kinematics(result.optimized_plan).ee_pose.clone()
# assert goal_pose.linear_distance(traj_pose) > 0.0
goal_pose = goal_pose.repeat(traj_pose.shape[0])
assert torch.max(goal_pose.angular_distance(traj_pose)) < 0.05
@pytest.mark.parametrize(
"motion_gen",
[
(True),
(False),
],
indirect=True,
)
def test_hold_position(motion_gen):
retract_cfg = motion_gen.get_retract_config()
state = motion_gen.compute_kinematics(JointState.from_position(retract_cfg.view(1, -1)))
goal_pose = state.ee_pose.clone()
goal_pose.position[0, 0] -= 0.1
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
start_pose = motion_gen.compute_kinematics(start_state).ee_pose.clone()
goal_pose.position = start_pose.position.clone()
m_config = MotionGenPlanConfig(
max_attempts=1,
pose_cost_metric=PoseCostMetric(
hold_partial_pose=True,
hold_vec_weight=motion_gen.tensor_args.to_device([0, 0, 0, 1, 1, 1]),
),
)
result = motion_gen.plan_single(start_state, goal_pose, m_config.clone())
assert torch.count_nonzero(result.success) == 1
traj_pose = motion_gen.compute_kinematics(result.optimized_plan).ee_pose.clone()
goal_pose = goal_pose.repeat(traj_pose.shape[0])
assert torch.max(goal_pose.linear_distance(traj_pose)) < 0.005
@pytest.mark.parametrize(
"motion_gen",
[
(False),
(True),
],
indirect=True,
)
def test_hold_partial_pose(motion_gen):
retract_cfg = motion_gen.get_retract_config()
state = motion_gen.compute_kinematics(JointState.from_position(retract_cfg.view(1, -1)))
goal_pose = state.ee_pose.clone()
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
start_pose = motion_gen.compute_kinematics(start_state).ee_pose.clone()
goal_pose.position = start_pose.position.clone()
goal_pose.quaternion = start_pose.quaternion.clone()
if motion_gen.project_pose_to_goal_frame:
offset_pose = Pose.from_list([0, 0.1, 0, 1, 0, 0, 0])
goal_pose = goal_pose.multiply(offset_pose)
else:
goal_pose.position[0, 1] += 0.2
m_config = MotionGenPlanConfig(
max_attempts=1,
pose_cost_metric=PoseCostMetric(
hold_partial_pose=True,
hold_vec_weight=motion_gen.tensor_args.to_device([1, 1, 1, 1, 0, 1]),
),
)
result = motion_gen.plan_single(start_state, goal_pose, m_config.clone())
assert torch.count_nonzero(result.success) == 1
traj_pose = motion_gen.compute_kinematics(result.optimized_plan).ee_pose.clone()
goal_pose = goal_pose.repeat(traj_pose.shape[0])
if motion_gen.project_pose_to_goal_frame:
traj_pose = goal_pose.compute_local_pose(traj_pose)
traj_pose.position[:, 1] = 0.0
assert torch.max(traj_pose.position) < 0.005
else:
goal_pose.position[:, 1] = 0.0
traj_pose.position[:, 1] = 0.0
assert torch.max(goal_pose.linear_distance(traj_pose)) < 0.005
@pytest.mark.parametrize(
"motion_gen",
[
(False),
(True),
],
indirect=True,
)
def test_hold_partial_pose_fail(motion_gen):
retract_cfg = motion_gen.get_retract_config()
state = motion_gen.compute_kinematics(JointState.from_position(retract_cfg.view(1, -1)))
goal_pose = state.ee_pose.clone()
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
start_pose = motion_gen.compute_kinematics(start_state).ee_pose.clone()
goal_pose.position = start_pose.position.clone()
goal_pose.quaternion = start_pose.quaternion.clone()
if motion_gen.project_pose_to_goal_frame:
offset_pose = Pose.from_list([0, 0.1, 0.1, 1, 0, 0, 0])
goal_pose = goal_pose.multiply(offset_pose)
else:
goal_pose.position[0, 1] += 0.2
goal_pose.position[0, 0] += 0.2
m_config = MotionGenPlanConfig(
max_attempts=1,
pose_cost_metric=PoseCostMetric(
hold_partial_pose=True,
hold_vec_weight=motion_gen.tensor_args.to_device([1, 1, 1, 1, 0, 1]),
),
)
result = motion_gen.plan_single(start_state, goal_pose, m_config.clone())
assert torch.count_nonzero(result.success) == 0
assert result.valid_query == False

201
tests/motion_gen_goalset_test.py Normal file
View File

@@ -0,0 +1,201 @@
#
# 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.
#
# Third Party
import pytest
import torch
# CuRobo
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
@pytest.fixture(scope="function")
def motion_gen():
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="function")
def motion_gen_batch():
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
def test_goalset_padded(motion_gen):
# run goalset planning
motion_gen.warmup(n_goalset=10)
motion_gen.reset()
m_config = MotionGenPlanConfig(False, True)
retract_cfg = motion_gen.get_retract_config()
state = motion_gen.compute_kinematics(JointState.from_position(retract_cfg.view(1, -1)))
goal_pose = Pose(
state.ee_pos_seq.repeat(10, 1).view(1, -1, 3),
quaternion=state.ee_quat_seq.repeat(10, 1).view(1, -1, 4),
)
goal_pose.position[0, 0, 0] -= 0.1
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
result = motion_gen.plan_goalset(start_state, goal_pose, m_config.clone())
assert torch.count_nonzero(result.success) == 1
retract_cfg = motion_gen.get_retract_config()
state = motion_gen.compute_kinematics(JointState.from_position(retract_cfg.view(1, -1)))
goal_pose = Pose(
state.ee_pos_seq.repeat(2, 1).view(1, -1, 3),
quaternion=state.ee_quat_seq.repeat(2, 1).view(1, -1, 4),
)
goal_pose.position[0, 0, 0] -= 0.1
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
result = motion_gen.plan_goalset(start_state, goal_pose, m_config.clone())
# run goalset with less goals:
# run single planning
retract_cfg = motion_gen.get_retract_config()
state = motion_gen.compute_kinematics(JointState.from_position(retract_cfg.view(1, -1)))
goal_pose = Pose(state.ee_pos_seq, quaternion=state.ee_quat_seq)
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
result = motion_gen.plan_single(start_state, goal_pose, m_config.clone())
assert torch.count_nonzero(result.success) == 1
def test_batch_goalset_padded(motion_gen_batch):
motion_gen = motion_gen_batch
motion_gen.warmup(n_goalset=10, batch=3, enable_graph=False)
# run goalset planning
motion_gen.reset()
retract_cfg = motion_gen.get_retract_config()
state = motion_gen.compute_kinematics(JointState.from_position(retract_cfg.view(1, -1)))
goal_pose = Pose(
state.ee_pos_seq.repeat(3 * 3, 1).view(3, -1, 3).contiguous(),
quaternion=state.ee_quat_seq.repeat(3 * 3, 1).view(3, -1, 4).contiguous(),
)
goal_pose.position[0, 1, 1] = 0.2
goal_pose.position[1, 0, 1] = 0.2
goal_pose.position[2, 1, 1] = 0.2
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.2).repeat_seeds(3)
m_config = MotionGenPlanConfig(False, True, max_attempts=10, enable_graph_attempt=20)
result = motion_gen.plan_batch_goalset(start_state, goal_pose, m_config.clone())
# get final solutions:
assert torch.count_nonzero(result.success) == result.success.shape[0]
reached_state = motion_gen.compute_kinematics(result.optimized_plan.trim_trajectory(-1))
#
goal_position = torch.cat(
[
goal_pose.position[x, result.goalset_index[x], :].unsqueeze(0)
for x in range(len(result.goalset_index))
]
)
assert result.goalset_index is not None
assert torch.max(torch.norm(goal_position - reached_state.ee_pos_seq, dim=-1)) < 0.005
# run goalset with less goals:
motion_gen.reset()
retract_cfg = motion_gen.get_retract_config()
state = motion_gen.compute_kinematics(JointState.from_position(retract_cfg.view(1, -1)))
goal_pose = Pose(
state.ee_pos_seq.repeat(6, 1).view(3, -1, 3).contiguous(),
quaternion=state.ee_quat_seq.repeat(6, 1).view(3, -1, 4).contiguous(),
)
goal_pose.position[0, 1, 1] = 0.2
goal_pose.position[1, 0, 1] = 0.2
goal_pose.position[2, 1, 1] = 0.2
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.2).repeat_seeds(3)
result = motion_gen.plan_batch_goalset(start_state, goal_pose, m_config)
# get final solutions:
assert torch.count_nonzero(result.success) == result.success.shape[0]
reached_state = motion_gen.compute_kinematics(result.optimized_plan.trim_trajectory(-1))
#
goal_position = torch.cat(
[
goal_pose.position[x, result.goalset_index[x], :].unsqueeze(0)
for x in range(len(result.goalset_index))
]
)
assert result.goalset_index is not None
assert torch.max(torch.norm(goal_position - reached_state.ee_pos_seq, dim=-1)) < 0.005
# run single planning
retract_cfg = motion_gen.get_retract_config()
state = motion_gen.compute_kinematics(JointState.from_position(retract_cfg.view(1, -1)))
goal_pose = Pose(
state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze()
).repeat_seeds(3)
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3).repeat_seeds(3)
goal_pose.position[1, 0] -= 0.1
result = motion_gen.plan_batch(start_state, goal_pose.clone(), m_config)
assert torch.count_nonzero(result.success) == 3
# get final solutions:
q = result.optimized_plan.trim_trajectory(-1).squeeze(1)
reached_state = motion_gen.compute_kinematics(q)
assert torch.norm(goal_pose.position - reached_state.ee_pos_seq) < 0.005

53
tests/motion_gen_module_test.py
View File

@@ -23,7 +23,7 @@ from curobo.util_file import get_robot_configs_path, get_world_configs_path, joi
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
@pytest.fixture(scope="function")
@pytest.fixture(scope="module")
def motion_gen():
tensor_args = TensorDeviceType()
world_file = "collision_table.yml"
@@ -38,7 +38,7 @@ def motion_gen():
return motion_gen_instance
@pytest.fixture(scope="function")
@pytest.fixture(scope="module")
def motion_gen_batch_env():
tensor_args = TensorDeviceType()
world_files = ["collision_table.yml", "collision_test.yml"]
@@ -101,6 +101,7 @@ def test_motion_gen_goalset(motion_gen):
state.ee_pos_seq.repeat(2, 1).view(1, -1, 3),
quaternion=state.ee_quat_seq.repeat(2, 1).view(1, -1, 4),
)
goal_pose.position[0, 0, 0] -= 0.1
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
@@ -121,6 +122,13 @@ def test_motion_gen_goalset(motion_gen):
< 0.005
)
assert result.goalset_index is not None
assert (
torch.norm(goal_pose.position[:, result.goalset_index, :] - reached_state.ee_pos_seq)
< 0.005
)
def test_motion_gen_batch_goalset(motion_gen):
motion_gen.reset()
@@ -130,29 +138,36 @@ def test_motion_gen_batch_goalset(motion_gen):
state = motion_gen.compute_kinematics(JointState.from_position(retract_cfg.view(1, -1)))
goal_pose = Pose(
state.ee_pos_seq.repeat(4, 1).view(2, -1, 3),
quaternion=state.ee_quat_seq.repeat(4, 1).view(2, -1, 4),
state.ee_pos_seq.repeat(6, 1).view(3, -1, 3).clone(),
quaternion=state.ee_quat_seq.repeat(6, 1).view(3, -1, 4).clone(),
)
goal_pose.position[0, 1, 1] = 0.2
goal_pose.position[1, 0, 1] = 0.2
goal_pose.position[2, 1, 1] = 0.2
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3).repeat_seeds(2)
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.2).repeat_seeds(3)
m_config = MotionGenPlanConfig(False, True, num_trajopt_seeds=10)
m_config = MotionGenPlanConfig(False, True, num_trajopt_seeds=10, max_attempts=1)
result = motion_gen.plan_batch_goalset(start_state, goal_pose, m_config)
# get final solutions:
assert torch.count_nonzero(result.success) > 0
assert torch.count_nonzero(result.success) == result.success.shape[0]
reached_state = motion_gen.compute_kinematics(result.optimized_plan.trim_trajectory(-1))
assert (
torch.min(
torch.norm(goal_pose.position[:, 0, :] - reached_state.ee_pos_seq),
torch.norm(goal_pose.position[:, 1, :] - reached_state.ee_pos_seq),
)
< 0.005
#
goal_position = torch.cat(
[
goal_pose.position[x, result.goalset_index[x], :].unsqueeze(0)
for x in range(len(result.goalset_index))
]
)
assert result.goalset_index is not None
assert torch.max(torch.norm(goal_position - reached_state.ee_pos_seq, dim=-1)) < 0.005
def test_motion_gen_batch(motion_gen):
motion_gen.reset()
@@ -188,7 +203,6 @@ def test_motion_gen_batch(motion_gen):
],
)
def test_motion_gen_batch_graph(motion_gen_str: str, interpolation: InterpolateType, request):
# return
motion_gen = request.getfixturevalue(motion_gen_str)
motion_gen.graph_planner.interpolation_type = interpolation
@@ -275,6 +289,17 @@ def test_motion_gen_batch_env_goalset(motion_gen_batch_env):
< 0.005
)
goal_position = torch.cat(
[
goal_pose.position[x, result.goalset_index[x], :].unsqueeze(0)
for x in range(len(result.goalset_index))
]
)
assert result.goalset_index is not None
assert torch.max(torch.norm(goal_position - reached_state.ee_pos_seq, dim=-1)) < 0.005
@pytest.mark.parametrize(
"motion_gen_str,enable_graph",

68
tests/motion_gen_speed_test.py Normal file
View File

@@ -0,0 +1,68 @@
#
# 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.
#
# Third Party
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.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
@pytest.fixture(scope="function")
def motion_gen(request):
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,
velocity_scale=request.param,
interpolation_steps=10000,
interpolation_dt=0.02,
)
motion_gen_instance = MotionGen(motion_gen_config)
return motion_gen_instance
@pytest.mark.parametrize(
"motion_gen",
[
(1.0),
(0.75),
(0.5),
(0.25),
(0.15),
(0.1),
],
indirect=True,
)
def test_motion_gen_velocity_scale(motion_gen):
retract_cfg = motion_gen.get_retract_config()
state = motion_gen.compute_kinematics(JointState.from_position(retract_cfg.view(1, -1)))
goal_pose = Pose(state.ee_pos_seq, quaternion=state.ee_quat_seq)
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
m_config = MotionGenPlanConfig(False, True, max_attempts=10)
result = motion_gen.plan_single(start_state, goal_pose, m_config)
assert torch.count_nonzero(result.success) == 1

2
tests/mpc_test.py
View File

@@ -130,7 +130,7 @@ def test_mpc_single(mpc_str, expected, request):
if result.metrics.pose_error.item() < 0.05:
converged = True
break
if tstep > 100:
if tstep > 200:
break
assert converged == expected

2
tests/robot_world_model_test.py
View File

@@ -85,7 +85,7 @@ def test_robot_world_collision_vector():
d_world, d_vec = model.get_collision_vector(state.link_spheres_tensor.view(n, 1, -1, 4))
assert d_world.shape[0] == n
assert torch.norm(d_world[0] - 0.1385) < 0.002
assert torch.norm(d_world[0] - 0.1385) < 0.005
assert torch.abs(d_vec[0, 0, 0, 2] + 0.7350) > 0.002