Skip to content

Commit 3af1610

Browse files
xiaoliangstdxiaoliangstd
committed
update g1 arm_sdk examples
1 parent f17114c commit 3af1610

2 files changed

Lines changed: 386 additions & 0 deletions

File tree

‎example/g1/high_level/g1_arm5_sdk_dds_example.py‎

Lines changed: 192 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,192 @@
1+
import time
2+
import sys
3+
4+
from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
5+
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
6+
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
7+
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_
8+
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_
9+
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_
10+
from unitree_sdk2py.utils.crc import CRC
11+
from unitree_sdk2py.utils.thread import RecurrentThread
12+
from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
13+
14+
import numpy as np
15+
16+
kPi = 3.141592654
17+
kPi_2 = 1.57079632
18+
19+
class G1JointIndex:
20+
# Left leg
21+
LeftHipPitch = 0
22+
LeftHipRoll = 1
23+
LeftHipYaw = 2
24+
LeftKnee = 3
25+
LeftAnklePitch = 4
26+
LeftAnkleB = 4
27+
LeftAnkleRoll = 5
28+
LeftAnkleA = 5
29+
30+
# Right leg
31+
RightHipPitch = 6
32+
RightHipRoll = 7
33+
RightHipYaw = 8
34+
RightKnee = 9
35+
RightAnklePitch = 10
36+
RightAnkleB = 10
37+
RightAnkleRoll = 11
38+
RightAnkleA = 11
39+
40+
WaistYaw = 12
41+
WaistRoll = 13 # NOTE: INVALID for g1 23dof/29dof with waist locked
42+
WaistA = 13 # NOTE: INVALID for g1 23dof/29dof with waist locked
43+
WaistPitch = 14 # NOTE: INVALID for g1 23dof/29dof with waist locked
44+
WaistB = 14 # NOTE: INVALID for g1 23dof/29dof with waist locked
45+
46+
# Left arm
47+
LeftShoulderPitch = 15
48+
LeftShoulderRoll = 16
49+
LeftShoulderYaw = 17
50+
LeftElbow = 18
51+
LeftWristRoll = 19
52+
LeftWristPitch = 20 # NOTE: INVALID for g1 23dof
53+
LeftWristYaw = 21 # NOTE: INVALID for g1 23dof
54+
55+
# Right arm
56+
RightShoulderPitch = 22
57+
RightShoulderRoll = 23
58+
RightShoulderYaw = 24
59+
RightElbow = 25
60+
RightWristRoll = 26
61+
RightWristPitch = 27 # NOTE: INVALID for g1 23dof
62+
RightWristYaw = 28 # NOTE: INVALID for g1 23dof
63+
64+
kNotUsedJoint = 29 # NOTE: Weight
65+
66+
class Custom:
67+
def __init__(self):
68+
self.time_ = 0.0
69+
self.control_dt_ = 0.02
70+
self.duration_ = 3.0
71+
self.counter_ = 0
72+
self.weight = 0.
73+
self.weight_rate = 0.2
74+
self.kp = 60.
75+
self.kd = 1.5
76+
self.dq = 0.
77+
self.tau_ff = 0.
78+
self.mode_machine_ = 0
79+
self.low_cmd = unitree_hg_msg_dds__LowCmd_()
80+
self.low_state = None
81+
self.first_update_low_state = False
82+
self.crc = CRC()
83+
self.done = False
84+
85+
self.target_pos = [
86+
0.0, kPi_2, 0.0, kPi_2, 0.0,
87+
0.0, -kPi_2, 0.0, kPi_2, 0.0,
88+
0.0, 0.0, 0.0
89+
]
90+
91+
self.arm_joints = [
92+
G1JointIndex.LeftShoulderPitch, G1JointIndex.LeftShoulderRoll,
93+
G1JointIndex.LeftShoulderYaw, G1JointIndex.LeftElbow,
94+
G1JointIndex.LeftWristRoll,
95+
G1JointIndex.RightShoulderPitch, G1JointIndex.RightShoulderRoll,
96+
G1JointIndex.RightShoulderYaw, G1JointIndex.RightElbow,
97+
G1JointIndex.RightWristRoll,
98+
G1JointIndex.WaistYaw,
99+
G1JointIndex.WaistRoll,
100+
G1JointIndex.WaistPitch
101+
]
102+
103+
def Init(self):
104+
# create publisher #
105+
self.arm_sdk_publisher = ChannelPublisher("rt/arm_sdk", LowCmd_)
106+
self.arm_sdk_publisher.Init()
107+
108+
# create subscriber #
109+
self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_)
110+
self.lowstate_subscriber.Init(self.LowStateHandler, 10)
111+
112+
def Start(self):
113+
self.lowCmdWriteThreadPtr = RecurrentThread(
114+
interval=self.control_dt_, target=self.LowCmdWrite, name="control"
115+
)
116+
while self.first_update_low_state == False:
117+
time.sleep(1)
118+
119+
if self.first_update_low_state == True:
120+
self.lowCmdWriteThreadPtr.Start()
121+
122+
def LowStateHandler(self, msg: LowState_):
123+
self.low_state = msg
124+
125+
if self.first_update_low_state == False:
126+
self.first_update_low_state = True
127+
128+
def LowCmdWrite(self):
129+
self.time_ += self.control_dt_
130+
131+
if self.time_ < self.duration_ :
132+
# [Stage 1]: set robot to zero posture
133+
self.low_cmd.motor_cmd[G1JointIndex.kNotUsedJoint].q = 1 # 1:Enable arm_sdk, 0:Disable arm_sdk
134+
for i,joint in enumerate(self.arm_joints):
135+
ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0)
136+
self.low_cmd.motor_cmd[joint].tau = 0.
137+
self.low_cmd.motor_cmd[joint].q = (1.0 - ratio) * self.low_state.motor_state[joint].q
138+
self.low_cmd.motor_cmd[joint].dq = 0.
139+
self.low_cmd.motor_cmd[joint].kp = self.kp
140+
self.low_cmd.motor_cmd[joint].kd = self.kd
141+
142+
elif self.time_ < self.duration_ * 3 :
143+
# [Stage 2]: lift arms up
144+
for i,joint in enumerate(self.arm_joints):
145+
ratio = np.clip((self.time_ - self.duration_) / (self.duration_ * 2), 0.0, 1.0)
146+
self.low_cmd.motor_cmd[joint].tau = 0.
147+
self.low_cmd.motor_cmd[joint].q = ratio * self.target_pos[i] + (1.0 - ratio) * self.low_state.motor_state[joint].q
148+
self.low_cmd.motor_cmd[joint].dq = 0.
149+
self.low_cmd.motor_cmd[joint].kp = self.kp
150+
self.low_cmd.motor_cmd[joint].kd = self.kd
151+
152+
elif self.time_ < self.duration_ * 6 :
153+
# [Stage 3]: set robot back to zero posture
154+
for i,joint in enumerate(self.arm_joints):
155+
ratio = np.clip((self.time_ - self.duration_*3) / (self.duration_ * 3), 0.0, 1.0)
156+
self.low_cmd.motor_cmd[joint].tau = 0.
157+
self.low_cmd.motor_cmd[joint].q = (1.0 - ratio) * self.low_state.motor_state[joint].q
158+
self.low_cmd.motor_cmd[joint].dq = 0.
159+
self.low_cmd.motor_cmd[joint].kp = self.kp
160+
self.low_cmd.motor_cmd[joint].kd = self.kd
161+
162+
elif self.time_ < self.duration_ * 7 :
163+
# [Stage 4]: release arm_sdk
164+
for i,joint in enumerate(self.arm_joints):
165+
ratio = np.clip((self.time_ - self.duration_*6) / (self.duration_), 0.0, 1.0)
166+
self.low_cmd.motor_cmd[G1JointIndex.kNotUsedJoint].q = (1 - ratio) # 1:Enable arm_sdk, 0:Disable arm_sdk
167+
168+
else:
169+
self.done = True
170+
171+
self.low_cmd.crc = self.crc.Crc(self.low_cmd)
172+
self.arm_sdk_publisher.Write(self.low_cmd)
173+
174+
if __name__ == '__main__':
175+
176+
print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
177+
input("Press Enter to continue...")
178+
179+
if len(sys.argv)>1:
180+
ChannelFactoryInitialize(0, sys.argv[1])
181+
else:
182+
ChannelFactoryInitialize(0)
183+
184+
custom = Custom()
185+
custom.Init()
186+
custom.Start()
187+
188+
while True:
189+
time.sleep(1)
190+
if custom.done:
191+
print("Done!")
192+
sys.exit(-1)

