299 lines
9.3 KiB
Python
299 lines
9.3 KiB
Python
|
import json
|
|||
|
|
import time
|
|||
|
|
|
|||
|
|
# ===== 关节列表 =====
|
|||
|
|
|
|||
|
|
set_joint_list = [
|
|||
|
|
|
|||
|
|
{'joint_name': 'R_WRIST_Y', 'rad': 0.066444339857},
|
|||
|
|
{'joint_name': 'R_WRIST_P', 'rad': -20.531186157241},
|
|||
|
|
{'joint_name': 'R_ELBOW_R', 'rad': -1.340070808067},
|
|||
|
|
{'joint_name': 'R_SHOULDER_Y', 'rad': -1.952500297438},
|
|||
|
|
{'joint_name': 'R_SHOULDER_R', 'rad': -0.626298915913},
|
|||
|
|
{'joint_name': 'R_SHOULDER_P', 'rad': 0.342241935176},
|
|||
|
|
]
|
|||
|
|
|
|||
|
|
drive_joint_list = [
|
|||
|
|
|
|||
|
|
{'joint_name': 'R_WRIST_Y', 'rad': -0.011299818873},
|
|||
|
|
{'joint_name': 'R_WRIST_P', 'rad': -20.60070130641},
|
|||
|
|
{'joint_name': 'R_ELBOW_R', 'rad': -1.364414209864},
|
|||
|
|
{'joint_name': 'R_SHOULDER_Y', 'rad': -1.980264780151},
|
|||
|
|
{'joint_name': 'R_SHOULDER_R', 'rad': -0.560519997177},
|
|||
|
|
{'joint_name': 'R_SHOULDER_P', 'rad': 0.442999602707},
|
|||
|
|
]
|
|||
|
|
|
|||
|
|
mode_joint_list = [
|
|||
|
|
|
|||
|
|
{'joint_name': 'R_WRIST_Y', 'rad': 0.274796141277},
|
|||
|
|
{'joint_name': 'R_WRIST_P', 'rad': -20.944670868464},
|
|||
|
|
{'joint_name': 'R_ELBOW_R', 'rad': -1.136851577265},
|
|||
|
|
{'joint_name': 'R_SHOULDER_Y', 'rad': -1.543831108824},
|
|||
|
|
{'joint_name': 'R_SHOULDER_R', 'rad': -0.465282192444},
|
|||
|
|
{'joint_name': 'R_SHOULDER_P', 'rad': 0.415577797633},
|
|||
|
|
]
|
|||
|
|
|
|||
|
|
home_joint_list = [
|
|||
|
|
{'joint_name': 'R_WRIST_Y', 'rad': 0.031161140931},
|
|||
|
|
{'joint_name': 'R_WRIST_P', 'rad': -20.577257789382},
|
|||
|
|
{'joint_name': 'R_ELBOW_R', 'rad': -1.381942977952},
|
|||
|
|
{'joint_name': 'R_SHOULDER_Y', 'rad': -2.055649114477},
|
|||
|
|
{'joint_name': 'R_SHOULDER_R', 'rad': -0.602941780228},
|
|||
|
|
{'joint_name': 'R_SHOULDER_P', 'rad': 0.314926445602},
|
|||
|
|
]
|
|||
|
|
|
|||
|
|
start_joint_list = [
|
|||
|
|
|
|||
|
|
{'joint_name': 'R_WRIST_Y', 'rad': 0.274967005474},
|
|||
|
|
{'joint_name': 'R_WRIST_P', 'rad': -20.603120933284},
|
|||
|
|
{'joint_name': 'R_ELBOW_R', 'rad': -1.499983558976},
|
|||
|
|
{'joint_name': 'R_SHOULDER_Y', 'rad': -1.492936360109},
|
|||
|
|
{'joint_name': 'R_SHOULDER_R', 'rad': -0.720539063587},
|
|||
|
|
{'joint_name': 'R_SHOULDER_P', 'rad': 0.056068136892},
|
|||
|
|
]
|
|||
|
|
# start_joint_list = [
|
|||
|
|
#
|
|||
|
|
# {'joint_name': 'R_WRIST_Y', 'rad': 0.182186797436},
|
|||
|
|
# {'joint_name': 'R_WRIST_P', 'rad': -20.304780654951},
|
|||
|
|
# {'joint_name': 'R_ELBOW_R', 'rad': -1.535033501186},
|
|||
|
|
# {'joint_name': 'R_SHOULDER_Y', 'rad': -1.596287366447},
|
|||
|
|
# {'joint_name': 'R_SHOULDER_R', 'rad': -1.034838057889},
|
|||
|
|
# {'joint_name': 'R_SHOULDER_P', 'rad': -0.0965316264},
|
|||
|
|
# ]
|
|||
|
|
|
|||
|
|
wyy_joint_list = [
|
|||
|
|
{'joint_name': 'R_WRIST_Y', 'rad': 0.319859674658},
|
|||
|
|
{'joint_name': 'R_WRIST_P', 'rad': -20.645610112769},
|
|||
|
|
{'joint_name': 'R_ELBOW_R', 'rad': -1.129972394858},
|
|||
|
|
{'joint_name': 'R_SHOULDER_Y', 'rad': -1.724514301329},
|
|||
|
|
{'joint_name': 'R_SHOULDER_R', 'rad': -0.557216622708},
|
|||
|
|
{'joint_name': 'R_SHOULDER_P', 'rad': 0.379009062563},
|
|||
|
|
]
|
|||
|
|
|
|||
|
|
|
|||
|
|
play_music_joint_list = [
|
|||
|
|
|
|||
|
|
{'joint_name': 'R_WRIST_Y', 'rad': 0.00},
|
|||
|
|
{'joint_name': 'R_WRIST_P', 'rad': -20.446466942304},
|
|||
|
|
{'joint_name': 'R_ELBOW_R', 'rad': -1.205457349211},
|
|||
|
|
{'joint_name': 'R_SHOULDER_Y', 'rad': -1.959358596443},
|
|||
|
|
{'joint_name': 'R_SHOULDER_R', 'rad': -0.776803694305},
|
|||
|
|
{'joint_name': 'R_SHOULDER_P', 'rad': 0.372088113353},
|
|||
|
|
]
|
|||
|
|
|
|||
|
|
|
|||
|
|
air_joint_list = [
|
|||
|
|
|
|||
|
|
{'joint_name': 'R_WRIST_Y', 'rad': 0.368498066731},
|
|||
|
|
{'joint_name': 'R_WRIST_P', 'rad': -20.806707522108},
|
|||
|
|
{'joint_name': 'R_ELBOW_R', 'rad': -1.160211560687},
|
|||
|
|
{'joint_name': 'R_SHOULDER_Y', 'rad': -1.754356682523},
|
|||
|
|
{'joint_name': 'R_SHOULDER_R', 'rad': -0.652379437044},
|
|||
|
|
{'joint_name': 'R_SHOULDER_P', 'rad': 0.325288409885},
|
|||
|
|
]
|
|||
|
|
|
|||
|
|
empty_joint_list = [
|
|||
|
|
{'joint_name': 'R_WRIST_Y', 'rad': 0.065426748642},
|
|||
|
|
{'joint_name': 'R_WRIST_P', 'rad': -20.572653948527},
|
|||
|
|
{'joint_name': 'R_ELBOW_R', 'rad': -1.354382583029},
|
|||
|
|
{'joint_name': 'R_SHOULDER_Y', 'rad': -1.857614662799},
|
|||
|
|
{'joint_name': 'R_SHOULDER_R', 'rad': -0.490473270507},
|
|||
|
|
{'joint_name': 'R_SHOULDER_P', 'rad': 0.343988546965},
|
|||
|
|
]
|
|||
|
|
|
|||
|
|
|
|||
|
|
nav_joint_list = [
|
|||
|
|
|
|||
|
|
{'joint_name': 'R_WRIST_Y', 'rad': 0.057895434551},
|
|||
|
|
{'joint_name': 'R_WRIST_P', 'rad': -20.659321964551},
|
|||
|
|
{'joint_name': 'R_ELBOW_R', 'rad': -1.348938659874},
|
|||
|
|
{'joint_name': 'R_SHOULDER_Y', 'rad': -1.969583869368},
|
|||
|
|
{'joint_name': 'R_SHOULDER_R', 'rad': -0.586725818719},
|
|||
|
|
{'joint_name': 'R_SHOULDER_P', 'rad': 0.35676823963},
|
|||
|
|
]
|
|||
|
|
|
|||
|
|
|
|||
|
|
air_key_joint_list = [
|
|||
|
|
|
|||
|
|
{'joint_name': 'R_WRIST_Y', 'rad': 0.394130543969},
|
|||
|
|
{'joint_name': 'R_WRIST_P', 'rad': -20.514247819877},
|
|||
|
|
{'joint_name': 'R_ELBOW_R', 'rad': -1.122382227544},
|
|||
|
|
{'joint_name': 'R_SHOULDER_Y', 'rad': -1.903357855984},
|
|||
|
|
{'joint_name': 'R_SHOULDER_R', 'rad': -0.756327518825},
|
|||
|
|
{'joint_name': 'R_SHOULDER_P', 'rad': 0.422019377847},
|
|||
|
|
]
|
|||
|
|
|
|||
|
|
def action_one():
|
|||
|
|
while True:
|
|||
|
|
client.send(start_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
time.sleep(2)
|
|||
|
|
client.send(set_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
client.send(start_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
time.sleep(2)
|
|||
|
|
client.send(drive_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
client.send(start_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
client.send(mode_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
client.send(start_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
client.send(home_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
|
|||
|
|
def action_two():
|
|||
|
|
while True:
|
|||
|
|
client.send(start_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
time.sleep(2.0)
|
|||
|
|
client.send(wyy_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
client.send(start_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
time.sleep(2.0)
|
|||
|
|
client.send(play_music_joint_list, vel=3.0, acc=3.0)
|
|||
|
|
client.send(start_joint_list, vel=3.0, acc=3.0)
|
|||
|
|
time.sleep(2.0)
|
|||
|
|
client.send(play_music_joint_list, vel=3.0, acc=3.0)
|
|||
|
|
client.send(start_joint_list, vel=3.0, acc=3.0)
|
|||
|
|
time.sleep(2.0)
|
|||
|
|
client.send(home_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
|
|||
|
|
def action_three():
|
|||
|
|
while True:
|
|||
|
|
client.send(start_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
time.sleep(2.0)
|
|||
|
|
client.send(empty_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
client.send(start_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
time.sleep(2.0)
|
|||
|
|
client.send(empty_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
client.send(start_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
time.sleep(2.0)
|
|||
|
|
client.send(nav_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
client.send(start_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
time.sleep(7.0)
|
|||
|
|
client.send(home_joint_list, vel=5.0, acc=5.0)
|
|||
|
|
|
|||
|
|
|
|||
|
|
# ===== 录制工具 =====
|
|||
|
|
JOINT_LISTS = {
|
|||
|
|
'set_joint_list': set_joint_list,
|
|||
|
|
'drive_joint_list': drive_joint_list,
|
|||
|
|
'mode_joint_list': mode_joint_list,
|
|||
|
|
'home_joint_list': home_joint_list,
|
|||
|
|
'start_joint_list': start_joint_list,
|
|||
|
|
'wyy_joint_list': wyy_joint_list,
|
|||
|
|
'play_music_joint_list': play_music_joint_list,
|
|||
|
|
'air_joint_list': air_joint_list,
|
|||
|
|
'air_key_joint_list': air_key_joint_list,
|
|||
|
|
'empty_joint_list': empty_joint_list,
|
|||
|
|
'nav_joint_list': nav_joint_list,
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
def find_joint_list_name(lst):
|
|||
|
|
for name, value in JOINT_LISTS.items():
|
|||
|
|
if value is lst:
|
|||
|
|
return name
|
|||
|
|
return None
|
|||
|
|
|
|||
|
|
|
|||
|
|
class StopRecording(Exception):
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
|
|||
|
|
class RecordingClient:
|
|||
|
|
def __init__(self, max_steps=100):
|
|||
|
|
self.max_steps = max_steps
|
|||
|
|
self.steps = []
|
|||
|
|
self._send_count = 0
|
|||
|
|
|
|||
|
|
def send(self, joint_list, vel=3.0, acc=3.0):
|
|||
|
|
name = find_joint_list_name(joint_list)
|
|||
|
|
self.steps.append(("move", name, vel, acc))
|
|||
|
|
self._send_count += 1
|
|||
|
|
|
|||
|
|
# ① 安全兜底：防止真死循环
|
|||
|
|
if self._send_count >= self.max_steps:
|
|||
|
|
raise StopRecording
|
|||
|
|
|
|||
|
|
# ② 关键：第一次回到 home 就结束一轮动作
|
|||
|
|
if name == "home_joint_list":
|
|||
|
|
raise StopRecording
|
|||
|
|
|
|||
|
|
def sleep(self, seconds):
|
|||
|
|
self.steps.append(("wait", seconds))
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
def record_action(func, max_steps=50):
|
|||
|
|
global client
|
|||
|
|
|
|||
|
|
real_client = client
|
|||
|
|
real_sleep = time.sleep
|
|||
|
|
|
|||
|
|
rec_client = RecordingClient(max_steps=max_steps)
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
client = rec_client
|
|||
|
|
time.sleep = rec_client.sleep
|
|||
|
|
func()
|
|||
|
|
except StopRecording:
|
|||
|
|
pass
|
|||
|
|
finally:
|
|||
|
|
client = real_client
|
|||
|
|
time.sleep = real_sleep
|
|||
|
|
|
|||
|
|
return rec_client.steps
|
|||
|
|
|
|||
|
|
|
|||
|
|
def joints_to_cmds(joint_list, default_vel=1.0):
|
|||
|
|
return [
|
|||
|
|
{
|
|||
|
|
"jointName": j["joint_name"],
|
|||
|
|
"rad": j["rad"],
|
|||
|
|
"vel": default_vel
|
|||
|
|
}
|
|||
|
|
for j in joint_list
|
|||
|
|
]
|
|||
|
|
|
|||
|
|
|
|||
|
|
def steps_to_json_obj(steps):
|
|||
|
|
obj = {}
|
|||
|
|
idx = 1
|
|||
|
|
for s in steps:
|
|||
|
|
kind = s[0]
|
|||
|
|
if kind == "move":
|
|||
|
|
_, list_name, vel, acc = s
|
|||
|
|
key = f"{idx}_{list_name}"
|
|||
|
|
joint_list = JOINT_LISTS[list_name]
|
|||
|
|
obj[key] = {
|
|||
|
|
"acc": acc,
|
|||
|
|
"vel": vel,
|
|||
|
|
"cmds": joints_to_cmds(joint_list, default_vel=1.0),
|
|||
|
|
}
|
|||
|
|
elif kind == "wait":
|
|||
|
|
_, seconds = s
|
|||
|
|
key = f"{idx}_wait"
|
|||
|
|
obj[key] = seconds
|
|||
|
|
idx += 1
|
|||
|
|
return obj
|
|||
|
|
|
|||
|
|
|
|||
|
|
# ===== 主入口：录制并保存到文件 =====
|
|||
|
|
if __name__ == "__main__":
|
|||
|
|
# 占位真实 client（防止 record_action 里引用失败）
|
|||
|
|
class DummyClient:
|
|||
|
|
def send(self, *args, **kwargs):
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
client = DummyClient()
|
|||
|
|
|
|||
|
|
# 分别录制三个动作
|
|||
|
|
steps_one = record_action(action_one, max_steps=20)
|
|||
|
|
steps_two = record_action(action_two, max_steps=20)
|
|||
|
|
steps_three = record_action(action_three, max_steps=20)
|
|||
|
|
|
|||
|
|
data = {
|
|||
|
|
"action_one": steps_to_json_obj(steps_one),
|
|||
|
|
"action_two": steps_to_json_obj(steps_two),
|
|||
|
|
"action_three": steps_to_json_obj(steps_three),
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
# ✅ 保存到文件
|
|||
|
|
with open("actions.json", "w", encoding="utf-8") as f:
|
|||
|
|
json.dump(data, f, indent=2, ensure_ascii=False)
|
|||
|
|
|
|||
|
|
print("已保存到 actions.json")
|