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