Skip to content

Commit 7c2459c

Browse files
zhigenzhaozhigenzhao
authored
Merge pull request #1 from XR-Robotics/dev/hardware
Dev/hardware
2 parents c03fd1d + 46b250b commit 7c2459c

17 files changed

+1345
-39
lines changed

‎README.md‎

Lines changed: 30 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -1,20 +1,10 @@
1-
# Teleop Demo MuJoCo
1+
# Teleop Demo Python
22

3-
Teleoperation demo with MuJoCo and UR5 robot.
3+
Pico teleoperation demo written in python for both mujoco simulation and robot hardware.
44

55
## Overview
66

7-
This project provides a framework for controlling robots in MuJoCo simulation through XR (VR/AR) input devices. It allows users to manipulate robot arms using natural hand movements captured through XR controllers.
8-
9-
The current implementation focuses on a UR5e robot arm, providing real-time control through position-based teleoperation.
10-
11-
## Features
12-
13-
- Real-time teleoperation of robot arms in MuJoCo physics simulation.
14-
- Intuitive control using XR controllers.
15-
- Coordinate frame transformations between XR and robot spaces.
16-
- Support for Universal Robots UR5e (expandable to other robots).
17-
- Inverse kinematics solving using Placo.
7+
This project provides a framework for controlling robots in robot hardware and MuJoCo simulation through XR (VR/AR) input devices. It allows users to manipulate robot arms using natural hand movements captured through XR controllers.
188

199
## Dependencies
2010

@@ -54,11 +44,34 @@ The main dependencies are listed in the [`pyproject.toml`](pyproject.toml) file
5444

5545
## Usage
5646

57-
### Running the Demo
47+
### Running the MuJoCo Simulation Demo
5848

59-
To run the teleoperation demo with a UR5e robot:
49+
To run the teleoperation demo with a UR5e robot in MuJoCo simulation:
6050

6151
```bash
62-
python scripts/ur5e_dual_arm_demo.py
52+
python scripts/teleop_dual_ur5e_mujoco.py
6353
```
64-
This script initializes the [`MujocoTeleopController`](teleop_demo_mujoco/mujoco_teleop_controller.py) with the UR5e model and starts the teleoperation loop.
54+
This script initializes the [`MujocoTeleopController`](teleop_demo_python/mujoco_teleop_controller.py) with the UR5e model and starts the teleoperation loop.
55+
56+
### Running the Hardware Demo (Dual UR Arms and Dynamixel Head)
57+
58+
To run the teleoperation demo with the physical dual UR arms and Dynamixel-based head:
59+
60+
1. **Normal Operation:**
61+
```bash
62+
python scripts/teleop_dual_ur5e_hardware.py
63+
```
64+
This script initializes the [`DynamixelHeadController`](teleop_demo_python/hardware/dynamixel.py) and [`DualArmURController`](teleop_demo_python/hardware/ur.py) and starts the teleoperation loops for both head tracking and arm control.
65+
66+
2. **Resetting Arm Positions:**
67+
If you need to reset the UR arms to their initial/home positions and initialize the robotiq grippers, you can run the script with the `--reset` flag:
68+
```bash
69+
python scripts/teleop_dual_ur5e_hardware.py --reset
70+
```
71+
This will execute the reset procedure defined in the [`DualArmURController`](teleop_demo_python/hardware/ur.py) and then exit.
72+
73+
3. **Visualizing IK results:**
74+
To visualize the inverse kinematics solution with placo during teleoperation, run the script with the `--visualize_placo` flag.
75+
```bash
76+
python scripts/teleop_dual_ur5e_hardware.py --visualize_placo
77+
```

‎assets/universal_robots_ur5e/dual_ur5e.urdf‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@
1111
<joint name="world_to_right_arm_base" type="fixed">
1212
<parent link="base_link"/>
1313
<child link="right_base_link"/>
14-
<origin xyz="0 -0.275 0.3" rpy="0 -0.7853981633974483 1.5707963267948966"/>
14+
<origin xyz="0 -0.275 0.3" rpy="0.7853981633974483 0 0"/>
1515
</joint>
1616

1717
<transmission name="right_shoulder_pan_trans">
@@ -307,7 +307,7 @@
307307
<joint name="world_to_left_arm_base" type="fixed">
308308
<parent link="base_link"/>
309309
<child link="left_base_link"/>
310-
<origin xyz="0 0.275 0.3" rpy="0 -0.7853981633974483 -1.5707963267948966"/>
310+
<origin xyz="0 0.275 0.3" rpy="0.7853981633974483 0 3.141592653589793"/>
311311
</joint>
312312

