Skip to content

Commit bddc17a

Browse files
author
yangning wu
committed
add unitree_hg
1 parent f5bfa08 commit bddc17a

34 files changed

Lines changed: 860 additions & 21 deletions

‎example/hg/arm_sdk.py‎

Lines changed: 133 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,133 @@
1+
import time
2+
import numpy as np
3+
from enum import IntEnum
4+
from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
5+
6+
# from user_data import *
7+
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_
8+
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import MotorCmd_
9+
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
10+
import unitree_sdk2py.idl.unitree_hg.msg.dds_ as dds
11+
print(dds.LowCmd_)
12+
print(dds.MotorCmd_)
13+
14+
kTopicArmSDK = "rt/arm_sdk"
15+
kPi = 3.141592654
16+
kPi_2 = 1.57079632
17+
18+
19+
class JointIndex(IntEnum):
20+
21+
# // Left arm
22+
kLeftShoulderPitch = 13
23+
kLeftShoulderRoll = 14
24+
kLeftShoulderYaw = 15
25+
kLeftElbow = 16
26+
kLeftWistYaw = 17
27+
kLeftWistPitch = 18
28+
kLeftWistRoll = 19
29+
# // Right arm
30+
kRightShoulderPitch = 20
31+
kRightShoulderRoll = 21
32+
kRightShoulderYaw = 22
33+
kRightElbow = 23
34+
kRightWistYaw = 24
35+
kRightWistPitch = 25
36+
kRightWistRoll = 26
37+
38+
kWaistYaw = 12
39+
40+
41+
kNotUsedJoint = 27
42+
43+
44+
if __name__ == "__main__":
45+
ChannelFactoryInitialize()
46+
47+
# Create a publisher to publish the data defined in UserData class
48+
arm_sdk_publisher = ChannelPublisher('rt/arm_sdk', LowCmd_)
49+
# pub = ChannelPublisher("rt/lowcmd", LowCmd_)
50+
arm_sdk_publisher.Init()
51+
52+
msg = unitree_hg_msg_dds__LowCmd_()
53+
54+
weight = 0
55+
weight_rate = 0.2
56+
57+
kp = 60
58+
kd = 1.5
59+
dq = 0
60+
tau_ff = 0
61+
62+
control_dt = 0.02
63+
max_joint_velocity = 0.5
64+
65+
delta_weight = weight_rate * control_dt
66+
max_joint_delta = max_joint_velocity * control_dt
67+
68+
init_pos = np.zeros(15)
69+
target_pos = np.array(
70+
[
71+
0.0,
72+
kPi_2,
73+
0.0,
74+
kPi_2,
75+
0.0,
76+
0.0,
77+
0.0,
78+
0.0,
79+
-kPi_2,
80+
0.0,
81+
kPi_2,
82+
0.0,
83+
0.0,
84+
0.0,
85+
0.0,
86+
]
87+
)
88+
print("Initailizing arms ...")
89+
90+
init_time = 5
91+
init_time_steps = int(init_time / control_dt)
92+
93+
for i in range(init_time_steps):
94+
weight += delta_weight
95+
weight = max(min(weight, 1.0), 0)
96+
msg.motor_cmd[kNotUsedJoint].q = weight * weight
97+
98+
for idx, joint in enumerate(JointIndex):
99+
msg.motor_cmd[joint].q = init_pos[idx]
100+
msg.motor_cmd[joint].dq = dq
101+
msg.motor_cmd[joint].kp = kp
102+
msg.motor_cmd[joint].kd = kd
103+
msg.motor_cmd[joint].tau = tau_ff
104+
105+
arm_sdk_publisher.Write(msg)
106+
107+
time.sleep(control_dt)
108+
109+
print("Done!")
110+
111+
period = 5
112+
num_time_steps = int(period / control_dt)
113+
114+
current_jpos_des = np.zeros_like(init_pos)
115+
116+
for i in range(num_time_steps):
117+
for j in range(len(current_jpos_des)):
118+
delta = target_pos[j] - current_jpos_des[j]
119+
clamped_delta = np.clip(delta, -max_joint_delta, max_joint_delta)
120+
current_jpos_des[j] += clamped_delta
121+
122+
for idx, joint in enumerate(JointIndex):
123+
msg.motor_cmd[joint].q = current_jpos_des[idx]
124+
msg.motor_cmd[joint].dq = dq
125+
msg.motor_cmd[joint].kp = kp
126+
msg.motor_cmd[joint].kd = kd
127+
msg.motor_cmd[joint].tau = tau_ff
128+
129+
arm_sdk_publisher.Write(msg)
130+
131+
time.sleep(control_dt)
132+
133+
exit()

‎example/hg/low_cmd.py‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
import time
2+
import numpy as np
3+
from enum import IntEnum
4+
from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
5+
import sys
6+
7+
# from user_data import *
8+
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_
9+
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
10+
import unitree_sdk2py.idl.unitree_hg.msg.dds_ as dds
11+
from unitree_sdk2py.utils.crc import CRC
12+
13+
14+
kTopicArmSDK = "rt/lowcmd"
15+
kPi = 3.141592654
16+
kPi_2 = 1.57079632
17+
kNumMotors = 35
18+
19+
if __name__ == "__main__":
20+
21+
if len(sys.argv)>1:
22+
ChannelFactoryInitialize(0, sys.argv[1])
23+
else:
24+
ChannelFactoryInitialize(0)
25+
26+
# 初始化发布节点
27+
low_cmd_publisher = ChannelPublisher(kTopicArmSDK, LowCmd_)
28+
low_cmd_publisher.Init()
29+
# 初始化LowCmd_
30+
msg = unitree_hg_msg_dds__LowCmd_()
31+
# CRC校验实例
32+
crc = CRC()
33+
# 控制时间参数
34+
control_dt = 0.02
35+
init_time = 20
36+
init_time_steps = int(init_time / control_dt)
37+
38+
# 控制参数设置
39+
msg.mode_pr = 0
40+
msg.mode_machine = 4
41+
for i in range(init_time_steps):
42+
for idx in range(kNumMotors):
43+
msg.motor_cmd[idx].mode = 0x01
44+
msg.crc = crc.Crc(msg)
45+
# print(msg.crc)
46+
low_cmd_publisher.Write(msg)
47+
time.sleep(control_dt)
48+
49+
print("Done!")
50+
exit()

‎example/hg/readme.md‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
This example is a test of Unitree G1/H1-2 robot.
2+
3+
**Note:**
4+
idl/unitree_go is used for Unitree Go2/B2/H1/B2w/Go2w robots
5+
idl/unitree_hg is used for Unitree G1/H1-2 robots

‎setup.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
author='Unitree',
66
author_email='unitree@unitree.com',
77
license="BSD-3-Clause",
8-
packages=find_packages(),
8+
packages=find_packages(include=['unitree_sdk2py','unitree_sdk2py.*']),
99
description='Unitree robot sdk version 2 for python',
1010
python_requires='>=3.8',
1111
install_requires=[

‎unitree_sdk2py/core/__init__.py‎

Whitespace-only changes.

‎unitree_sdk2py/go2/__init__.py‎

Whitespace-only changes.

‎unitree_sdk2py/go2/obstacles_avoid/__init__.py‎

Whitespace-only changes.

‎unitree_sdk2py/go2/robot_state/__init__.py‎

Whitespace-only changes.

‎unitree_sdk2py/go2/sport/__init__.py‎

Whitespace-only changes.

‎unitree_sdk2py/go2/video/__init__.py‎

Whitespace-only changes.

0 commit comments

Comments
 (0)