Interactive Robot Control with Gizmo

Interactive Robot Control with Gizmo#

This tutorial demonstrates how to use the Gizmo class for interactive robot manipulation in SimulationManager. You’ll learn how to create a gizmo attached to a robot’s end-effector and use it for real-time inverse kinematics (IK) control, allowing intuitive manipulation of robot poses through visual interaction.

The Code#

The tutorial corresponds to the gizmo_robot.py script in the scripts/tutorials/sim directory.

Code for gizmo_robot.py
  1# ----------------------------------------------------------------------------
  2# Copyright (c) 2021-2025 DexForce Technology Co., Ltd.
  3#
  4# Licensed under the Apache License, Version 2.0 (the "License");
  5# you may not use this file except in compliance with the License.
  6# You may obtain a copy of the License at
  7#
  8#     http://www.apache.org/licenses/LICENSE-2.0
  9#
 10# Unless required by applicable law or agreed to in writing, software
 11# distributed under the License is distributed on an "AS IS" BASIS,
 12# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 13# See the License for the specific language governing permissions and
 14# limitations under the License.
 15# ----------------------------------------------------------------------------
 16"""
 17Gizmo-Robot Example: Test Gizmo class on a robot (UR10)
 18"""
 19
 20import time
 21import torch
 22import numpy as np
 23import argparse
 24
 25from embodichain.lab.sim import SimulationManager, SimulationManagerCfg
 26from embodichain.lab.sim.cfg import (
 27    RobotCfg,
 28    URDFCfg,
 29    JointDrivePropertiesCfg,
 30)
 31
 32from embodichain.lab.sim.solvers import PinkSolverCfg
 33from embodichain.data import get_data_path
 34from embodichain.utils import logger
 35
 36
 37def main():
 38    """Main function to create and run the simulation scene."""
 39
 40    # Parse command line arguments
 41    parser = argparse.ArgumentParser(
 42        description="Create a simulation scene with SimulationManager"
 43    )
 44    parser.add_argument(
 45        "--num_envs", type=int, default=1, help="Number of parallel environments"
 46    )
 47    parser.add_argument(
 48        "--device", type=str, default="cpu", help="Simulation device (cuda or cpu)"
 49    )
 50    parser.add_argument(
 51        "--enable_rt",
 52        action="store_true",
 53        default=False,
 54        help="Enable ray tracing for better visuals",
 55    )
 56    args = parser.parse_args()
 57
 58    # Configure the simulation
 59    sim_cfg = SimulationManagerCfg(
 60        width=1920,
 61        height=1080,
 62        physics_dt=1.0 / 100.0,
 63        sim_device=args.device,
 64        enable_rt=args.enable_rt,
 65    )
 66
 67    sim = SimulationManager(sim_cfg)
 68    sim.set_manual_update(False)
 69
 70    # Build multiple arenas if requested
 71    if args.num_envs > 1:
 72        sim.build_multiple_arenas(args.num_envs, space=3.0)
 73
 74    # Get UR10 URDF path
 75    urdf_path = get_data_path("UniversalRobots/UR10/UR10.urdf")
 76
 77    # Create UR10 robot
 78    robot_cfg = RobotCfg(
 79        uid="ur10_gizmo_test",
 80        urdf_cfg=URDFCfg(
 81            components=[{"component_type": "arm", "urdf_path": urdf_path}]
 82        ),
 83        control_parts={"arm": ["Joint[1-6]"]},
 84        solver_cfg={
 85            "arm": PinkSolverCfg(
 86                urdf_path=urdf_path,
 87                end_link_name="ee_link",
 88                root_link_name="base_link",
 89                pos_eps=1e-2,
 90                rot_eps=5e-2,
 91                max_iterations=300,
 92                dt=0.1,
 93            )
 94        },
 95        drive_pros=JointDrivePropertiesCfg(
 96            stiffness={"Joint[1-6]": 1e4},
 97            damping={"Joint[1-6]": 1e3},
 98        ),
 99    )
