From 9c087a0dd29c19c8e0f28200363744e2c4c64dc5 Mon Sep 17 00:00:00 2001 From: scharalambous-sony Date: Mon, 29 Apr 2024 11:13:17 +0200 Subject: [PATCH 1/3] Fix trajectory evaluation Remove unused imports --- src/curobo/wrap/reacher/evaluator.py | 5 ++- tests/motion_gen_eval_test.py | 60 ++++++++++++++++++++++++++++ 2 files changed, 63 insertions(+), 2 deletions(-) create mode 100644 tests/motion_gen_eval_test.py diff --git a/src/curobo/wrap/reacher/evaluator.py b/src/curobo/wrap/reacher/evaluator.py index 863b6a9..cb3aee7 100644 --- a/src/curobo/wrap/reacher/evaluator.py +++ b/src/curobo/wrap/reacher/evaluator.py @@ -218,12 +218,13 @@ def compute_smoothness( scale_dt = (1 / dt_score).view(-1, 1, 1) abs_acc = torch.abs(acc) * (scale_dt**2) # mean_acc_val = torch.max(torch.mean(abs_acc, dim=-1), dim=-1)[0] - max_acc_val = torch.max(torch.max(abs_acc, dim=-1)[0], dim=-1)[0] + max_acc_val = torch.max(abs_acc, dim=-2)[0] # batch x dof abs_jerk = torch.abs(jerk) * scale_dt**3 # calculate max mean jerk: # mean_jerk_val = torch.max(torch.mean(abs_jerk, dim=-1), dim=-1)[0] - max_jerk_val = torch.max(torch.max(abs_jerk, dim=-1)[0], dim=-1)[0] + max_jerk_val = torch.max(abs_jerk, dim=-2)[0] # batch x dof acc_label = torch.logical_and(max_acc_val <= max_acc, max_jerk_val <= max_jerk) + acc_label = torch.all(acc_label, dim=-1) return (acc_label, smooth_cost(abs_acc, abs_jerk, dt_score)) diff --git a/tests/motion_gen_eval_test.py b/tests/motion_gen_eval_test.py new file mode 100644 index 0000000..4debe92 --- /dev/null +++ b/tests/motion_gen_eval_test.py @@ -0,0 +1,60 @@ +# +# 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.robot import JointState +from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig +from curobo.wrap.reacher.evaluator import TrajEvaluatorConfig + +@pytest.fixture(scope="module", params=[True, False]) +def evaluate_interpolated_trajectory(request): + return request.param + +@pytest.fixture(scope="module") +def motion_gen(evaluate_interpolated_trajectory): + tensor_args = TensorDeviceType() + world_file = "collision_test.yml" + robot_file = "franka.yml" + dof = 9 + traj_evaluator_config = TrajEvaluatorConfig( + max_acc=torch.ones((dof), device=tensor_args.device, dtype=tensor_args.dtype), + max_jerk=torch.ones((dof), device=tensor_args.device, dtype=tensor_args.dtype), + min_dt=torch.tensor(0.01, device=tensor_args.device, dtype=tensor_args.dtype), + max_dt=torch.tensor(1.5, device=tensor_args.device, dtype=tensor_args.dtype) + ) + motion_gen_config = MotionGenConfig.load_from_robot_config( + robot_file, + world_file, + tensor_args, + trajopt_tsteps=26, + use_cuda_graph=False, + num_trajopt_seeds=50, + fixed_iters_trajopt=True, + evaluate_interpolated_trajectory=evaluate_interpolated_trajectory, + traj_evaluator_config=traj_evaluator_config, + ) + motion_gen = MotionGen(motion_gen_config) + motion_gen.warmup(warmup_js_trajopt=True) + + retract_cfg = motion_gen.get_retract_config() + + start_state = JointState.from_position(retract_cfg.view(1, -1).clone()) + goal_state = JointState.from_position(retract_cfg.view(1, -1).clone()) + + result = motion_gen.plan_single_js(start_state, goal_state, MotionGenPlanConfig(max_attempts=1)) + +def test_motion_gen(motion_gen): + return True \ No newline at end of file From e070deab902fc4c9b6226624b60dcd56dfc3120a Mon Sep 17 00:00:00 2001 From: scharalambous-sony Date: Mon, 22 Apr 2024 13:15:51 +0200 Subject: [PATCH 2/3] Minor fixes --- src/curobo/types/state.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/curobo/types/state.py b/src/curobo/types/state.py index 2655e46..4be430d 100644 --- a/src/curobo/types/state.py +++ b/src/curobo/types/state.py @@ -81,7 +81,7 @@ class JointState(State): @staticmethod def from_numpy( joint_names: List[str], - position: np.ndarry, + position: np.ndarray, velocity: Optional[np.ndarray] = None, acceleration: Optional[np.ndarray] = None, jerk: Optional[np.ndarray] = None, @@ -91,6 +91,8 @@ class JointState(State): vel = acc = je = None if velocity is not None: vel = tensor_args.to_device(velocity) + else: + vel = pos * 0.0 if acceleration is not None: acc = tensor_args.to_device(acceleration) else: From e6a5ab2d1875a81cebcff95fb6d0037eba59843b Mon Sep 17 00:00:00 2001 From: Balakumar Sundaralingam Date: Mon, 29 Apr 2024 20:16:49 -0700 Subject: [PATCH 3/3] improve unit test --- CHANGELOG.md | 6 +++ src/curobo/wrap/reacher/evaluator.py | 2 +- tests/motion_gen_eval_test.py | 60 +++++++++++++++++----------- 3 files changed, 43 insertions(+), 25 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index e1350cb..9ac53c7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,12 @@ its affiliates is strictly prohibited. --> # Changelog +## Latest Commit + +### BugFixes & Misc. +- Fix bug in evaluator to account for dof maximum acceleration and jerk. +- Add unit test for different acceleration and jerk limits. + ## Version 0.7.2 ### New Features diff --git a/src/curobo/wrap/reacher/evaluator.py b/src/curobo/wrap/reacher/evaluator.py index cb3aee7..c48beb9 100644 --- a/src/curobo/wrap/reacher/evaluator.py +++ b/src/curobo/wrap/reacher/evaluator.py @@ -218,7 +218,7 @@ def compute_smoothness( scale_dt = (1 / dt_score).view(-1, 1, 1) abs_acc = torch.abs(acc) * (scale_dt**2) # mean_acc_val = torch.max(torch.mean(abs_acc, dim=-1), dim=-1)[0] - max_acc_val = torch.max(abs_acc, dim=-2)[0] # batch x dof + max_acc_val = torch.max(abs_acc, dim=-2)[0] # batch x dof abs_jerk = torch.abs(jerk) * scale_dt**3 # calculate max mean jerk: # mean_jerk_val = torch.max(torch.mean(abs_jerk, dim=-1), dim=-1)[0] diff --git a/tests/motion_gen_eval_test.py b/tests/motion_gen_eval_test.py index 4debe92..3e64832 100644 --- a/tests/motion_gen_eval_test.py +++ b/tests/motion_gen_eval_test.py @@ -16,45 +16,57 @@ import torch # CuRobo from curobo.types.base import TensorDeviceType from curobo.types.robot import JointState -from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig +from curobo.util_file import get_robot_configs_path, join_path, load_yaml from curobo.wrap.reacher.evaluator import TrajEvaluatorConfig +from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig -@pytest.fixture(scope="module", params=[True, False]) -def evaluate_interpolated_trajectory(request): - return request.param -@pytest.fixture(scope="module") -def motion_gen(evaluate_interpolated_trajectory): +def run_motion_gen(robot_file, evaluate_interpolated_trajectory, max_acc, max_jerk): tensor_args = TensorDeviceType() world_file = "collision_test.yml" - robot_file = "franka.yml" - dof = 9 - traj_evaluator_config = TrajEvaluatorConfig( - max_acc=torch.ones((dof), device=tensor_args.device, dtype=tensor_args.dtype), - max_jerk=torch.ones((dof), device=tensor_args.device, dtype=tensor_args.dtype), - min_dt=torch.tensor(0.01, device=tensor_args.device, dtype=tensor_args.dtype), - max_dt=torch.tensor(1.5, device=tensor_args.device, dtype=tensor_args.dtype) - ) + robot_data = load_yaml(join_path(get_robot_configs_path(), robot_file)) + dof = len(robot_data["robot_cfg"]["kinematics"]["cspace"]["joint_names"]) + + robot_data["robot_cfg"]["kinematics"]["cspace"]["max_acceleration"] = [ + max_acc for i in range(9) + ] + robot_data["robot_cfg"]["kinematics"]["cspace"]["max_jerk"] = [max_jerk for i in range(9)] + motion_gen_config = MotionGenConfig.load_from_robot_config( - robot_file, + robot_data, world_file, tensor_args, - trajopt_tsteps=26, use_cuda_graph=False, - num_trajopt_seeds=50, - fixed_iters_trajopt=True, + maximum_trajectory_dt=1.5, evaluate_interpolated_trajectory=evaluate_interpolated_trajectory, - traj_evaluator_config=traj_evaluator_config, ) motion_gen = MotionGen(motion_gen_config) - motion_gen.warmup(warmup_js_trajopt=True) retract_cfg = motion_gen.get_retract_config() start_state = JointState.from_position(retract_cfg.view(1, -1).clone()) - goal_state = JointState.from_position(retract_cfg.view(1, -1).clone()) + goal_state = JointState.from_position(retract_cfg.view(1, -1).clone() + 0.2) - result = motion_gen.plan_single_js(start_state, goal_state, MotionGenPlanConfig(max_attempts=1)) + result = motion_gen.plan_single_js( + start_state, goal_state, MotionGenPlanConfig(max_attempts=5, enable_graph_attempt=10) + ) + return result -def test_motion_gen(motion_gen): - return True \ No newline at end of file + +@pytest.mark.parametrize( + "robot_file, evaluate_interpolated_traj, max_acc, max_jerk", + [ + ("franka.yml", False, 1.0, 500.0), + ("franka.yml", True, 0.1, 500.0), + ("franka.yml", True, 1.0, 500.0), + ("ur5e.yml", False, 1.0, 500.0), + ("ur5e.yml", True, 0.1, 500.0), + ("ur5e.yml", True, 1.0, 500.0), + ], +) +def test_motion_gen_trajectory(robot_file, evaluate_interpolated_traj, max_acc, max_jerk): + result = run_motion_gen(robot_file, evaluate_interpolated_traj, max_acc, max_jerk) + + assert result.success.item() + assert torch.max(torch.abs(result.optimized_plan.acceleration)) <= max_acc + assert torch.max(torch.abs(result.optimized_plan.jerk)) <= max_jerk