313313
<transmission name="left_shoulder_pan_trans">

‎assets/universal_robots_ur5e/dual_ur5e.xml‎

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -69,7 +69,7 @@
6969
</body>
7070

7171
<!-- Left Arm -->
72-
<body name="left_robot_base" pos="0 0.275 0.3" quat="0.65328148 -0.27059805 -0.27059805 -0.65328148" childclass="ur5e">
72+
<body name="left_robot_base" pos="0 0.275 0.3" quat="0 0 0.3826834 0.9238795" childclass="ur5e">
7373
<inertial mass="4" pos="0 0 0" diaginertia="0.00443333156 0.00443333156 0.0072"/>
7474
<geom mesh="base_0" material="black" class="visual"/>
7575
<geom mesh="base_1" material="jointgray" class="visual"/>
@@ -133,7 +133,7 @@
133133
</body>
134134
</body>
135135
<!-- Right Arm -->
136-
<body name="right_robot_base" pos="0 -0.275 0.3" quat="0.65328148 0.27059805 -0.27059805 0.65328148" childclass="ur5e">
136+
<body name="right_robot_base" pos="0 -0.275 0.3" quat="0.92387953 0.38268343 0. 0." childclass="ur5e">
137137
<inertial mass="4" pos="0 0 0" diaginertia="0.00443333156 0.00443333156 0.0072"/>
138138
<geom mesh="base_0" material="black" class="visual"/>
139139
<geom mesh="base_1" material="jointgray" class="visual"/>
@@ -219,7 +219,10 @@
219219

220220
<keyframe>
221221
<key name="home"
222-
qpos="1.5708 -1.0472 1.5708 -1.5708 -1.5708 0 1.5708 -2.0944 -1.5708 -1.5708 1.5708 0"
223-
ctrl="1.5708 -1.0472 1.5708 -1.5708 -1.5708 0 1.5708 -2.0944 -1.5708 -1.5708 1.5708 0"/>
222+
qpos="2.88433112 -0.82903139 2.07572008 -0.67998028 1.52733763 2.61031443 3.3777357 -2.86530703 -1.99002441 1.0124655 1.77796691 -2.41553568"
223+
ctrl="2.88433112 -0.82903139 2.07572008 -0.67998028 1.52733763 2.61031443 3.3777357 -2.86530703 -1.99002441 1.0124655 1.77796691 -2.41553568"/>
224+
<key name="zero"
225+
qpos="0 0 0 0 0 0 0 0 0 0 0 0"
226+
ctrl="0 0 0 0 0 0 0 0 0 0 0 0"/>
224227
</keyframe>
225228
</mujoco>

‎pyproject.toml‎

