add articulation affordance - #509
Open
matafela wants to merge 10 commits into
Open
Conversation
Greptile SummaryThe PR adds articulation-aware affordances and atomic actions for turning knobs, pressing buttons, and pulling or pushing articulated parts, while replacing the legacy press action.
Confidence Score: 3/5The PR does not appear safe to merge until valid vertical turn and press axes can produce trajectory frames without raising. Turn and press frame construction still crosses each transformed action axis with fixed world-up and raises for parallel axes, so both planners can reject valid affordances before producing trajectories. Files Needing Attention: embodichain/lab/sim/atomic_actions/affordance.py, embodichain/lab/sim/atomic_actions/primitives/turn_knob.py, embodichain/lab/sim/atomic_actions/primitives/press_button.py
|
| Filename | Overview |
|---|---|
| embodichain/lab/sim/atomic_actions/affordance.py | Adds turn, press-button, and pull/push affordances; the previously reported vertical-axis frame failures remain. |
| embodichain/lab/sim/atomic_actions/primitives/turn_knob.py | Adds trajectory planning for knob approach, grasp, rotation, release, and retraction, but still relies on the restricted grasp-frame construction. |
| embodichain/lab/sim/atomic_actions/primitives/press_button.py | Adds approach, press, and retract planning, but still relies on the restricted press-frame construction. |
| embodichain/lab/sim/atomic_actions/primitives/pull_push_articulated_part.py | Adds linear pull and push trajectory planning around articulation-backed antipodal grasps. |
| tests/sim/atomic_actions/test_affordance.py | Adds validation and pose-construction coverage for the new affordances. |
Reviews (9): Last reviewed commit: "fix module import" | Re-trigger Greptile
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
| return parser.parse_args() | ||
|
|
||
|
|
||
| def create_microwave(sim) -> Articulation: |
There was a problem hiding this comment.
The new create_microwave helper leaves sim untyped, while MicrowaveOven.__init__ declares data_root as str despite accepting None, giving type checkers and API consumers incomplete or inaccurate signatures.
Context Used: AGENTS.md (source)
Prompt To Fix With AI
This is a comment left during a code review.
Path: scripts/tutorials/atomic_action/turn_knob.py
Line: 78
Comment:
**Public API annotations are incomplete**
The new `create_microwave` helper leaves `sim` untyped, while `MicrowaveOven.__init__` declares `data_root` as `str` despite accepting `None`, giving type checkers and API consumers incomplete or inaccurate signatures.
**Context Used:** AGENTS.md ([source](https://github.com/dexforce/embodichain/blob/main/AGENTS.md))
---
For each issue above, determine whether it is valid and should be fixed. If so, fix it directly.Note: If this suggestion doesn't match your team's coding style, reply to this and let me know. I'll remember it for next time!
Contributor
There was a problem hiding this comment.
Pull request overview
This PR extends the atomic-actions subsystem with articulation-backed affordances and a new TurnKnob primitive, enabling deterministic knob-turn planning from an articulation link’s live pose/geometry and providing an end-to-end tutorial + docs/tests.
Changes:
- Added
TurnAffordance(articulation-link knob semantics) andTurnKnobatomic action (approach → reach → close → turn → open → retract). - Extended
AntipodalAffordanceto optionally resolve mesh/pose from an articulation link. - Added tests, docs, and a new tutorial script demonstrating knob turning on a microwave articulation.
Reviewed changes
Copilot reviewed 12 out of 12 changed files in this pull request and generated 3 comments.
Show a summary per file
| File | Description |
|---|---|
| tests/sim/atomic_actions/test_affordance.py | Adds coverage for articulation-backed antipodal affordances and TurnAffordance grasp-pose behavior. |
| tests/sim/atomic_actions/test_actions.py | Adds coverage for TurnKnob planning/segments and validates affordance-type requirements. |
| scripts/tutorials/atomic_action/tutorial_utils.py | Adds optional init_qpos support for tutorial robot setup. |
| scripts/tutorials/atomic_action/turn_knob.py | New tutorial demonstrating TurnKnob on a microwave articulation. |
| embodichain/lab/sim/atomic_actions/primitives/turn_knob.py | New TurnKnob primitive, goal, and options implementation. |
| embodichain/lab/sim/atomic_actions/primitives/init.py | Registers TurnKnob as a built-in primitive and exports symbols. |
| embodichain/lab/sim/atomic_actions/affordance.py | Adds TurnAffordance and extends AntipodalAffordance to support articulation-link geometry/pose resolution. |
| embodichain/lab/sim/atomic_actions/init.py | Re-exports TurnAffordance and TurnKnob* public API. |
| embodichain/data/assets/obj_assets.py | Adds a MicrowaveOven dataset helper entry for the tutorial asset. |
| docs/source/overview/sim/atomic_actions/index.md | Updates built-in action count reference (now 10). |
| docs/source/overview/sim/atomic_actions/builtin_actions.md | Documents TurnKnob contract and adds it to the built-in actions table. |
| docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.primitives.rst | Adds API reference entries for TurnKnob, TurnKnobGoal, TurnKnobOptions. |
Suppressed comments (2)
embodichain/lab/sim/atomic_actions/primitives/turn_knob.py:206
- TurnKnob allocates the output trajectory with self.n_envs rows, but fills it from context.last_qpos and hand_* tensors built with context.batch_size. This will error if those batch sizes ever diverge. Allocate using context.batch_size (or link_pose.shape[0]) so the tensor shapes are consistent within _plan.
full = torch.empty(
(self.n_envs, sum(lengths), self.robot_dof),
dtype=context.robot.qpos.dtype,
device=self.device,
)
full[:] = context.last_qpos.unsqueeze(1)
scripts/tutorials/atomic_action/tutorial_utils.py:759
- create_ur5_gripper_robot_cfg adds an init_qpos parameter but the docstring Args section doesn't document it, which makes the function contract unclear in the tutorial utilities.
init_qpos: Sequence[float] | None = None,
) -> RobotCfg:
"""Build a UR5 arm + DH_PGI_140_80 gripper robot configuration.
The arm is taken from :class:`~embodichain.lab.sim.robots.ur_robot.URRobotCfg`
💡 Add a code-review agent skill or configure MCP servers for context-aware, tailored reviews. Learn more in the docs.
Comment on lines
+328
to
+336
| y_axis = torch.tensor( | ||
| [0.0, 0.0, 1.0], dtype=torch.float32, device=device | ||
| ).expand_as(z_axis) | ||
| x_axis = torch.linalg.cross(y_axis, z_axis, dim=1) | ||
| if torch.any(torch.linalg.vector_norm(x_axis, dim=1) <= 1.0e-6): | ||
| raise ValueError( | ||
| "TurnAffordance turn axis must not be parallel to world (0, 0, 1)." | ||
| ) | ||
| x_axis = torch.nn.functional.normalize(x_axis, dim=1) |
Comment on lines
+123
to
+130
| link_pose = affordance.get_link_pose().to( | ||
| device=self.device, dtype=torch.float32 | ||
| ) | ||
| if link_pose.shape != (self.n_envs, 4, 4): | ||
| raise ValueError( | ||
| "Articulation link pose must have shape " | ||
| f"({self.n_envs}, 4, 4), got {tuple(link_pose.shape)}." | ||
| ) |
Comment on lines
224
to
228
| def add_ur5_gripper_robot( | ||
| sim: SimulationManager, | ||
| init_pos: Sequence[float] = (0.0, 0.0, 0.0), | ||
| init_qpos: Sequence[float] | None = None, | ||
| ) -> Robot: |
Contributor
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 12 out of 12 changed files in this pull request and generated no new comments.
Suppressed comments (3)
scripts/tutorials/atomic_action/tutorial_utils.py:784
init_qposis accepted as an arbitrary sequence, but there is no validation that it matches the expected number of joints for this tutorial robot (arm + gripper). A length mismatch will fail later during robot initialization/reset with a harder-to-debug error.
qpos = (
[0.0, -1.57, 1.57, -1.57, -1.57, 0.0, 0.0, 0.0]
if init_qpos is None
else list(init_qpos)
)
scripts/tutorials/atomic_action/tutorial_utils.py:230
- The new
init_qposparameter is not mentioned in the function docstring, so it’s easy to miss that callers can override the default tutorial joint pose.
"""Add the standard UR5 plus PGI gripper tutorial robot.
scripts/tutorials/atomic_action/tutorial_utils.py:758
init_qposwas added to the signature, but the docstring description doesn’t mention what it does. Adding a short note here makes the new capability discoverable without scanning the whole function.
This issue also appears on line 780 of the same file.
"""Build a UR5 arm + DH_PGI_140_80 gripper robot configuration.
Contributor
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 11 out of 11 changed files in this pull request and generated no new comments.
Suppressed comments (2)
embodichain/lab/sim/atomic_actions/primitives/turn_knob.py:115
- TurnKnob mixes batch dimensions: hand joint targets are built with context.batch_size, but the rest of the planner (link_pose shape check, trajectory allocation) assumes self.n_envs from the bound robot. If PlanningContext.batch_size differs (env subset planning), this can trigger shape mismatches or incorrect broadcasting.
hand_open_qpos = end_effector.joint_positions(
OPEN_COMMAND,
n_envs=context.batch_size,
device=self.device,
dtype=context.robot.qpos.dtype,
)
embodichain/data/assets/obj_assets.py:257
- The new MicrowaveOven dataset block has formatting that deviates from the surrounding DataDescriptor pattern (closing paren on the same line, extra whitespace) and is likely to fail black/linters. It also removes the blank line separation between dataset classes.
data_descriptor = o3d.data.DataDescriptor(
os.path.join(EMBODICHAIN_DOWNLOAD_PREFIX, obj_assets, "MicrowaveOven.zip"),
"5c90aa6911b445811fc81d704d461057", )
prefix = type(self).__name__
Contributor
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 12 out of 12 changed files in this pull request and generated no new comments.
Suppressed comments (2)
embodichain/lab/sim/atomic_actions/affordance.py:332
- TurnAffordance.get_grasp_pose constructs a rotation matrix with y_axis fixed to world up without re-orthogonalizing it against z_axis. If the provided turn_axis has any world-up component, y_axis will not be perpendicular to z_axis and the resulting pose will not be a valid orthonormal transform (can break downstream FK/IK / relative-rotation math). Project world-up onto the plane orthogonal to z_axis and normalize before computing x_axis.
y_axis = torch.tensor(
[0.0, 0.0, 1.0], dtype=torch.float32, device=device
).expand_as(z_axis)
x_axis = torch.linalg.cross(y_axis, z_axis, dim=1)
if torch.any(torch.linalg.vector_norm(x_axis, dim=1) <= 1.0e-6):
embodichain/data/assets/obj_assets.py:246
- MicrowaveOven dataset docstring points to "MicrowaveOven/microwave_oven.urdf", but this PR’s new TurnKnob tutorial uses "MicrowaveOven/microwave_oven_with_inertials.urdf". This mismatch is confusing for users trying to locate the correct asset path via get_data_path(). Consider updating the docstring to match the tutorial asset (or mention both URDFs if both are shipped).
class MicrowaveOven(EmbodiChainDataset):
"""get_data_path("MicrowaveOven/microwave_oven.urdf")"""
Contributor
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 15 out of 15 changed files in this pull request and generated 2 comments.
Suppressed comments (2)
docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst:75
- The autosummary list was updated for
PressButton*, but it still omits the newTurnKnob*symbols andTurnAffordance, even though they are exported fromembodichain.lab.sim.atomic_actions. This makes the generated API reference incomplete.
PressGoal
PressButtonGoal
PressButtonOptions
PressButtonAffordance
CoordinatedPickGoal
embodichain/data/assets/obj_assets.py:246
- The
MicrowaveOvendataset docstring example path is inconsistent with the new tutorials (which referencemicrowave_oven_with_inertials.urdf). Updating this example avoids confusion about which asset path is intended.
class MicrowaveOven(EmbodiChainDataset):
"""get_data_path("MicrowaveOven/microwave_oven.urdf")"""
| angle_b = get_relative_rotation( | ||
| reference_xpos[:, :3, :3], symmetric_xpos[:, :3, :3] | ||
| ) | ||
| target_xpos = torch.where(angle_a < angle_b, target_xpos, symmetric_xpos) |
| angle_b = get_relative_rotation( | ||
| reference_xpos[:, :3, :3], symmetric_xpos[:, :3, :3] | ||
| ) | ||
| target_xpos = torch.where(angle_a < angle_b, target_xpos, symmetric_xpos) |
Contributor
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 15 out of 15 changed files in this pull request and generated no new comments.
Suppressed comments (4)
embodichain/lab/sim/atomic_actions/affordance.py:336
- TurnAffordance.get_grasp_pose currently sets y_axis to world up and sets z_axis from the turn axis, but it never re-orthogonalizes y_axis against z_axis. Unless z_axis is exactly perpendicular to world up, the resulting rotation is not orthonormal (y·z != 0), which can break downstream IK / motion generation that assumes valid rotation matrices.
Compute y_axis as cross(z_axis, x_axis) after constructing x_axis from a world-up reference vector.
y_axis = torch.tensor(
[0.0, 0.0, 1.0], dtype=torch.float32, device=device
).expand_as(z_axis)
x_axis = torch.linalg.cross(y_axis, z_axis, dim=1)
if torch.any(torch.linalg.vector_norm(x_axis, dim=1) <= 1.0e-6):
raise ValueError(
"TurnAffordance turn axis must not be parallel to world (0, 0, 1)."
)
x_axis = torch.nn.functional.normalize(x_axis, dim=1)
embodichain/lab/sim/atomic_actions/affordance.py:310
- The TurnAffordance.get_grasp_pose docstring states that the pose y-axis is fixed to world (0, 0, 1), but the implementation needs to (and should) orthonormalize the frame for arbitrary turn axes. The docstring should describe that world-up is used as a reference to build an orthonormal basis rather than being kept as an exact axis.
The pose z-axis follows :attr:`turn_axis` transformed into the world
frame, while its y-axis is fixed to world ``(0, 0, 1)``.
docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst:75
- The atomic actions API reference autosummary was updated for PressButton, but it doesn’t list the newly added TurnKnob / TurnAffordance symbols. This makes the public API docs incomplete/inconsistent with the new built-ins exported from embodichain.lab.sim.atomic_actions.
PressGoal
PressButtonGoal
PressButtonOptions
PressButtonAffordance
CoordinatedPickGoal
embodichain/lab/sim/atomic_actions/affordance.py:450
- PressButtonAffordance.get_press_pose sets y_axis to world up but doesn’t enforce orthogonality between y_axis and the computed z_axis. For press axes that aren’t perpendicular to world up, this produces a non-orthonormal rotation matrix (invalid transform).
Derive y_axis from cross(z_axis, x_axis) after constructing x_axis from the world-up reference vector.
z_axis = torch.matmul(link_pose[:, :3, :3], press_axis)
z_axis = torch.nn.functional.normalize(z_axis, dim=1)
y_axis = torch.tensor(
[0.0, 0.0, 1.0], dtype=torch.float32, device=device
).expand_as(z_axis)
x_axis = torch.linalg.cross(y_axis, z_axis, dim=1)
if torch.any(torch.linalg.vector_norm(x_axis, dim=1) <= 1.0e-6):
Contributor
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 18 out of 18 changed files in this pull request and generated 1 comment.
Suppressed comments (7)
embodichain/lab/sim/atomic_actions/affordance.py:356
- TurnAffordance.get_grasp_pose builds a pose with y_axis fixed to world up, but z_axis is generally not orthogonal to that y_axis. This yields a non-orthonormal rotation matrix (not a valid rigid transform), which can break downstream rotation math/IK. Consider computing y_axis as the normalized projection of world_up onto the plane orthogonal to z_axis (and then x_axis = cross(y_axis, z_axis)) so the basis is orthonormal while staying as close as possible to world up.
z_axis = torch.matmul(link_pose[:, :3, :3], turn_axis)
z_axis = torch.nn.functional.normalize(z_axis, dim=1)
y_axis = torch.tensor(
[0.0, 0.0, 1.0], dtype=torch.float32, device=device
).expand_as(z_axis)
embodichain/lab/sim/atomic_actions/affordance.py:501
- PressButtonAffordance.get_press_pose has the same basis construction issue as TurnAffordance: it fixes y_axis to world up but does not enforce/restore orthogonality with z_axis. The resulting transform can have a non-orthonormal rotation submatrix, which is not a valid pose for IK/planning. Consider projecting world_up onto the plane orthogonal to z_axis (or otherwise orthonormalizing) before forming the rotation matrix.
z_axis = torch.matmul(link_pose[:, :3, :3], press_axis)
z_axis = torch.nn.functional.normalize(z_axis, dim=1)
y_axis = torch.tensor(
[0.0, 0.0, 1.0], dtype=torch.float32, device=device
).expand_as(z_axis)
docs/source/overview/sim/atomic_actions/index.md:301
- The docs claim there are "eleven" built-in actions, but BUILTIN_ACTION_TYPES now contains 12 actions (MoveJoints plus the 3 newly added primitives). This count should match the code so users don’t get confused when enumerating engine.actions.
# All eleven built-ins are immediately usable by stable skill ID.
docs/source/overview/sim/atomic_actions/builtin_actions.md:8
- This page says there are eleven built-in actions, but BUILTIN_ACTION_TYPES now contains 12 actions (including MoveJoints and the 3 new primitives). The headline count should be updated to stay consistent with the code.
EmbodiChain ships eleven built-in action implementations with stable skill IDs;
docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst:77
- The autosummary list was updated for PressButton and PullPush, but it does not include the newly exported TurnKnob/TurnAffordance symbols (TurnKnobGoal, TurnKnobOptions, TurnAffordance, TurnKnob). This makes the API reference incomplete relative to embodichain.lab.sim.atomic_actions.all.
PressButtonGoal
PressButtonOptions
PressButtonAffordance
PullPushArticulatedPartGoal
PullPushArticulatedPartOptions
PullPushAffordance
scripts/tutorials/atomic_action/turn_knob.py:57
- draw_axis_marker is imported but never used in this tutorial script. Unused imports are easy to miss in examples and can fail linting if enabled; either use it under the visualize_axes feature or remove it.
from scripts.tutorials.atomic_action.tutorial_utils import (
add_ur5_gripper_robot,
create_toppra_motion_generator,
create_tutorial_argument_parser,
create_tutorial_simulation,
draw_axis_marker,
get_hand_open_close_qpos,
scripts/tutorials/atomic_action/press_button.py:56
- draw_axis_marker is imported but never used in this tutorial script. If the visualize_axes feature is intended here, consider drawing an axis marker; otherwise remove the unused import to avoid lint failures and keep the example minimal.
from scripts.tutorials.atomic_action.tutorial_utils import (
add_ur5_gripper_robot,
create_toppra_motion_generator,
create_tutorial_argument_parser,
create_tutorial_simulation,
draw_axis_marker,
get_hand_open_close_qpos,
Comment on lines
65
to
69
| GRIPPER_URDF_PATH = "DH_PGI_140_80/DH_PGI_140_80.urdf" | ||
| GRIPPER_HAND_JOINT_PATTERN = "gripper_finger1_joint_1" | ||
| GRIPPER_TCP_Z = 0.15 | ||
| GRIPPER_MAX_OPEN_WIDTH = 0.100 | ||
| GRIPPER_MIN_OPEN_WIDTH = 0.003 | ||
| GRIPPER_FINGER_LENGTH = 0.10 |
Contributor
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 26 out of 27 changed files in this pull request and generated no new comments.
Suppressed comments (4)
scripts/tutorials/atomic_action/tutorial_utils.py:67
GRIPPER_TCP_Zwas removed from this module, but it is still listed in__all__later in the file. This can breakfrom scripts.tutorials.atomic_action.tutorial_utils import *(and any code importingGRIPPER_TCP_Z). Either reintroduce the constant (keeping it aligned with the new defaulttcp_z) or remove it from__all__.
GRIPPER_URDF_PATH = "DH_PGI_140_80/DH_PGI_140_80.urdf"
GRIPPER_HAND_JOINT_PATTERN = "gripper_finger1_joint_1"
GRIPPER_MAX_OPEN_WIDTH = 0.100
embodichain/lab/sim/atomic_actions/primitives/turn_knob.py:80
TurnKnobOptions.__post_init__validates thatpre_grasp_distanceis non-negative, but it doesn't reject NaN/inf values. A non-finite distance will propagate NaNs into pose targets and can make planning fail in hard-to-debug ways.
raise ValueError("turn_waypoint_count must be at least 1.")
if self.pre_grasp_distance < 0.0:
raise ValueError("pre_grasp_distance must be non-negative.")
if not math.isfinite(self.turn_angle):
raise ValueError("turn_angle must be finite.")
embodichain/data/assets/obj_assets.py:246
- The
MicrowaveOvendataset docstring advertisesMicrowaveOven/microwave_oven.urdf, but the new tutorials referenceMicrowaveOven/microwave_oven_with_inertials.urdf. This mismatch makes it harder to find the right asset path.
class MicrowaveOven(EmbodiChainDataset):
"""get_data_path("MicrowaveOven/microwave_oven.urdf")"""
docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst:75
- The API reference autosummary lists the new PressButton and PullPush symbols, but it omits the TurnKnob/TurnAffordance symbols that are now exported from
embodichain.lab.sim.atomic_actions. This causes the generated docs to be incomplete.
PressButtonOptions
PressButtonAffordance
PullPushArticulatedPartGoal
PullPushArticulatedPartOptions
PullPushAffordance
Contributor
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 26 out of 27 changed files in this pull request and generated 1 comment.
Suppressed comments (3)
embodichain/lab/sim/atomic_actions/affordance.py:379
- TurnAffordance.get_grasp_pose() builds a rotation matrix with y_axis fixed to world-up and z_axis set from the turn axis. Unless z_axis is exactly perpendicular to world-up, the resulting [x,y,z] basis is not orthonormal (y and z are not perpendicular), which can produce invalid transforms for IK/planning.
z_axis = torch.matmul(link_pose[:, :3, :3], turn_axis)
z_axis = torch.nn.functional.normalize(z_axis, dim=1)
y_axis = torch.tensor(
[0.0, 0.0, 1.0], dtype=torch.float32, device=device
).expand_as(z_axis)
scripts/tutorials/atomic_action/tutorial_utils.py:67
- GRIPPER_TCP_Z was removed from the constants block, but it is still referenced later in this module’s all list (and may be imported externally). As-is, importing this module will raise NameError when evaluating all.
GRIPPER_URDF_PATH = "DH_PGI_140_80/DH_PGI_140_80.urdf"
GRIPPER_HAND_JOINT_PATTERN = "gripper_finger1_joint_1"
GRIPPER_MAX_OPEN_WIDTH = 0.100
docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst:74
- The autosummary list for built-in goals/actions omits the newly added TurnKnob and TurnAffordance symbols, so the API reference doesn’t reflect all built-ins shipped by the package.
PressButtonGoal
PressButtonOptions
PressButtonAffordance
PullPushArticulatedPartGoal
PullPushArticulatedPartOptions
Comment on lines
+563
to
+574
| z_axis = torch.matmul(link_pose[:, :3, :3], press_axis) | ||
| z_axis = torch.nn.functional.normalize(z_axis, dim=1) | ||
| y_axis = torch.tensor( | ||
| [0.0, 0.0, 1.0], dtype=torch.float32, device=device | ||
| ).expand_as(z_axis) | ||
| x_axis = torch.linalg.cross(y_axis, z_axis, dim=1) | ||
| if torch.any(torch.linalg.vector_norm(x_axis, dim=1) <= 1.0e-6): | ||
| raise ValueError( | ||
| "PressButtonAffordance press axis must not be parallel to world " | ||
| "(0, 0, 1)." | ||
| ) | ||
| x_axis = torch.nn.functional.normalize(x_axis, dim=1) |
Contributor
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 35 out of 36 changed files in this pull request and generated 1 comment.
Suppressed comments (3)
embodichain/lab/sim/atomic_actions/affordance.py:392
TurnAffordance.get_grasp_pose()builds axes withx = cross(world_up, z)but then keepsy_axisfixed toworld_upwithout re-orthogonalizing it againstz_axis. Whenz_axishas any non-zero component along world-up, the resulting rotation matrix is not orthonormal (i.e., not a valid rigid transform), which can cause IK/planning instability. Consider constructing an orthonormal frame (e.g.,x = normalize(cross(up, z)),y = normalize(cross(z, x))) similar toembodichain/utils/math.py:1904-1919.
z_axis = torch.matmul(link_pose[:, :3, :3], turn_axis)
z_axis = torch.nn.functional.normalize(z_axis, dim=1)
y_axis = torch.tensor(
[0.0, 0.0, 1.0], dtype=torch.float32, device=device
).expand_as(z_axis)
x_axis = torch.linalg.cross(y_axis, z_axis, dim=1)
if torch.any(torch.linalg.vector_norm(x_axis, dim=1) <= 1.0e-6):
raise ValueError(
"TurnAffordance turn axis must not be parallel to world (0, 0, 1)."
)
x_axis = torch.nn.functional.normalize(x_axis, dim=1)
grasp_pose = torch.eye(4, dtype=torch.float32, device=device).repeat(
link_pose.shape[0], 1, 1
)
grasp_pose[:, :3, 0] = x_axis
grasp_pose[:, :3, 1] = y_axis
grasp_pose[:, :3, 2] = z_axis
embodichain/lab/sim/atomic_actions/affordance.py:582
PressButtonAffordance.get_press_pose()has the same frame-construction issue asTurnAffordance:y_axisis fixed to world-up and never re-orthogonalized after choosingz_axis, so the produced rotation may be non-orthonormal (invalid) whenever the press axis is not exactly perpendicular to world-up. This can propagate invalid transforms into motion planning/IK. Prefer an orthonormal basis build (e.g.,x = normalize(cross(up, z)),y = normalize(cross(z, x))).
z_axis = torch.matmul(link_pose[:, :3, :3], press_axis)
z_axis = torch.nn.functional.normalize(z_axis, dim=1)
y_axis = torch.tensor(
[0.0, 0.0, 1.0], dtype=torch.float32, device=device
).expand_as(z_axis)
x_axis = torch.linalg.cross(y_axis, z_axis, dim=1)
if torch.any(torch.linalg.vector_norm(x_axis, dim=1) <= 1.0e-6):
raise ValueError(
"PressButtonAffordance press axis must not be parallel to world "
"(0, 0, 1)."
)
x_axis = torch.nn.functional.normalize(x_axis, dim=1)
press_pose = torch.eye(4, dtype=torch.float32, device=device).repeat(
link_pose.shape[0], 1, 1
)
press_pose[:, :3, 0] = x_axis
press_pose[:, :3, 1] = y_axis
press_pose[:, :3, 2] = z_axis
press_pose[:, :3, 3] = (
scripts/tutorials/atomic_action/tutorial_utils.py:67
GRIPPER_TCP_Zwas removed, buttutorial_utilsstill exports it via__all__. Afrom ...tutorial_utils import *(or docs tooling that expects the symbol) will now fail withAttributeError/missing symbol. Reintroduce the constant (or alternatively remove it from__all__) to keep the module exports consistent.
Comment on lines
+31
to
+35
| PUBLIC_PRIMITIVE_SCRIPTS = tuple( | ||
| path | ||
| for path in sorted(PRIMITIVES_DIRECTORY.glob("*.py")) | ||
| if not path.name.startswith("_") | ||
| ) |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Description
python scripts/tutorials/atomic_action/turn_knob.py (--rigid_object)python scripts/tutorials/atomic_action/press_button.py (--rigid_object)python scripts/tutorials/atomic_action/pull_push_articulated_part.pyType of change
Checklist
black .command to format the code base.