100    robot = sim.add_robot(cfg=robot_cfg)
101
102    # Set initial joint positions
103    initial_qpos = torch.tensor(
104        [[0, -np.pi / 2, np.pi / 2, 0.0, np.pi / 2, 0.0]],
105        dtype=torch.float32,
106        device="cpu",
107    )
108    joint_ids = robot.get_joint_ids("arm")
109    robot.set_qpos(qpos=initial_qpos, joint_ids=joint_ids)
110
111    time.sleep(0.2)  # Wait for a moment to ensure everything is set up
112
113    # Enable gizmo using the new API
114    sim.enable_gizmo(uid="ur10_gizmo_test", control_part="arm")
115    if not sim.has_gizmo("ur10_gizmo_test", control_part="arm"):
116        logger.log_error("Failed to enable gizmo!")
117        return
118
119    sim.open_window()
120
121    logger.log_info("Gizmo-Robot example started!")
122    logger.log_info("Use the gizmo to drag the robot end-effector (EE)")
123    logger.log_info("Press Ctrl+C to stop the simulation")
124
125    run_simulation(sim)
126
127
128def run_simulation(sim: SimulationManager):
129    step_count = 0
130    try:
131        last_time = time.time()
132        last_step = 0
133        while True:
134            time.sleep(0.033)  # 30Hz
135            # Update all gizmos managed by sim
136            sim.update_gizmos()
137            step_count += 1
138
139            if step_count % 100 == 0:
140                current_time = time.time()
141                elapsed = current_time - last_time
142                fps = (
143                    sim.num_envs * (step_count - last_step) / elapsed
144                    if elapsed > 0
145                    else 0
146                )
147                logger.log_info(f"Simulation step: {step_count}, FPS: {fps:.2f}")
148                last_time = current_time
149                last_step = step_count
150    except KeyboardInterrupt:
151        logger.log_info("\nStopping simulation...")
152    finally:
153        sim.destroy()
154        logger.log_info("Simulation terminated successfully")
155
156
157if __name__ == "__main__":
158    main()

The Code Explained#

Similar to the previous tutorial on robot simulation, we use the SimulationManager class to set up the simulation environment. If you haven’t read that tutorial yet, please refer to Simulating a Robot first.

Important: Gizmo only supports single environment mode (num_envs=1). Using multiple environments will raise an exception.

All gizmo creation, visibility, and destruction operations must be managed via the SimulationManager API:

# Toggle visibility for a gizmo
sim.toggle_gizmo_visibility("ur10_gizmo_test", control_part="arm")

# Set visibility explicitly
sim.set_gizmo_visibility("ur10_gizmo_test", visible=False, control_part="arm")

Always use the SimulationManager API to control gizmo visibility and lifecycle. Do not operate on the Gizmo instance directly.

What is a Gizmo?#

A Gizmo is an interactive visual tool that allows users to manipulate simulation objects in real-time through mouse interactions. In robotics applications, gizmos are particularly useful for:

  • Interactive Robot Control: Drag the robot’s end-effector to desired positions

  • Inverse Kinematics: Automatically solve joint angles to reach target poses

  • Real-time Manipulation: Provide immediate visual feedback during robot motion planning

  • Debugging and Visualization: Test robot reachability and workspace limits

The objects.Gizmo class provides a unified interface for interactive control of different simulation elements including robots, rigid objects, and cameras.

Setting up Robot Configuration#

First, we configure a UR10 robot with an IK solver for end-effector control:

Key components of the robot configuration:

  • URDF Configuration: Loads the robot’s kinematic and visual model

  • Control Parts: Defines which joints can be controlled ("Joint[1-6]" for UR10)

  • IK Solver: solvers.PinkSolverCfg provides inverse kinematics capabilities

  • Drive Properties: Sets stiffness and damping for joint control

The IK solver is crucial for gizmo functionality, as it enables the robot to automatically calculate joint angles needed to reach gizmo target positions.

Creating and Attaching a Gizmo#

After configuring the robot, enable the gizmo for interactive control using the SimulationManager API (supports robot, rigid object, camera; key is uid:control_part):

# Enable gizmo for the robot's arm
sim.enable_gizmo(uid="ur10_gizmo_test", control_part="arm")
if not sim.has_gizmo("ur10_gizmo_test", control_part="arm"):
    logger.log_error("Failed to enable gizmo!")
    return

The Gizmo instance is managed internally by SimulationManager. If you need to access it:

gizmo = sim.get_gizmo("ur10_gizmo_test", control_part="arm")

The Gizmo system will automatically:

  1. Detect Target Type: Identify that the target is a robot (vs. rigid object or camera)

  2. Find End-Effector: Locate the robot’s end-effector link (ee_link for UR10)

  3. Create Proxy Object: Generate a small invisible cube at the end-effector position

  4. Set Up IK Callback: Configure the gizmo to trigger IK solving when moved

How Gizmo-Robot Interaction Works#

