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/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: diff --git a/src/curobo/wrap/reacher/evaluator.py b/src/curobo/wrap/reacher/evaluator.py index 863b6a9..c48beb9 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..3e64832 --- /dev/null +++ b/tests/motion_gen_eval_test.py @@ -0,0 +1,72 @@ +# +# 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.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 + + +def run_motion_gen(robot_file, evaluate_interpolated_trajectory, max_acc, max_jerk): + tensor_args = TensorDeviceType() + world_file = "collision_test.yml" + 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_data, + world_file, + tensor_args, + use_cuda_graph=False, + maximum_trajectory_dt=1.5, + evaluate_interpolated_trajectory=evaluate_interpolated_trajectory, + ) + motion_gen = MotionGen(motion_gen_config) + + 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() + 0.2) + + result = motion_gen.plan_single_js( + start_state, goal_state, MotionGenPlanConfig(max_attempts=5, enable_graph_attempt=10) + ) + return result + + +@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