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

improved joint space planning

Browse Source
This commit is contained in:
Balakumar Sundaralingam
2024-05-30 14:42:22 -07:00
parent 3bfed9d773
commit 0c51dd2da8
28 changed files with 1135 additions and 213 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
tests/motion_gen_js_test.py
View File

@@ -11,6 +11,7 @@
# Third Party
import pytest
import torch
# CuRobo
@@ -18,21 +19,20 @@ from curobo.types.robot import JointState
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
def test_motion_gen_plan_js():
@pytest.fixture(scope="module")
def motion_gen():
world_file = "collision_table.yml"
robot_file = "ur5e.yml"
motion_gen_config = MotionGenConfig.load_from_robot_config(
robot_file,
world_file,
trajopt_tsteps=32,
use_cuda_graph=False,
num_trajopt_seeds=4,
fixed_iters_trajopt=True,
evaluate_interpolated_trajectory=True,
)
motion_gen = MotionGen(motion_gen_config)
motion_gen.warmup(warmup_js_trajopt=True)
motion_gen_instance = MotionGen(motion_gen_config)
motion_gen_instance.warmup(warmup_js_trajopt=True)
return motion_gen_instance
def test_motion_gen_plan_js(motion_gen):
retract_cfg = motion_gen.get_retract_config()
start_state = JointState.from_position(retract_cfg.view(1, -1).clone())
@@ -42,3 +42,27 @@ def test_motion_gen_plan_js():
)
result = motion_gen.plan_single_js(start_state, goal_state, MotionGenPlanConfig(max_attempts=1))
assert result.success.item()
@pytest.mark.parametrize(
"motion_gen_str, delta",
[
("motion_gen", 0.1),
("motion_gen", 0.2),
("motion_gen", 0.3),
("motion_gen", 0.4),
("motion_gen", 0.5),
],
)
def test_motion_gen_plan_js_delta(motion_gen_str, delta, request):
motion_gen = request.getfixturevalue(motion_gen_str)
start_state = JointState.from_position(
motion_gen.get_retract_config().view(1, -1).clone(),
joint_names=motion_gen.joint_names,
)
goal_state = JointState.from_position(
motion_gen.get_retract_config().view(1, -1).clone() + delta,
joint_names=motion_gen.joint_names,
)
result = motion_gen.plan_single_js(start_state, goal_state, MotionGenPlanConfig(max_attempts=1))
assert result.success.item()