From 4997fe7016248a361497cc53fa41beaea1ae9790 Mon Sep 17 00:00:00 2001 From: Ruixin Wu <781785786@qq.com> Date: Sun, 5 Apr 2026 17:20:10 +0800 Subject: [PATCH] [fix] fix using bug of the get_aligned_pose function (1) fix the get_aligned_pose and get_changed_pose function (2) fix support for non-NumPy affordance_datas in the _prepare_warpping function. --- embodichain/lab/gym/envs/action_bank/configurable_action.py | 2 +- embodichain/lab/gym/utils/misc.py | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/embodichain/lab/gym/envs/action_bank/configurable_action.py b/embodichain/lab/gym/envs/action_bank/configurable_action.py index 556d6edf..c0e7130d 100644 --- a/embodichain/lab/gym/envs/action_bank/configurable_action.py +++ b/embodichain/lab/gym/envs/action_bank/configurable_action.py @@ -694,7 +694,7 @@ def _prepare_warpping(self, env): if hasattr(env, "affordance_datas"): for affordance_name, affordance_value in env.affordance_datas.items(): # NOTE: take only first arena's affordance data - if affordance_value.ndim == 3: + if hasattr(affordance_value, "ndim") and affordance_value.ndim == 3: affordance_value = affordance_value[0] if isinstance(affordance_value, torch.Tensor): affordance_value = np.asarray(affordance_value.cpu()) diff --git a/embodichain/lab/gym/utils/misc.py b/embodichain/lab/gym/utils/misc.py index 7472d52c..de02b392 100644 --- a/embodichain/lab/gym/utils/misc.py +++ b/embodichain/lab/gym/utils/misc.py @@ -488,7 +488,7 @@ def get_aligned_pose( rotation_axis_norm = np.linalg.norm(rotation_axis) if rotation_axis_norm >= 1e-5: rotation_axis = rotation_axis / rotation_axis_norm - rotation_angle = np.arccos(pose_to_change[:3, 2].dot(align_vector)) + rotation_angle = np.arccos(pose_to_change[:3, pose_axis].dot(align_vector)) pose_to_change[:3, :3] = ( R.from_rotvec(rotation_axis * rotation_angle).as_matrix() @ pose_to_change[:3, :3] @@ -537,7 +537,9 @@ def get_changed_pose( pose_to_change, pose_change_value, *change_partition[1:] ) elif change_mode == "align": - get_aligned_pose(pose_to_change, pose_change_value, change_partition[1]) + pose_to_change = get_aligned_pose( + pose_to_change, pose_change_value, change_partition[1] + ) else: # TODO log_error(f"The {change_mode} change mode haven't realized yet!")