Skip to content

Commit f89c2e8

Browse files
MilotrinceMilotrince
committed
move keybindings to vis
1 parent c8005f5 commit f89c2e8

File tree

12 files changed

+153
-111
lines changed

12 files changed

+153
-111
lines changed

‎examples/IPC_Solver/ipc_arm_cloth.py‎

Lines changed: 32 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -25,10 +25,10 @@
2525

2626
import numpy as np
2727
from huggingface_hub import snapshot_download
28-
from scipy.spatial.transform import Rotation as R
2928

3029
import genesis as gs
31-
from genesis.ext.pyrender.interaction.keybindings import KeyAction, Keybind
30+
import genesis.utils.geom as gu
31+
from genesis.vis.keybindings import Key, KeyAction, Keybind
3232

3333

3434
def build_scene(use_ipc=False, show_viewer=False, enable_ipc_gui=False):
@@ -152,9 +152,9 @@ def run_sim(scene, entities, mode="interactive", trajectory_file=None):
152152
target_entity = entities["target"]
153153

154154
robot_init_pos = np.array([0.5, 0, 0.55])
155-
robot_init_R = R.from_euler("y", np.pi)
155+
robot_init_quat = gu.xyz_to_quat(np.array([0, np.pi, 0])) # Rotation around Y axis
156156
target_pos = [robot_init_pos.copy()] # Use list for mutability in closures
157-
target_R = [robot_init_R] # Use list for mutability in closures
157+
target_quat = [robot_init_quat.copy()] # Use list for mutability in closures
158158

159159
n_dofs = robot.n_dofs
160160
motors_dof = np.arange(n_dofs - 2)
@@ -174,10 +174,9 @@ def run_sim(scene, entities, mode="interactive", trajectory_file=None):
174174

175175
def reset_scene():
176176
target_pos[0] = robot_init_pos.copy()
177-
target_R[0] = robot_init_R
178-
target_quat = target_R[0].as_quat(scalar_first=True)
179-
target_entity.set_qpos(np.concatenate([target_pos[0], target_quat]))
180-
q = robot.inverse_kinematics(link=ee_link, pos=target_pos[0], quat=target_quat)
177+
target_quat[0] = robot_init_quat.copy()
178+
target_entity.set_qpos(np.concatenate([target_pos[0], target_quat[0]]))
179+
q = robot.inverse_kinematics(link=ee_link, pos=target_pos[0], quat=target_quat[0])
181180
robot.set_qpos(q[:-2], motors_dof)
182181

183182
# entities["cube"].set_pos((random.uniform(0.2, 0.4), random.uniform(-0.2, 0.2), 0.05))
@@ -190,29 +189,32 @@ def move(dpos_delta: tuple[float, float, float]):
190189
target_pos[0] += np.array(dpos_delta, dtype=gs.np_float)
191190

192191
def rotate(axis: str, angle: float):
193-
target_R[0] = R.from_euler(axis, angle) * target_R[0]
192+
# Create rotation quaternion for the specified axis
193+
euler = np.zeros(3)
194+
axis_map = {"x": 0, "y": 1, "z": 2}
195+
euler[axis_map[axis]] = angle
196+
drot_quat = gu.xyz_to_quat(euler)
197+
target_quat[0] = gu.transform_quat_by_quat(target_quat[0], drot_quat)
194198

195199
def toggle_gripper(close: bool = True):
196200
gripper_closed[0] = close
197201