The gizmo-robot interaction follows this efficient workflow:

  1. Gizmo Callback: When the user drags the gizmo, a callback function updates the proxy object’s transform

  2. Deferred IK Solving: Instead of solving IK immediately in the callback (which causes UI lag), the target transform is stored

  3. Update Loop: During each simulation step, gizmo.update() solves IK and applies joint commands

  4. Robot Motion: The robot smoothly moves to follow the gizmo position

This design separates UI responsiveness from computational IK solving, ensuring smooth interaction even with complex robots.

The Simulation Loop#

In the main loop, simply call sim.update_gizmos(). There is no need to manually update any Gizmo instance.

def run_simulation(sim: SimulationManager):
    step_count = 0
    try:
        last_time = time.time()
        last_step = 0
        while True:
            time.sleep(0.033)  # 30Hz
            sim.update_gizmos()  # Update all gizmos
            step_count += 1
            # ...performance statistics, etc...
    except KeyboardInterrupt:
        logger.log_info("\nStopping simulation...")
    finally:
        sim.destroy()  # Release all resources
        logger.log_info("Simulation terminated successfully")

Main loop highlights:

  • Gizmo update: Only sim.update_gizmos() is needed, no gizmo.update()

  • Performance monitoring: Optional FPS statistics

  • Resource cleanup: Only sim.destroy() is needed, no manual Gizmo destruction

  • Graceful shutdown: Supports Ctrl+C interruption

Gizmo Lifecycle Management#

Gizmo lifecycle is managed by SimulationManager:

  • Enable: sim.enable_gizmo(…)

  • Update: Main loop automatically calls sim.update_gizmos()

  • Destroy/disable: sim.disable_gizmo(…) or sim.destroy() (recommended)

There is no need to manually create or destroy Gizmo instances. All resources are managed by SimulationManager.

Available Gizmo Methods#

If you need to access the underlying Gizmo instance (via sim.get_gizmo), you can use the following methods:

Transform Control:

  • set_world_pose(pose): Set gizmo world position and orientation

  • get_world_pose(): Get current gizmo world transform

  • set_local_pose(pose): Set gizmo local transform relative to parent

  • get_local_pose(): Get gizmo local transform

Visual properties (strongly recommend using SimulationManager API):

  • sim.toggle_gizmo_visibility(uid, control_part=None): Toggle gizmo visibility

  • sim.set_gizmo_visibility(uid, visible, control_part=None): Set gizmo visibility

Hierarchy Management:

  • get_parent(): Get gizmo’s parent node in scene hierarchy

  • get_name(): Get gizmo node name for debugging

  • detach(): Disconnect gizmo from current target

  • attach(target): Attach gizmo to a new simulation object

Running the Tutorial#

To run the gizmo robot tutorial:

cd scripts/tutorials/sim
python gizmo_robot.py --device cpu

Command-line options:

  • --device cpu|cuda: Choose simulation device

  • --num_envs N: Number of parallel environments

  • --headless: Run without GUI for automated testing

  • --enable_rt: Enable ray tracing for better visuals

Once running:

  1. Mouse Interaction: Click and drag the gizmo (colorful axes) to move the robot

  2. Real-time IK: Watch the robot joints automatically adjust to follow the gizmo

  3. Workspace Limits: Observe how the robot behaves at workspace boundaries

  4. Performance: Monitor FPS in the console output

Tips and Best Practices#

Performance optimization:

  • Only call sim.update_gizmos() in the main loop, no need for gizmo.update()

  • Reduce IK solver iterations for better real-time performance if needed

  • Use set_manual_update(False) for smoother interaction

Debugging tips:

  • Check console output for IK solver success/failure messages

  • Use get_world_pose() to check gizmo position (if needed)

  • Monitor FPS to identify performance bottlenecks

Robot compatibility:

  • Ensure your robot is configured with a correct IK solver

  • Check the end-effector (EE) link name

  • Test joint limits and workspace boundaries

Visualization customization:

  • Adjust gizmo appearance via Gizmo config (e.g., set_line_width(); requires access to the instance via sim.get_gizmo)

  • Adjust gizmo scale according to robot size

  • Enable collision for debugging if needed

Next Steps#

After mastering basic gizmo usage, you can explore:

  • Multi-robot Gizmos: Attach gizmos to multiple robots simultaneously

  • Custom Gizmo Callbacks: Implement application-specific interaction logic

  • Gizmo with Rigid Objects: Use gizmos for interactive object manipulation

  • Advanced IK Configuration: Fine-tune solver parameters for specific robots

For more advanced robot control and simulation features, refer to the complete Simulating a Robot tutorial and the API documentation for objects.Gizmo and solvers.PinkSolverCfg.

Contents