[fix] fix using bugs of the "get_aligned_pose" and "_prepare_warpping" function

[wuxinxin27]

Description

• fix the get_aligned_pose and get_changed_pose function.
• fix support for non-NumPy affordance_datas in the _prepare_warpping function.

Type of change

• Bug fix (non-breaking change which fixes an issue)

Checklist

• I have run the black . command to format the code base.
• I have made corresponding changes to the documentation
• I have added tests that prove my fix is effective or that my feature works
• Dependencies have been updated, if applicable.

[Copilot]

Pull request overview

This PR fixes pose-alignment logic in get_aligned_pose / get_changed_pose and makes action-bank “warpping” preparation more robust when env.affordance_datas contains non-NumPy values.

Changes:

• Fix get_aligned_pose to respect the provided pose_axis instead of always using the Z axis.
• Fix get_changed_pose(..., change_mode="align") to propagate the returned pose consistently with other change modes.
• Prevent _prepare_warpping from raising when an affordance value does not define .ndim.

Reviewed changes

Copilot reviewed 2 out of 2 changed files in this pull request and generated 1 comment.

File	Description
embodichain/lab/gym/utils/misc.py	Corrects axis selection in pose alignment and updates align-mode pose propagation.
embodichain/lab/gym/envs/action_bank/configurable_action.py	Hardens _prepare_warpping to tolerate affordance values without .ndim.

Comments suppressed due to low confidence (1)

embodichain/lab/gym/envs/action_bank/configurable_action.py:701

• This change is intended to support non-NumPy affordance_datas values (by avoiding .ndim AttributeError), but there’s no regression test covering _prepare_warpping with values that lack ndim (e.g., Python scalars/objects) or with 3D affordance data that should be sliced to the first arena. Adding a unit test in tests/gym/action_bank/test_configurable_action.py that constructs a fake env with affordance_datas containing (1) a scalar and (2) a 3D tensor/ndarray would help prevent future breakage.

    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 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())
                env.affordance_datas[affordance_name] = affordance_value

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

rotation_angle is computed via np.arccos(pose_to_change[:3, pose_axis].dot(align_vector)) without normalizing align_vector (and without clipping the dot into [-1, 1]). If align_vector is not unit-length or there is minor floating-point drift, this can produce NaNs from arccos, breaking the alignment. Consider normalizing align_vector (and handling zero-norm) and clipping the dot product before calling arccos.