198-
from pyglet.window import key
199-
200202
scene.viewer.register_keybinds(
201-
Keybind(key.UP, KeyAction.HOLD, name="move_forward", callback=move, args=((-dpos, 0, 0),)),
202-
Keybind(key.DOWN, KeyAction.HOLD, name="move_backward", callback=move, args=((dpos, 0, 0),)),
203-
Keybind(key.LEFT, KeyAction.HOLD, name="move_left", callback=move, args=((0, -dpos, 0),)),
204-
Keybind(key.RIGHT, KeyAction.HOLD, name="move_right", callback=move, args=((0, dpos, 0),)),
205-
Keybind(key.N, KeyAction.HOLD, name="move_up", callback=move, args=((0, 0, dpos),)),
206-
Keybind(key.M, KeyAction.HOLD, name="move_down", callback=move, args=((0, 0, -dpos),)),
207-
Keybind(key.J, KeyAction.HOLD, name="yaw_left", callback=rotate, args=("z", drot)),
208-
Keybind(key.K, KeyAction.HOLD, name="yaw_right", callback=rotate, args=("z", -drot)),
209-
Keybind(key.I, KeyAction.HOLD, name="pitch_up", callback=rotate, args=("y", drot)),
210-
Keybind(key.O, KeyAction.HOLD, name="pitch_down", callback=rotate, args=("y", -drot)),
211-
Keybind(key.L, KeyAction.HOLD, name="roll_left", callback=rotate, args=("x", drot)),
212-
Keybind(key.SEMICOLON, KeyAction.HOLD, name="roll_right", callback=rotate, args=("x", -drot)),
213-
Keybind(key.U, KeyAction.HOLD, name="reset_scene", callback=reset_scene),
214-
Keybind(key.SPACE, KeyAction.PRESS, name="close_gripper", callback=toggle_gripper, args=(True,)),
215-
Keybind(key.SPACE, KeyAction.RELEASE, name="open_gripper", callback=toggle_gripper, args=(False,)),
203+
Keybind(Key.UP, KeyAction.HOLD, name="move_forward", callback=move, args=((-dpos, 0, 0),)),
204+
Keybind(Key.DOWN, KeyAction.HOLD, name="move_backward", callback=move, args=((dpos, 0, 0),)),
205+
Keybind(Key.LEFT, KeyAction.HOLD, name="move_left", callback=move, args=((0, -dpos, 0),)),
206+
Keybind(Key.RIGHT, KeyAction.HOLD, name="move_right", callback=move, args=((0, dpos, 0),)),
207+
Keybind(Key.N, KeyAction.HOLD, name="move_up", callback=move, args=((0, 0, dpos),)),
208+
Keybind(Key.M, KeyAction.HOLD, name="move_down", callback=move, args=((0, 0, -dpos),)),
209+
Keybind(Key.J, KeyAction.HOLD, name="yaw_left", callback=rotate, args=("z", drot)),
210+
Keybind(Key.K, KeyAction.HOLD, name="yaw_right", callback=rotate, args=("z", -drot)),
211+
Keybind(Key.I, KeyAction.HOLD, name="pitch_up", callback=rotate, args=("y", drot)),
212+
Keybind(Key.O, KeyAction.HOLD, name="pitch_down", callback=rotate, args=("y", -drot)),
213+
Keybind(Key.L, KeyAction.HOLD, name="roll_left", callback=rotate, args=("x", drot)),
214+
Keybind(Key.SEMICOLON, KeyAction.HOLD, name="roll_right", callback=rotate, args=("x", -drot)),
215+
Keybind(Key.U, KeyAction.HOLD, name="reset_scene", callback=reset_scene),
216+
Keybind(Key.SPACE, KeyAction.PRESS, name="close_gripper", callback=toggle_gripper, args=(True,)),
217+
Keybind(Key.SPACE, KeyAction.RELEASE, name="open_gripper", callback=toggle_gripper, args=(False,)),
216218
)
217219

218220
# Load trajectory if in playback mode
@@ -278,7 +280,7 @@ def toggle_gripper(close: bool = True):
278280
if step_count < len(trajectory):
279281
step_data = trajectory[step_count]
280282
target_pos[0] = step_data["target_pos"]
281-
target_R[0] = R.from_quat(step_data["target_quat"])
283+
target_quat[0] = step_data["target_quat"]
282284
gripper_closed[0] = step_data["gripper_closed"]
283285
step_count += 1
284286
print(f"\rPlayback step: {step_count}/{len(trajectory)}", end="")
@@ -292,16 +294,15 @@ def toggle_gripper(close: bool = True):
292294
if recording:
293295
step_data = {
294296
"target_pos": target_pos[0].copy(),
295-
"target_quat": target_R[0].as_quat(), # x,y,z,w format
297+
"target_quat": target_quat[0].copy(),
296298
"gripper_closed": gripper_closed[0],
297299
"step": step_count,
298300
}
299301
trajectory.append(step_data)
300302

301303
# control arm
302-
target_quat = target_R[0].as_quat(scalar_first=True)
303-
target_entity.set_qpos(np.concatenate([target_pos[0], target_quat]))
304-
q, err = robot.inverse_kinematics(link=ee_link, pos=target_pos[0], quat=target_quat, return_error=True)
304+
target_entity.set_qpos(np.concatenate([target_pos[0], target_quat[0]]))
305+
q, err = robot.inverse_kinematics(link=ee_link, pos=target_pos[0], quat=target_quat[0], return_error=True)
305306
robot.control_dofs_position(q[:-2], motors_dof)
306307
# control gripper
307308
if gripper_closed[0]:

