From 580cdd39c8dca44d016c4d717f79b7245d9c4377 Mon Sep 17 00:00:00 2001 From: chenjian Date: Wed, 6 May 2026 15:05:13 +0800 Subject: [PATCH] update --- embodichain/lab/sim/solvers/pytorch_solver.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/embodichain/lab/sim/solvers/pytorch_solver.py b/embodichain/lab/sim/solvers/pytorch_solver.py index 2e98faf5..c0fcf465 100644 --- a/embodichain/lab/sim/solvers/pytorch_solver.py +++ b/embodichain/lab/sim/solvers/pytorch_solver.py @@ -303,6 +303,19 @@ def _qpos_map_to_limits( is_within_limits = (qpos_mapped >= self.lower_qpos_limits) & ( qpos_mapped <= self.upper_qpos_limits ) + + # if qpos_mapped is valid near zero, use it + k_zero = torch.ceil( + (-torch.pi - qpos) / two_pi + ) # [-pi, pi] is the valid range near zero + qpos_mapped_near_zero = qpos + k_zero * two_pi + is_within_limits_near_zero = ( + qpos_mapped_near_zero >= self.lower_qpos_limits + ) & (qpos_mapped_near_zero <= self.upper_qpos_limits) + qpos_mapped[is_within_limits_near_zero] = qpos_mapped_near_zero[ + is_within_limits_near_zero + ] + return is_within_limits.all(dim=1), qpos_mapped @ensure_pose_shape