‎example/g1/high_level/g1_arm7_sdk_dds_example.py‎

Lines changed: 194 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,194 @@
1+
import time
2+
import sys
3+
4+
from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
5+
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
6+
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
7+
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_
8+
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_
9+
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_
10+
from unitree_sdk2py.utils.crc import CRC
11+
from unitree_sdk2py.utils.thread import RecurrentThread
12+
from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
13+
14+
import numpy as np
15+
16+
kPi = 3.141592654
17+
kPi_2 = 1.57079632
18+
19+
class G1JointIndex:
20+
# Left leg
21+
LeftHipPitch = 0
22+
LeftHipRoll = 1
23+
LeftHipYaw = 2
24+
LeftKnee = 3
25+
LeftAnklePitch = 4
26+
LeftAnkleB = 4
27+
LeftAnkleRoll = 5
28+
LeftAnkleA = 5
29+
30+
# Right leg
31+
RightHipPitch = 6
32+
RightHipRoll = 7
33+
RightHipYaw = 8
34+
RightKnee = 9
35+
RightAnklePitch = 10
36+
RightAnkleB = 10
37+
RightAnkleRoll = 11
38+
RightAnkleA = 11
39+
40+
WaistYaw = 12
41+
WaistRoll = 13 # NOTE: INVALID for g1 23dof/29dof with waist locked
42+
WaistA = 13 # NOTE: INVALID for g1 23dof/29dof with waist locked
43+
WaistPitch = 14 # NOTE: INVALID for g1 23dof/29dof with waist locked
44+
WaistB = 14 # NOTE: INVALID for g1 23dof/29dof with waist locked
45+
46+
# Left arm
47+
LeftShoulderPitch = 15
48+
LeftShoulderRoll = 16
49+
LeftShoulderYaw = 17
50+
LeftElbow = 18
51+
LeftWristRoll = 19
52+
LeftWristPitch = 20 # NOTE: INVALID for g1 23dof
53+
LeftWristYaw = 21 # NOTE: INVALID for g1 23dof
54+
55+
# Right arm
56+
RightShoulderPitch = 22
57+
RightShoulderRoll = 23
58+
RightShoulderYaw = 24
59+
RightElbow = 25
60+
RightWristRoll = 26
61+
RightWristPitch = 27 # NOTE: INVALID for g1 23dof
62+
RightWristYaw = 28 # NOTE: INVALID for g1 23dof
63+
64+
kNotUsedJoint = 29 # NOTE: Weight
65+
66+
class Custom:
67+
def __init__(self):
68+
self.time_ = 0.0
69+
self.control_dt_ = 0.02
70+
self.duration_ = 3.0
71+
self.counter_ = 0
72+
self.weight = 0.
73+
self.weight_rate = 0.2
74+
self.kp = 60.
75+
self.kd = 1.5
76+
self.dq = 0.
77+
self.tau_ff = 0.
78+
self.mode_machine_ = 0
79+
self.low_cmd = unitree_hg_msg_dds__LowCmd_()
80+
self.low_state = None
81+
self.first_update_low_state = False
82+
self.crc = CRC()
83+
self.done = False
84+
85+
self.target_pos = [
86+
0., kPi_2, 0., kPi_2, 0., 0., 0.,
87+
0., -kPi_2, 0., kPi_2, 0., 0., 0.,
88+
0, 0, 0
89+
]
90+
91+
self.arm_joints = [
92+
G1JointIndex.LeftShoulderPitch, G1JointIndex.LeftShoulderRoll,
93+
G1JointIndex.LeftShoulderYaw, G1JointIndex.LeftElbow,
94+
G1JointIndex.LeftWristRoll, G1JointIndex.LeftWristPitch,
95+
G1JointIndex.LeftWristYaw,
96+
G1JointIndex.RightShoulderPitch, G1JointIndex.RightShoulderRoll,
97+
G1JointIndex.RightShoulderYaw, G1JointIndex.RightElbow,
98+
G1JointIndex.RightWristRoll, G1JointIndex.RightWristPitch,
99+
G1JointIndex.RightWristYaw,
100+
G1JointIndex.WaistYaw,
101+
G1JointIndex.WaistRoll,
102+
G1JointIndex.WaistPitch
103+
]
104+
105+
def Init(self):
106+
# create publisher #
107+
self.arm_sdk_publisher = ChannelPublisher("rt/arm_sdk", LowCmd_)
108+
self.arm_sdk_publisher.Init()
109+
110+
# create subscriber #
111+
self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_)
112+
self.lowstate_subscriber.Init(self.LowStateHandler, 10)
113+
114+
def Start(self):
115+
self.lowCmdWriteThreadPtr = RecurrentThread(
116+
interval=self.control_dt_, target=self.LowCmdWrite, name="control"
117+
)
118+
while self.first_update_low_state == False:
119+
time.sleep(1)
120+
121+
if self.first_update_low_state == True:
122+
self.lowCmdWriteThreadPtr.Start()
123+
124+
def LowStateHandler(self, msg: LowState_):
125+
self.low_state = msg
126+
127+
if self.first_update_low_state == False:
128+
self.first_update_low_state = True
129+
130+
def LowCmdWrite(self):
131+
self.time_ += self.control_dt_
132+
133+
if self.time_ < self.duration_ :
134+
# [Stage 1]: set robot to zero posture
135+
self.low_cmd.motor_cmd[G1JointIndex.kNotUsedJoint].q = 1 # 1:Enable arm_sdk, 0:Disable arm_sdk
136+
for i,joint in enumerate(self.arm_joints):
137+
ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0)
138+
self.low_cmd.motor_cmd[joint].tau = 0.
139+
self.low_cmd.motor_cmd[joint].q = (1.0 - ratio) * self.low_state.motor_state[joint].q
140+
self.low_cmd.motor_cmd[joint].dq = 0.
141+
self.low_cmd.motor_cmd[joint].kp = self.kp
142+
self.low_cmd.motor_cmd[joint].kd = self.kd
143+
144+
elif self.time_ < self.duration_ * 3 :
145+
# [Stage 2]: lift arms up
146+
for i,joint in enumerate(self.arm_joints):
147+
ratio = np.clip((self.time_ - self.duration_) / (self.duration_ * 2), 0.0, 1.0)
148+
self.low_cmd.motor_cmd[joint].tau = 0.
149+
self.low_cmd.motor_cmd[joint].q = ratio * self.target_pos[i] + (1.0 - ratio) * self.low_state.motor_state[joint].q
150+
self.low_cmd.motor_cmd[joint].dq = 0.
151+
self.low_cmd.motor_cmd[joint].kp = self.kp
152+
self.low_cmd.motor_cmd[joint].kd = self.kd
153+
154+
elif self.time_ < self.duration_ * 6 :
155+
# [Stage 3]: set robot back to zero posture
156+
for i,joint in enumerate(self.arm_joints):
157+
ratio = np.clip((self.time_ - self.duration_*3) / (self.duration_ * 3), 0.0, 1.0)
158+
self.low_cmd.motor_cmd[joint].tau = 0.
159+
self.low_cmd.motor_cmd[joint].q = (1.0 - ratio) * self.low_state.motor_state[joint].q
160+
self.low_cmd.motor_cmd[joint].dq = 0.
161+
self.low_cmd.motor_cmd[joint].kp = self.kp
162+
self.low_cmd.motor_cmd[joint].kd = self.kd
163+
164+
elif self.time_ < self.duration_ * 7 :
165+
# [Stage 4]: release arm_sdk
166+
for i,joint in enumerate(self.arm_joints):
167+
ratio = np.clip((self.time_ - self.duration_*6) / (self.duration_), 0.0, 1.0)
168+
self.low_cmd.motor_cmd[G1JointIndex.kNotUsedJoint].q = (1 - ratio) # 1:Enable arm_sdk, 0:Disable arm_sdk
169+
170+
else:
171+
self.done = True
172+
173+
self.low_cmd.crc = self.crc.Crc(self.low_cmd)
174+
self.arm_sdk_publisher.Write(self.low_cmd)
175+
176+
if __name__ == '__main__':
177+
178+
print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
179+
input("Press Enter to continue...")
180+
181+
if len(sys.argv)>1:
182+
ChannelFactoryInitialize(0, sys.argv[1])
183+
else:
184+
ChannelFactoryInitialize(0)
185+
186+
custom = Custom()
187+
custom.Init()
188+
custom.Start()
189+
190+
while True:
191+
time.sleep(1)
192+
if custom.done:
193+
print("Done!")
194+
sys.exit(-1)

0 commit comments

Comments
 (0)