Skip to content

[feat]:Add end effector rotation function #27

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
YilingQiao merged 5 commits into from
Sep 27, 2025
Merged

[feat]:Add end effector rotation function #27

Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
b846174
解决run_ppo_gs.py的keyError问题，完善teleop_wrapper.py的末端旋转功能
niyunjun Sep 11, 2025
af9fcba
修正下末端旋转功能的注释
niyunjun Sep 11, 2025
ee1a2ff
Merge branch 'main' into feature/my-feature
niyunjun Sep 12, 2025
343a5f6
convert Chinese annotations to English annotations
niyunjun Sep 12, 2025
02c75d5
Apply ruff auto-format
niyunjun Sep 13, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 22 additions & 2 deletions src/agent/gs_agent/wrappers/teleop_wrapper.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
import numpy as np
import torch
from pynput import keyboard
from scipy.spatial.transform import Rotation as R

from gs_agent.bases.env_wrapper import BaseEnvWrapper

Expand Down Expand Up @@ -255,6 +256,7 @@ def _process_input(self) -> KeyboardCommand:
# get ee target pose
is_close_gripper = False
dpos = 0.005
drot = 0.01
for key in pressed_keys:
if key == keyboard.Key.up:
self.target_position[0, 0] -= dpos
Expand All @@ -269,9 +271,27 @@ def _process_input(self) -> KeyboardCommand:
elif key == keyboard.KeyCode.from_char("m"):
self.target_position[0, 2] -= dpos
elif key == keyboard.KeyCode.from_char("j"):
raise NotImplementedError("Rotation not implemented")
# keep the end effector vertical down, only change the rotation around the x axis
current_rotation = R.from_quat(self.target_orientation[0, :].cpu().numpy())
# get the current x axis rotation angle
current_euler = current_rotation.as_euler("xyz", degrees=False)
# only change the x axis rotation angle
new_euler = [current_euler[0] + drot, current_euler[1], current_euler[2]]
new_rotation = R.from_euler("xyz", new_euler)
self.target_orientation[0, :] = torch.from_numpy(new_rotation.as_quat()).to(
self.target_orientation.device
)
elif key == keyboard.KeyCode.from_char("k"):
raise NotImplementedError("Rotation not implemented")
# keep the end effector vertical down, only change the rotation around the x axis
current_rotation = R.from_quat(self.target_orientation[0, :].cpu().numpy())
# get the current x axis rotation angle
current_euler = current_rotation.as_euler("xyz", degrees=False)
# only change the x axis rotation angle
new_euler = [current_euler[0] - drot, current_euler[1], current_euler[2]]
new_rotation = R.from_euler("xyz", new_euler)
self.target_orientation[0, :] = torch.from_numpy(new_rotation.as_quat()).to(
self.target_orientation.device
)
elif key == keyboard.Key.space:
is_close_gripper = True

Expand Down