-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathrun_check.py
210 lines (177 loc) · 7.42 KB
/
run_check.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
# This is the unitree python sdk's original example
# Added here as a sanity check
# The original file can be found here:
# https://github.com/unitreerobotics/unitree_sdk2_python/blob/master/example/g1/low_level/g1_low_level_example.py
import time
import sys
from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_
from unitree_sdk2py.utils.crc import CRC
from unitree_sdk2py.utils.thread import RecurrentThread
from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
import numpy as np
G1_NUM_MOTOR = 29
Kp = [
60, 60, 60, 100, 40, 40, # legs
60, 60, 60, 100, 40, 40, # legs
60, 40, 40, # waist
40, 40, 40, 40, 40, 40, 40, # arms
40, 40, 40, 40, 40, 40, 40 # arms
]
Kd = [
1, 1, 1, 2, 1, 1, # legs
1, 1, 1, 2, 1, 1, # legs
1, 1, 1, # waist
1, 1, 1, 1, 1, 1, 1, # arms
1, 1, 1, 1, 1, 1, 1 # arms
]
class G1JointIndex:
LeftHipPitch = 0
LeftHipRoll = 1
LeftHipYaw = 2
LeftKnee = 3
LeftAnklePitch = 4
LeftAnkleB = 4
LeftAnkleRoll = 5
LeftAnkleA = 5
RightHipPitch = 6
RightHipRoll = 7
RightHipYaw = 8
RightKnee = 9
RightAnklePitch = 10
RightAnkleB = 10
RightAnkleRoll = 11
RightAnkleA = 11
WaistYaw = 12
WaistRoll = 13 # NOTE: INVALID for g1 23dof/29dof with waist locked
WaistA = 13 # NOTE: INVALID for g1 23dof/29dof with waist locked
WaistPitch = 14 # NOTE: INVALID for g1 23dof/29dof with waist locked
WaistB = 14 # NOTE: INVALID for g1 23dof/29dof with waist locked
LeftShoulderPitch = 15
LeftShoulderRoll = 16
LeftShoulderYaw = 17
LeftElbow = 18
LeftWristRoll = 19
LeftWristPitch = 20 # NOTE: INVALID for g1 23dof
LeftWristYaw = 21 # NOTE: INVALID for g1 23dof
RightShoulderPitch = 22
RightShoulderRoll = 23
RightShoulderYaw = 24
RightElbow = 25
RightWristRoll = 26
RightWristPitch = 27 # NOTE: INVALID for g1 23dof
RightWristYaw = 28 # NOTE: INVALID for g1 23dof
class Mode:
PR = 0 # Series Control for Pitch/Roll Joints
AB = 1 # Parallel Control for A/B Joints
class Custom:
def __init__(self):
self.time_ = 0.0
self.control_dt_ = 0.002 # [2ms]
self.duration_ = 3.0 # [3 s]
self.counter_ = 0
self.mode_pr_ = Mode.PR
self.mode_machine_ = 0
self.low_cmd = unitree_hg_msg_dds__LowCmd_()
self.low_state = None
self.update_mode_machine_ = False
self.crc = CRC()
def Init(self):
self.msc = MotionSwitcherClient()
self.msc.SetTimeout(5.0)
self.msc.Init()
status, result = self.msc.CheckMode()
while result['name']:
self.msc.ReleaseMode()
status, result = self.msc.CheckMode()
time.sleep(1)
# create publisher #
self.lowcmd_publisher_ = ChannelPublisher("rt/lowcmd", LowCmd_)
self.lowcmd_publisher_.Init()
# create subscriber #
self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_)
self.lowstate_subscriber.Init(self.LowStateHandler, 10)
def Start(self):
self.lowCmdWriteThreadPtr = RecurrentThread(
interval=self.control_dt_, target=self.LowCmdWrite, name="control"
)
while self.update_mode_machine_ == False:
time.sleep(1)
if self.update_mode_machine_ == True:
self.lowCmdWriteThreadPtr.Start()
def LowStateHandler(self, msg: LowState_):
self.low_state = msg
if self.update_mode_machine_ == False:
self.mode_machine_ = self.low_state.mode_machine
self.update_mode_machine_ = True
self.counter_ +=1
if (self.counter_ % 500 == 0) :
self.counter_ = 0
print(self.low_state.imu_state.rpy)
def LowCmdWrite(self):
self.time_ += self.control_dt_
if self.time_ < self.duration_ :
# [Stage 1]: set robot to zero posture
for i in range(G1_NUM_MOTOR):
ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0)
self.low_cmd.mode_pr = Mode.PR
self.low_cmd.mode_machine = self.mode_machine_
self.low_cmd.motor_cmd[i].mode = 1 # 1:Enable, 0:Disable
self.low_cmd.motor_cmd[i].tau = 0.
self.low_cmd.motor_cmd[i].q = (1.0 - ratio) * self.low_state.motor_state[i].q
self.low_cmd.motor_cmd[i].dq = 0.
self.low_cmd.motor_cmd[i].kp = Kp[i]
self.low_cmd.motor_cmd[i].kd = Kd[i]
elif self.time_ < self.duration_ * 2 :
# [Stage 2]: swing ankle using PR mode
max_P = np.pi * 30.0 / 180.0
max_R = np.pi * 10.0 / 180.0
t = self.time_ - self.duration_
L_P_des = max_P * np.sin(2.0 * np.pi * t)
L_R_des = max_R * np.sin(2.0 * np.pi * t)
R_P_des = max_P * np.sin(2.0 * np.pi * t)
R_R_des = -max_R * np.sin(2.0 * np.pi * t)
self.low_cmd.mode_pr = Mode.PR
self.low_cmd.mode_machine = self.mode_machine_
self.low_cmd.motor_cmd[G1JointIndex.LeftAnklePitch].q = L_P_des
self.low_cmd.motor_cmd[G1JointIndex.LeftAnkleRoll].q = L_R_des
self.low_cmd.motor_cmd[G1JointIndex.RightAnklePitch].q = R_P_des
self.low_cmd.motor_cmd[G1JointIndex.RightAnkleRoll].q = R_R_des
else :
# [Stage 3]: swing ankle using AB mode
max_A = np.pi * 30.0 / 180.0
max_B = np.pi * 10.0 / 180.0
t = self.time_ - self.duration_ * 2
L_A_des = max_A * np.sin(2.0 * np.pi * t)
L_B_des = max_B * np.sin(2.0 * np.pi * t + np.pi)
R_A_des = -max_A * np.sin(2.0 * np.pi * t)
R_B_des = -max_B * np.sin(2.0 * np.pi * t + np.pi)
self.low_cmd.mode_pr = Mode.AB
self.low_cmd.mode_machine = self.mode_machine_
self.low_cmd.motor_cmd[G1JointIndex.LeftAnkleA].q = L_A_des
self.low_cmd.motor_cmd[G1JointIndex.LeftAnkleB].q = L_B_des
self.low_cmd.motor_cmd[G1JointIndex.RightAnkleA].q = R_A_des
self.low_cmd.motor_cmd[G1JointIndex.RightAnkleB].q = R_B_des
max_WristYaw = np.pi * 30.0 / 180.0
L_WristYaw_des = max_WristYaw * np.sin(2.0 * np.pi * t)
R_WristYaw_des = max_WristYaw * np.sin(2.0 * np.pi * t)
self.low_cmd.motor_cmd[G1JointIndex.LeftWristRoll].q = L_WristYaw_des
self.low_cmd.motor_cmd[G1JointIndex.RightWristRoll].q = R_WristYaw_des
self.low_cmd.crc = self.crc.Crc(self.low_cmd)
self.lowcmd_publisher_.Write(self.low_cmd)
if __name__ == '__main__':
print("WARNING: Please ensure there are no obstacles around the robot while running this example.")
input("Press Enter to continue...")
if len(sys.argv)>1:
ChannelFactoryInitialize(0, sys.argv[1])
else:
ChannelFactoryInitialize(0)
custom = Custom()
custom.Init()
custom.Start()
while True:
time.sleep(1)