Fix robot forward kinematics#223
Conversation
There was a problem hiding this comment.
Pull request overview
Fixes forward kinematics failures when qpos is on a different torch device than the Robot/solver, by moving qpos onto self.device before invoking FK.
Changes:
- In
compute_fk, moveqposto the robot device prior to callingsolver.get_fk. - In
compute_batch_fk, moveqposto the robot device prior to reshaping and callingsolver.get_fk.
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| if qpos.device != self.device: | ||
| qpos_ = qpos.to(self.device) | ||
| else: | ||
| qpos_ = qpos |
There was a problem hiding this comment.
compute_fk advertises qpos can be a torch.Tensor | np.ndarray | None, but this code path assumes a Tensor (dim(), unsqueeze(), and now device). Consider normalizing qpos at the start with the existing to_tensor(..., device=self.device) helper (and/or narrowing the type annotation/docstring) so numpy inputs are either supported as documented or rejected explicitly with a clear error.
There was a problem hiding this comment.
We can directly use to (device). This interface will check the device internally.
| @@ -633,8 +638,13 @@ def compute_batch_fk( | |||
| f"Joint positions shape mismatch. Expected {solver.dof} joints, got {qpos.shape[1]}." | |||
There was a problem hiding this comment.
In compute_batch_fk, the shape check uses qpos.shape[2] != solver.dof but the error message reports qpos.shape[1]. This will misreport the number of joints; it should report the last dimension (shape[2]) to match the validation.
| f"Joint positions shape mismatch. Expected {solver.dof} joints, got {qpos.shape[1]}." | |
| f"Joint positions shape mismatch. Expected {solver.dof} joints, got {qpos.shape[2]}." |
| if qpos.device != self.device: | ||
| qpos_ = qpos.to(self.device) | ||
| else: | ||
| qpos_ = qpos |
There was a problem hiding this comment.
We can directly use to (device). This interface will check the device internally.
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 2 out of 2 changed files in this pull request and generated 1 comment.
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| @@ -633,8 +634,9 @@ def compute_batch_fk( | |||
| f"Joint positions shape mismatch. Expected {solver.dof} joints, got {qpos.shape[1]}." | |||
There was a problem hiding this comment.
In compute_batch_fk, the dof check compares qpos.shape[2] to solver.dof, but the error message reports got {qpos.shape[1]} (batch dimension) instead of the joint dimension. This can be misleading when debugging shape issues; the message should report qpos.shape[2] (or the actual joint-axis size being validated).
| f"Joint positions shape mismatch. Expected {solver.dof} joints, got {qpos.shape[1]}." | |
| f"Joint positions shape mismatch. Expected {solver.dof} joints, got {qpos.shape[2]}." |
Co-authored-by: chenjian <chenjian@dexforce.com>
Co-authored-by: chenjian <chenjian@dexforce.com>
Description
Fix robot forward kinematic error when qpos has different device with robot.
Type of change
Checklist
black .command to format the code base.