Add gizmo helper tool for robot control by yuecideng · Pull Request #132 · DexForce/EmbodiChain

yuecideng · 2026-02-12T13:23:26Z

Description

This PR add a help function to run gizmo control for robot in loop.

Type of change

New feature (non-breaking change which adds functionality)

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 introduces an interactive helper utility to drive robot gizmo controls in a loop (with keyboard shortcuts) and adjusts the simulation manager’s enable_gizmo API to return the created gizmo object.

Changes:

Added run_gizmo_robot_control_loop() helper for interactive gizmo-based robot manipulation via keyboard input.
Updated SimulationManager.enable_gizmo() return annotation and added an explicit return gizmo.

Reviewed changes

Copilot reviewed 2 out of 2 changed files in this pull request and generated 12 comments.

File	Description
`embodichain/lab/sim/utility/gizmo_utils.py`	Adds an interactive terminal-driven gizmo control loop helper for robots.
`embodichain/lab/sim/sim_manager.py`	Changes `enable_gizmo` to (attempt to) return the created `Gizmo`.

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

embodichain/lab/sim/utility/gizmo_utils.py

Copilot · 2026-02-12T13:28:16Z

embodichain/lab/sim/sim_manager.py

    def enable_gizmo(
        self, uid: str, control_part: str | None = None, gizmo_cfg: object = None
-    ) -> None:
+    ) -> Gizmo:
        """Enable gizmo control for any simulation object (Robot, RigidObject, Camera, etc.).


The return type was changed to Gizmo, but this method still has code paths that return without a value (e.g., when the gizmo already exists or the UID isn't found). To keep the API consistent, either return the existing gizmo / raise, or change the annotation to Gizmo | None and return None explicitly on those paths.

embodichain/lab/sim/utility/gizmo_utils.py

Copilot · 2026-02-12T13:28:17Z

embodichain/lab/sim/sim_manager.py

        except Exception as e:
            logger.log_error(
                f"Failed to create gizmo for {object_type} '{uid}' with control_part '{control_part}': {e}"
            )

+        return gizmo


If gizmo creation throws, the except block logs but execution falls through to return gizmo. In that case gizmo may be unbound, raising UnboundLocalError. Initialize gizmo = None before the try and return None (or re-raise) on failure.

embodichain/lab/sim/utility/gizmo_utils.py

Copilot

Pull request overview

Copilot reviewed 2 out of 2 changed files in this pull request and generated 8 comments.

Comments suppressed due to low confidence (1)

embodichain/lab/sim/sim_manager.py:1149

enable_gizmo is annotated to return Gizmo, but there are code paths that return None (e.g., when the gizmo already exists or the target uid is missing). This is now a type/API mismatch and can break callers that rely on the return value. Update the signature to Gizmo | None (or return the existing gizmo instance when already enabled) and make the return behavior consistent.

    def enable_gizmo(
        self, uid: str, control_part: str | None = None, gizmo_cfg: object = None
    ) -> Gizmo:
        """Enable gizmo control for any simulation object (Robot, RigidObject, Camera, etc.).

        Args:
            uid (str): UID of the object to attach gizmo to (searches in robots, rigid_objects, sensors, etc.)
            control_part (str | None, optional): Control part name for robots. Defaults to "arm".
            gizmo_cfg (object, optional): Gizmo configuration object. Defaults to None.
        """
        # Create gizmo key combining uid and control_part
        gizmo_key = f"{uid}:{control_part}" if control_part else uid

        # Check if gizmo already exists
        if gizmo_key in self._gizmos:
            logger.log_warning(
                f"Gizmo for '{uid}' with control_part '{control_part}' already exists."
            )
            return

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

Copilot · 2026-02-13T07:21:53Z

embodichain/lab/sim/utility/gizmo_utils.py

+                    eef_pose = robot.compute_fk(name=control_part, qpos=current_qpos)
+                    log_info(f"\n=== Robot State ===")
+                    log_info(f"Control part: {control_part}")
+                    log_info(f"Joint positions: {current_qpos.squeeze().tolist()}")
+                    log_info(f"End-effector pose:\n{eef_pose.squeeze().numpy()}")
+
+                    if eef_pose is None:


eef_pose is logged via eef_pose.squeeze().numpy() before checking whether compute_fk returned None, and .numpy() is not valid on a torch tensor without moving to CPU/detaching. This will throw when eef_pose is None or a CUDA tensor. Only log the pose after the None check, and convert with eef_pose.detach().cpu().numpy() (or similar).

Copilot · 2026-02-13T07:21:53Z

embodichain/lab/sim/utility/gizmo_utils.py

+    robot_solver = robot.get_solver(name=control_part)
+    control_part_link_names = robot.get_control_part_link_names(name=control_part)
+    end_link_name = (
+        control_part_link_names[-1] if end_link_name is None else end_link_name
+    )
+    pink_solver_cfg = PinkSolverCfg(
+        urdf_path=robot.cfg.fpath,
+        end_link_name=end_link_name,
+        root_link_name=robot_solver.root_link_name,
+        pos_eps=1e-2,


robot.get_solver(name=control_part) can return None (e.g., if solvers aren’t configured), and get_control_part_link_names can return an empty list when control_part is invalid. This code dereferences robot_solver.root_link_name and indexes control_part_link_names[-1] unconditionally, which can crash. Add validation/error handling for missing solver / missing links before building PinkSolverCfg.

Copilot · 2026-02-13T07:21:53Z

embodichain/lab/sim/utility/gizmo_utils.py

+    from embodichain.lab.sim import SimulationManager
+    from embodichain.lab.sim.objects import Robot
+    from embodichain.lab.sim.solvers import PinkSolverCfg
+
+    from embodichain.utils.logger import log_info, log_warning, log_error
+
+    sim = SimulationManager.get_instance()
+
+    # Enter auto-update mode.
+    sim.set_manual_update(False)
+
+    # Replace robot's default solver with PinkSolver for gizmo control.
+    robot_solver = robot.get_solver(name=control_part)
+    control_part_link_names = robot.get_control_part_link_names(name=control_part)
+    end_link_name = (
+        control_part_link_names[-1] if end_link_name is None else end_link_name
+    )
+    pink_solver_cfg = PinkSolverCfg(
+        urdf_path=robot.cfg.fpath,
+        end_link_name=end_link_name,
+        root_link_name=robot_solver.root_link_name,
+        pos_eps=1e-2,
+        rot_eps=5e-2,
+        max_iterations=300,
+        dt=0.1,
+    )
+    robot.init_solver(cfg={control_part: pink_solver_cfg})
+
+    # Enable gizmo for the robot
+    gizmo = sim.enable_gizmo(uid=robot.uid, control_part=control_part)
+


Several imports/variables in this helper are unused (Robot imported inside the function, log_warning, log_error, and the gizmo local). Please remove unused imports/assignments to keep this utility lean and avoid implying side effects that don’t exist.

Copilot · 2026-02-13T07:21:54Z

embodichain/lab/sim/utility/gizmo_utils.py

+    # Enter auto-update mode.
+    sim.set_manual_update(False)
+


This helper switches the global simulation into auto-update mode via sim.set_manual_update(False) but never restores the previous setting on exit. That can unexpectedly change how the rest of the program advances the simulation after this loop ends. Consider saving the prior mode (if available) and restoring it in finally, or explicitly setting it back to the project default (manual update True).

Copilot · 2026-02-13T07:21:54Z

embodichain/lab/sim/utility/gizmo_utils.py

+    old_settings = termios.tcgetattr(sys.stdin)
+    tty.setcbreak(sys.stdin.fileno())


termios.tcgetattr expects a file descriptor (int) but this passes sys.stdin (a file object). This will raise TypeError on most platforms. Use sys.stdin.fileno() (and consider handling non-TTY stdin cases) when saving terminal settings.

Suggested change

old_settings = termios.tcgetattr(sys.stdin)

tty.setcbreak(sys.stdin.fileno())

stdin_fd = sys.stdin.fileno()

old_settings = termios.tcgetattr(stdin_fd)

tty.setcbreak(stdin_fd)

Copilot · 2026-02-13T07:21:54Z

embodichain/lab/sim/utility/gizmo_utils.py

+    robot.init_solver(cfg={control_part: pink_solver_cfg})
+
+    # Enable gizmo for the robot
+    gizmo = sim.enable_gizmo(uid=robot.uid, control_part=control_part)


Variable gizmo is not used.

Suggested change

gizmo = sim.enable_gizmo(uid=robot.uid, control_part=control_part)

sim.enable_gizmo(uid=robot.uid, control_part=control_part)

Copilot · 2026-02-13T07:21:54Z

embodichain/lab/sim/utility/gizmo_utils.py

+        try:
+            # Restore terminal settings
+            termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
+        except:


'except' clause does nothing but pass and there is no explanatory comment.

Suggested change

except:

except:

# Intentionally ignore errors while restoring terminal settings during shutdown

Copilot · 2026-02-13T07:21:54Z

embodichain/lab/sim/utility/gizmo_utils.py

+        try:
+            # Restore terminal settings
+            termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
+        except:


Except block directly handles BaseException.

Suggested change

except:

except Exception:

Copilot AI review requested due to automatic review settings February 12, 2026 13:23

yuecideng added robot Module related to robot interaction The interaction simulation features labels Feb 12, 2026

Copilot started reviewing on behalf of yuecideng February 12, 2026 13:23 View session

Copilot AI reviewed Feb 12, 2026

View reviewed changes

yuecideng added 4 commits February 13, 2026 15:17

wip

ac62f6e

wip

57ea75c

wip

b724029

wip

b75065b

Copilot AI review requested due to automatic review settings February 13, 2026 07:17

yuecideng force-pushed the yueci/add-gizmo-control branch from 8b7b80c to b75065b Compare February 13, 2026 07:17

Copilot started reviewing on behalf of yuecideng February 13, 2026 07:18 View session

Copilot AI reviewed Feb 13, 2026

View reviewed changes

yuecideng merged commit 342f284 into main Feb 13, 2026
11 checks passed

yuecideng deleted the yueci/add-gizmo-control branch February 13, 2026 07:35

		old_settings = termios.tcgetattr(sys.stdin)
		tty.setcbreak(sys.stdin.fileno())

-    old_settings = termios.tcgetattr(sys.stdin)
-    tty.setcbreak(sys.stdin.fileno())
+    stdin_fd = sys.stdin.fileno()
+    old_settings = termios.tcgetattr(stdin_fd)
+    tty.setcbreak(stdin_fd)

	gizmo = sim.enable_gizmo(uid=robot.uid, control_part=control_part)
	sim.enable_gizmo(uid=robot.uid, control_part=control_part)

	except:
	except:
	# Intentionally ignore errors while restoring terminal settings during shutdown

Conversation

yuecideng commented Feb 12, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Type of change

Checklist

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Uh oh!

Uh oh!

Copilot AI Feb 12, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Copilot AI Feb 12, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Uh oh!

Copilot AI Feb 13, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 13, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 13, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 13, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 13, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 13, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 13, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 13, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

yuecideng commented Feb 12, 2026 •

edited

Loading