‎examples/drone/interactive_drone.py‎

Lines changed: 7 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
import numpy as np
44

55
import genesis as gs
6-
from genesis.ext.pyrender.interaction.keybindings import KeyAction, Keybind
6+
from genesis.vis.keybindings import Key, KeyAction, Keybind
77

88

99
class DroneController:
@@ -79,8 +79,6 @@ def main():
7979
scene.build()
8080

8181
# Register keybindings
82-
from pyglet.window import key
83-
8482
def direction_keybinds(key_code, name: str, direction: tuple[float, float, float, float]):
8583
"""Helper to create press/release keybinds for a direction"""
8684
dir_arr = np.array(direction)
@@ -102,12 +100,12 @@ def direction_keybinds(key_code, name: str, direction: tuple[float, float, float
102100
]
103101

104102
scene.viewer.register_keybinds(
105-
*direction_keybinds(key.UP, "move_forward", (1.0, 1.0, -1.0, -1.0)),
106-
*direction_keybinds(key.DOWN, "move_backward", (-1.0, -1.0, 1.0, 1.0)),
107-
*direction_keybinds(key.LEFT, "move_left", (-1.0, 1.0, -1.0, 1.0)),
108-
*direction_keybinds(key.RIGHT, "move_right", (1.0, -1.0, 1.0, -1.0)),
109-
Keybind(key.SPACE, KeyAction.HOLD, name="accelerate", callback=controller.accelerate),
110-
Keybind(key.LSHIFT, KeyAction.HOLD, name="decelerate", callback=controller.decelerate),
103+
*direction_keybinds(Key.UP, "move_forward", (1.0, 1.0, -1.0, -1.0)),
104+
*direction_keybinds(Key.DOWN, "move_backward", (-1.0, -1.0, 1.0, 1.0)),
105+
*direction_keybinds(Key.LEFT, "move_left", (-1.0, 1.0, -1.0, 1.0)),
106+
*direction_keybinds(Key.RIGHT, "move_right", (1.0, -1.0, 1.0, -1.0)),
107+
Keybind(Key.SPACE, KeyAction.HOLD, name="accelerate", callback=controller.accelerate),
108+
Keybind(Key.LSHIFT, KeyAction.HOLD, name="decelerate", callback=controller.decelerate),
111109
)
112110

113111
# Print control instructions

‎examples/keyboard_teleop.py‎

Lines changed: 24 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -18,10 +18,10 @@
1818
import random
1919

2020
import numpy as np
21-
from scipy.spatial.transform import Rotation as R
2221

2322
import genesis as gs
24-
from genesis.ext.pyrender.interaction.keybindings import KeyAction, Keybind
23+
import genesis.utils.geom as gu
24+
from genesis.vis.keybindings import Key, KeyAction, Keybind
2525

2626
if __name__ == "__main__":
2727
########################## init ##########################
@@ -86,7 +86,7 @@
8686

8787
# Initialize robot control state
8888
robot_init_pos = np.array([0.5, 0, 0.55])
89-
robot_init_R = R.from_euler("y", np.pi)
89+
robot_init_quat = gu.xyz_to_quat(np.array([0, np.pi, 0])) # Rotation around Y axis
9090

9191
# Get DOF indices
9292
n_dofs = robot.n_dofs
@@ -96,7 +96,7 @@
9696

9797
# Initialize target pose
9898
target_pos = robot_init_pos.copy()
99-
target_R = [robot_init_R] # Use list to make it mutable in closures
99+
target_quat = [robot_init_quat.copy()] # Use list to make it mutable in closures
100100

101101
# Control parameters
102102
dpos = 0.002
@@ -106,15 +106,15 @@
106106
def reset_robot():
107107
"""Reset robot and cube to initial positions."""
108108
target_pos[:] = robot_init_pos.copy()
109-
target_R[0] = robot_init_R
110-
target_quat = target_R[0].as_quat(scalar_first=True)
111-
target.set_qpos(np.concatenate([target_pos, target_quat]))
112-
q = robot.inverse_kinematics(link=ee_link, pos=target_pos, quat=target_quat)
109+
target_quat[0] = robot_init_quat.copy()
110+
target.set_qpos(np.concatenate([target_pos, target_quat[0]]))
111+
q = robot.inverse_kinematics(link=ee_link, pos=target_pos, quat=target_quat[0])
113112
robot.set_qpos(q[:-2], motors_dof)
114113

115114
# Randomize cube position
116115
cube.set_pos((random.uniform(0.2, 0.4), random.uniform(-0.2, 0.2), 0.05))
117-
cube.set_quat(R.from_euler("z", random.uniform(0, np.pi * 2)).as_quat(scalar_first=True))
116+
random_angle = random.uniform(0, np.pi * 2)
117+
cube.set_quat(gu.xyz_to_quat(np.array([0, 0, random_angle])))
118118

119119
# Initialize robot pose
120120
reset_robot()
@@ -125,38 +125,36 @@ def move(dpos: tuple[float, float, float]):
125125
target_pos += np.array(dpos, dtype=gs.np_float)
126126

127127
def rotate(drot: float):
128-
target_R[0] = R.from_euler("z", drot) * target_R[0]
128+
drot_quat = gu.xyz_to_quat(np.array([0, 0, drot]))
129+
target_quat[0] = gu.transform_quat_by_quat(target_quat[0], drot_quat)
129130

130131
def toggle_gripper(close: bool = True):
131132
pos = -1.0 if close else 1.0
132133
robot.control_dofs_force(np.array([pos, pos]), fingers_dof)
133134

134135
# Register robot teleoperation keybindings
135-
from pyglet.window import key
136-
137136
scene.viewer.register_keybinds(
138-
Keybind(key.UP, KeyAction.HOLD, name="move_forward", callback=move, args=((-dpos, 0, 0),)),
139-
Keybind(key.DOWN, KeyAction.HOLD, name="move_back", callback=move, args=((dpos, 0, 0),)),
140-
Keybind(key.LEFT, KeyAction.HOLD, name="move_left", callback=move, args=((0, -dpos, 0),)),
141-
Keybind(key.RIGHT, KeyAction.HOLD, name="move_right", callback=move, args=((0, dpos, 0),)),
142-
Keybind(key.N, KeyAction.HOLD, name="move_up", callback=move, args=((0, 0, dpos),)),
143-
Keybind(key.M, KeyAction.HOLD, name="move_down", callback=move, args=((0, 0, -dpos),)),
144-
Keybind(key.J, KeyAction.HOLD, name="rotate_ccw", callback=rotate, args=(drot,)),
145-
Keybind(key.K, KeyAction.HOLD, name="rotate_cw", callback=rotate, args=(-drot,)),
146-
Keybind(key.U, KeyAction.HOLD, name="reset_scene", callback=reset_robot),
147-
Keybind(key.SPACE, KeyAction.PRESS, name="close_gripper", callback=toggle_gripper, args=(True,)),
148-
Keybind(key.SPACE, KeyAction.RELEASE, name="open_gripper", callback=toggle_gripper, args=(False,)),
137+
Keybind(Key.UP, KeyAction.HOLD, name="move_forward", callback=move, args=((-dpos, 0, 0),)),
138+
Keybind(Key.DOWN, KeyAction.HOLD, name="move_back", callback=move, args=((dpos, 0, 0),)),
139+
Keybind(Key.LEFT, KeyAction.HOLD, name="move_left", callback=move, args=((0, -dpos, 0),)),
140+
Keybind(Key.RIGHT, KeyAction.HOLD, name="move_right", callback=move, args=((0, dpos, 0),)),
141+
Keybind(Key.N, KeyAction.HOLD, name="move_up", callback=move, args=((0, 0, dpos),)),
142+
Keybind(Key.M, KeyAction.HOLD, name="move_down", callback=move, args=((0, 0, -dpos),)),
143+
Keybind(Key.J, KeyAction.HOLD, name="rotate_ccw", callback=rotate, args=(drot,)),
144+
Keybind(Key.K, KeyAction.HOLD, name="rotate_cw", callback=rotate, args=(-drot,)),
145+
Keybind(Key.U, KeyAction.HOLD, name="reset_scene", callback=reset_robot),
146+
Keybind(Key.SPACE, KeyAction.PRESS, name="close_gripper", callback=toggle_gripper, args=(True,)),
147+
Keybind(Key.SPACE, KeyAction.RELEASE, name="open_gripper", callback=toggle_gripper, args=(False,)),
149148
)
150149

151150
########################## run simulation ##########################
152151
try:
153152
while True:
154153
# Update target entity visualization
155-
target_quat = target_R[0].as_quat(scalar_first=True)
156-
target.set_qpos(np.concatenate([target_pos, target_quat]))
154+
target.set_qpos(np.concatenate([target_pos, target_quat[0]]))
157155

158156
# Control arm with inverse kinematics
159-
q, err = robot.inverse_kinematics(link=ee_link, pos=target_pos, quat=target_quat, return_error=True)
157+
q, err = robot.inverse_kinematics(link=ee_link, pos=target_pos, quat=target_quat[0], return_error=True)
160158
robot.control_dofs_position(q[:-2], motors_dof)
161159

162160
scene.step()

‎examples/sensors/lidar_teleop.py‎

Lines changed: 14 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -4,8 +4,8 @@
44
import numpy as np
55

66
import genesis as gs
7-
from genesis.ext.pyrender.interaction.keybindings import KeyAction, Keybind
87
from genesis.utils.geom import euler_to_quat
8+
from genesis.vis.keybindings import Key, KeyAction, Keybind
99

1010
# Position and angle increments for keyboard teleop control
1111
KEY_DPOS = 0.1
@@ -178,22 +178,20 @@ def yaw_cw():
178178
target_euler[2] -= KEY_DANGLE
179179

180180
# Register keybindings
181-
from pyglet.window import key
182-
183181
scene.viewer.register_keybinds(
184-
Keybind(key_code=key.UP, key_action=KeyAction.HOLD, name="move_forward", callback=move_forward),
185-
Keybind(key_code=key.DOWN, key_action=KeyAction.HOLD, name="move_backward", callback=move_backward),
186-
Keybind(key_code=key.RIGHT, key_action=KeyAction.HOLD, name="move_right", callback=move_right),
187-
Keybind(key_code=key.LEFT, key_action=KeyAction.HOLD, name="move_left", callback=move_left),
188-
Keybind(key_code=key.J, key_action=KeyAction.HOLD, name="move_down", callback=move_down),
189-
Keybind(key_code=key.K, key_action=KeyAction.HOLD, name="move_up", callback=move_up),
190-
Keybind(key_code=key.N, key_action=KeyAction.HOLD, name="roll_ccw", callback=roll_ccw),
191-
Keybind(key_code=key.M, key_action=KeyAction.HOLD, name="roll_cw", callback=roll_cw),
192-
Keybind(key_code=key.COMMA, key_action=KeyAction.HOLD, name="pitch_up", callback=pitch_up),
193-
Keybind(key_code=key.PERIOD, key_action=KeyAction.HOLD, name="pitch_down", callback=pitch_down),
194-
Keybind(key_code=key.O, key_action=KeyAction.HOLD, name="yaw_ccw", callback=yaw_ccw),
195-
Keybind(key_code=key.P, key_action=KeyAction.HOLD, name="yaw_cw", callback=yaw_cw),
196-
Keybind(key_code=key.BACKSLASH, key_action=KeyAction.HOLD, name="reset", callback=reset_pose),
182+
Keybind(key_code=Key.UP, key_action=KeyAction.HOLD, name="move_forward", callback=move_forward),
183+
Keybind(key_code=Key.DOWN, key_action=KeyAction.HOLD, name="move_backward", callback=move_backward),
184+
Keybind(key_code=Key.RIGHT, key_action=KeyAction.HOLD, name="move_right", callback=move_right),
185+
Keybind(key_code=Key.LEFT, key_action=KeyAction.HOLD, name="move_left", callback=move_left),
186+
Keybind(key_code=Key.J, key_action=KeyAction.HOLD, name="move_down", callback=move_down),
187+
Keybind(key_code=Key.K, key_action=KeyAction.HOLD, name="move_up", callback=move_up),
188+
Keybind(key_code=Key.N, key_action=KeyAction.HOLD, name="roll_ccw", callback=roll_ccw),
189+
Keybind(key_code=Key.M, key_action=KeyAction.HOLD, name="roll_cw", callback=roll_cw),
190+
Keybind(key_code=Key.COMMA, key_action=KeyAction.HOLD, name="pitch_up", callback=pitch_up),
191+
Keybind(key_code=Key.PERIOD, key_action=KeyAction.HOLD, name="pitch_down", callback=pitch_down),
192+
Keybind(key_code=Key.O, key_action=KeyAction.HOLD, name="yaw_ccw", callback=yaw_ccw),
193+
Keybind(key_code=Key.P, key_action=KeyAction.HOLD, name="yaw_cw", callback=yaw_cw),
194+
Keybind(key_code=Key.BACKSLASH, key_action=KeyAction.HOLD, name="reset", callback=reset_pose),
197195
)
198196

199197
# Print controls

0 commit comments

Comments
 (0)