Lines changed: 1 addition & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@ name = "teleop_demo_python"
77
version = "0.1.0"
88
description = "Teleoperation demo with MuJoCo and UR5 robot"
99
readme = "README.md"
10-
requires-python = ">=3.7"
10+
requires-python = ">=3.10"
1111
license = {text = "MIT"}
1212
authors = [
1313
{name = "Zhigen", email = "[email protected]"}
@@ -34,10 +34,3 @@ packages = ["teleop_demo_python"]
3434

3535
[tool.setuptools.package-data]
3636
teleop_demo_python = ["../assets/**/*"]
37-
38-
[tool.black]
39-
line-length = 88
40-
41-
[tool.isort]
42-
profile = "black"
43-
line_length = 88

‎scripts/dual_arm_control.py‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
from teleop_demo_python.hardware import DualArmURController
2+
from teleop_demo_python.utils.pico_client import PicoClient
3+
4+
5+
def main():
6+
pico_client = PicoClient()
7+
controller = DualArmURController(pico_client)
8+
controller.run()
9+
10+
11+
if __name__ == "__main__":
12+
main()

‎scripts/head_control.py‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
import threading
2+
import time
3+
4+
from teleop_demo_python.hardware import DynamixelHeadController
5+
from teleop_demo_python.utils.pico_client import PicoClient
6+
7+
8+
def main():
9+
pico_client = PicoClient()
10+
11+
controller = DynamixelHeadController(pico_client)
12+
13+
stop_signal = threading.Event()
14+
15+
control_thread = threading.Thread(
16+
target=controller.run_thread,
17+
args=(stop_signal,),
18+
)
19+
control_thread.start()
20+
21+
while True:
22+
try:
23+
time.sleep(0.01)
24+
except KeyboardInterrupt:
25+
print("Stopping head control...")
26+
stop_signal.set()
27+
control_thread.join()
28+
break
29+
30+
31+
if __name__ == "__main__":
32+
main()

‎scripts/teleop_dual_ur5e_hardware.py‎

Lines changed: 82 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
import argparse
2+
import threading
3+
import time
4+
5+
from teleop_demo_python.hardware import (
6+
DualArmURController,
7+
DynamixelHeadController,
8+
)
9+
from teleop_demo_python.utils.pico_client import PicoClient
10+
11+
12+
def main():
13+
parser = argparse.ArgumentParser(description="Run head and dual arm teleoperation control.")
14+
parser.add_argument(
15+
"--reset",
16+
action="store_true",
17+
help="Run the reset procedure for the dual arm controller.",
18+
)
19+
parser.add_argument(
20+
"--visualize_placo", action="store_true", help="Visualize Placo in the arm controller."
21+
)
22+
23+
args = parser.parse_args()
24+
25+
pico_client = PicoClient()
26+
head_controller = DynamixelHeadController(pico_client)
27+
arm_controller = DualArmURController(pico_client, visualize_placo=args.visualize_placo)
28+
29+
if args.reset:
30+
print("Reset flag detected. Running arm controller reset procedure...")
31+
try:
32+
arm_controller.reset()
33+
print("Arm controller reset procedure completed.")
34+
except Exception as e:
35+
print(f"Error during arm_controller.reset(): {e}")
36+
else:
37+
print("No reset flag detected. Proceeding with normal operation.")
38+
arm_controller.calc_target_joint_position()
39+
40+
stop_signal = threading.Event()
41+
head_thread = threading.Thread(
42+
target=head_controller.run_thread,
43+
args=(stop_signal,),
44+
)
45+
left_arm_thread = threading.Thread(
46+
target=arm_controller.run_left_controller_thread,
47+
args=(stop_signal,),
48+
)
49+
right_arm_thread = threading.Thread(
50+
target=arm_controller.run_right_controller_thread,
51+
args=(stop_signal,),
52+
)
53+
ik_thread = threading.Thread(
54+
target=arm_controller.run_ik_thread,
55+
args=(stop_signal,),
56+
)
57+
58+
# Start the threads
59+
head_thread.start()
60+
left_arm_thread.start()
61+
right_arm_thread.start()
62+
ik_thread.start()
63+
64+
while not stop_signal.is_set():
65+
try:
66+
time.sleep(0.01)
67+
except KeyboardInterrupt:
68+
print("KeyboardInterrupt detected. Exiting...")
69+
stop_signal.set() # Trigger the stop signal for all threads
70+
71+
head_thread.join()
72+
left_arm_thread.join()
73+
right_arm_thread.join()
74+
ik_thread.join()
75+
76+
head_controller.close()
77+
arm_controller.close()
78+
print("All controllers have been stopped and disconnected.")
79+
80+
81+
if __name__ == "__main__":
82+
main()

scripts/ur5e_dual_arm_demo.py renamed to scripts/teleop_dual_ur5e_mujoco.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,7 @@ def main():
3434
robot_urdf_path=robot_urdf_path,
3535
end_effector_config=config,
3636
scale_factor=1.5,
37-
# visualize_placo=True,
37+
visualize_placo=True,
3838
)
3939

4040
controller.run()

‎scripts/vis_mjcf.py‎

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,5 @@
1-
from typing import Optional
2-
import typer
31
import mujoco
42
from mujoco import viewer as mj_viewer
5-
import numpy as np
63

74

85
def main():

‎scripts/vis_urdf.py‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,13 +6,28 @@
66
robot_viz,
77
robot_frame_viz,
88
)
9+
import numpy as np
10+
11+
from teleop_demo_python.hardware.ur import (
12+
LEFT_INITIAL_JOINT,
13+
RIGHT_INITIAL_JOINT,
14+
)
915

1016

1117
def main():
1218
urdf_path = "assets/universal_robots_ur5e/dual_ur5e.urdf"
1319
robot = placo.RobotWrapper(urdf_path)
20+
robot.state.q[7:] = np.concatenate(
21+
(
22+
LEFT_INITIAL_JOINT,
23+
RIGHT_INITIAL_JOINT,
24+
)
25+
) # Set initial joint positions for both arms
1426
robot.update_kinematics()
1527

28+
print(f"left initial joint: {LEFT_INITIAL_JOINT}")
29+
print(f"right initial joint: {RIGHT_INITIAL_JOINT}")
30+
1631
viz = robot_viz(robot)
1732
while True:
1833
viz.display(robot.state.q)

0 commit comments

Comments
 (0)