Skip to content

Commit 965cb64

Browse files
zhigenzhaozhigenzhao
committed
added hardware dual arm control
1 parent 410a185 commit 965cb64

File tree

9 files changed

+833
-14
lines changed

9 files changed

+833
-14
lines changed

‎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>

‎scripts/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.ur 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/mujoco_dual_arm_demo.py‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
import os
2+
from teleop_demo_python.mujoco_teleop_controller import (
3+
MujocoTeleopController,
4+
)
5+
from teleop_demo_python.utils.path_utils import ASSET_PATH
6+
7+
8+
def main():
9+
xml_path = os.path.join(
10+
ASSET_PATH, "universal_robots_ur5e/scene_dual_arm.xml"
11+
)
12+
robot_urdf_path = os.path.join(
13+
ASSET_PATH, "universal_robots_ur5e/dual_ur5e.urdf"
14+
)
15+
16+
config = {
17+
"right_hand": {
18+
"link_name": "right_tool0",
19+
"pose_source": "right_controller",
20+
"control_trigger": "right_grip",
21+
"vis_target": "right_target",
22+
},
23+
"left_hand": {
24+
"link_name": "left_tool0",
25+
"pose_source": "left_controller",
26+
"control_trigger": "left_grip",
27+
"vis_target": "left_target",
28+
},
29+
}
30+
31+
# Create and initialize the teleoperation controller
32+
controller = MujocoTeleopController(
33+
xml_path=xml_path,
34+
robot_urdf_path=robot_urdf_path,
35+
end_effector_config=config,
36+
scale_factor=1.5,
37+
visualize_placo=True,
38+
)
39+
40+
controller.run()
41+
42+
43+
if __name__ == "__main__":
44+
main()

‎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)