diff --git a/.idea/grpc_client.iml b/.idea/grpc_client.iml
index 7584dbd..2c80e12 100644
--- a/.idea/grpc_client.iml
+++ b/.idea/grpc_client.iml
@@ -4,7 +4,7 @@
-
+
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
index a031d5d..366343e 100644
--- a/.idea/misc.xml
+++ b/.idea/misc.xml
@@ -3,5 +3,5 @@
-
+
\ No newline at end of file
diff --git a/clients/__init__.py b/clients/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/clients/__pycache__/__init__.cpython-39.pyc b/clients/__pycache__/__init__.cpython-39.pyc
new file mode 100644
index 0000000..bf63648
Binary files /dev/null and b/clients/__pycache__/__init__.cpython-39.pyc differ
diff --git a/clients/__pycache__/base_client.cpython-39.pyc b/clients/__pycache__/base_client.cpython-39.pyc
new file mode 100644
index 0000000..985fd70
Binary files /dev/null and b/clients/__pycache__/base_client.cpython-39.pyc differ
diff --git a/clients/__pycache__/hand_client.cpython-39.pyc b/clients/__pycache__/hand_client.cpython-39.pyc
new file mode 100644
index 0000000..2a93c5c
Binary files /dev/null and b/clients/__pycache__/hand_client.cpython-39.pyc differ
diff --git a/clients/__pycache__/http_server.cpython-39.pyc b/clients/__pycache__/http_server.cpython-39.pyc
new file mode 100644
index 0000000..c5a6359
Binary files /dev/null and b/clients/__pycache__/http_server.cpython-39.pyc differ
diff --git a/clients/__pycache__/movej_client.cpython-39.pyc b/clients/__pycache__/movej_client.cpython-39.pyc
new file mode 100644
index 0000000..90a9f1c
Binary files /dev/null and b/clients/__pycache__/movej_client.cpython-39.pyc differ
diff --git a/clients/base_client.py b/clients/base_client.py
index dc6c854..6896f03 100644
--- a/clients/base_client.py
+++ b/clients/base_client.py
@@ -1,8 +1,11 @@
import grpc
import time
import sys
-sys.path.append("/home/lgv/cmvr/0-workspace/grpc_client/generated") # 指向 generated
-sys.path.append("/home/lgv/cmvr/0-workspace/grpc_client/generated/cmvr") # 指向 cmvr 顶层
+from pathlib import Path
+BASE_DIR = Path(__file__).resolve().parent.parent # 上一级目录
+
+sys.path.append(str(BASE_DIR / "generated"))
+sys.path.append(str(BASE_DIR / "generated" / "cmvr"))
from cmvr.api import humanoid_robot_service_pb2_grpc
from cmvr.api import dexhand_service_pb2_grpc
diff --git a/clients/get_joint_state_client.py b/clients/get_joint_state_client.py
index f0d8d00..e856b76 100644
--- a/clients/get_joint_state_client.py
+++ b/clients/get_joint_state_client.py
@@ -2,19 +2,42 @@ import sys
import time
from google.protobuf import timestamp_pb2
+import matplotlib.pyplot as plt # 新增
+
sys.path.append("../generated")
from generated.cmvr.api import humanoid_robot_service_pb2_grpc as rpc
from generated.cmvr.api import humanoid_robot_command_pb2 as pb
from clients.base_client import RobotClientBase
+# 固定关节顺序
+JOINT_ORDER = [
+ "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
+ "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R",
+ "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
+ "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R",
+ "HEAD_P", "HEAD_Y", "HEAD_R","WAIST_Y","WAIST_R"
+]
+
class GetJointStateClient(RobotClientBase):
"""Continuous client to fetch robot joint states"""
+ def __init__(self, *args, **kwargs):
+ super().__init__(*args, **kwargs)
+ # 时间轴
+ self.time_history = []
+ self._start_time = None
+ # 记录每个关节的角度、速度
+ self.pos_history = {name: [] for name in JOINT_ORDER}
+ self.vel_history = {name: [] for name in JOINT_ORDER}
+
def send(self, interval=0.5, device_id="hc01"):
"""Continuously fetch joint states and print in fixed order"""
try:
+ if self._start_time is None:
+ self._start_time = time.time()
+
while True:
# Construct request
req = pb.JointRequest()
@@ -31,24 +54,25 @@ class GetJointStateClient(RobotClientBase):
time.sleep(interval)
continue
- # Fixed order for printing
- joint_order = [
- "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
- "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R",
- "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
- "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R",
- "HEAD_P", "HEAD_Y","HEAD_R"
- ]
-
-
- # Map response to dictionary
- joint_dict = {}
+ # ====== 1. 同时取 position 和 velocity ======
+ joint_pos_dict = {}
+ joint_vel_dict = {}
for state in resp.state:
- for name, pos in zip(state.name, state.position):
- joint_dict[name] = round(pos, 12)
+ # 这里就用你说的三元 zip
+ for name, pos, vel in zip(state.name, state.position, state.velocity):
+ joint_pos_dict[name] = round(pos, 12)
+ joint_vel_dict[name] = round(vel, 12)
- joint_list = [{"joint_name": name, "rad": joint_dict.get(name, 0.0)}
- for name in joint_order]
+ # ====== 2. 记录到历史数据里，用于之后画图 ======
+ t = time.time() - self._start_time
+ self.time_history.append(t)
+ for name in JOINT_ORDER:
+ self.pos_history[name].append(joint_pos_dict.get(name, 0.0))
+ self.vel_history[name].append(joint_vel_dict.get(name, 0.0))
+
+ # 保持原来的打印逻辑（打印角度）
+ joint_list = [{"joint_name": name, "rad": joint_pos_dict.get(name, 0.0)}
+ for name in JOINT_ORDER]
# Print timestamp
print(f"[{time.strftime('%H:%M:%S')}]")
@@ -63,11 +87,67 @@ class GetJointStateClient(RobotClientBase):
except KeyboardInterrupt:
print("\nStopped fetching joint states.")
+ # 例子：退出时画几个关节的角度 & 速度曲线
+ # try:
+ # # 你可以根据需要改关节列表
+ # self.plot_pos_and_vel([ "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
+ # "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"])
+ # except Exception as e:
+ # print(f"绘图失败: {e}")
+
+ # ====== 3. 绘制速度和角度（同一张图，两行子图） ======
+ def plot_pos_and_vel(self, joint_names):
+ """
+ 在一张图中绘制多个关节的角度和速度：
+ - 上 subplot：角度（rad）
+ - 下 subplot：速度（rad/s）
+
+ joint_names: str 或 [str, ...]
+ """
+ if isinstance(joint_names, str):
+ joint_names = [joint_names]
+
+ # 检查关节名合法性
+ invalid = [n for n in joint_names if n not in self.pos_history]
+ if invalid:
+ print(f"未知关节名: {invalid}")
+ print(f"可选关节: {list(self.pos_history.keys())}")
+ return
+
+ if not self.time_history:
+ print("还没有采集到任何数据，无法绘图。")
+ return
+
+ t = self.time_history
+
+ plt.figure(figsize=(10, 6))
+
+ # 上：角度
+ plt.subplot(2, 1, 1)
+ for name in joint_names:
+ plt.plot(t, self.pos_history[name], label=name)
+ plt.ylabel("Position (rad)")
+ plt.title("Joint Position")
+ plt.grid(True)
+ plt.legend()
+
+ # 下：速度
+ plt.subplot(2, 1, 2)
+ for name in joint_names:
+ plt.plot(t, self.vel_history[name], label=name)
+ plt.xlabel("Time (s)")
+ plt.ylabel("Velocity (rad/s)")
+ plt.title("Joint Velocity")
+ plt.grid(True)
+ plt.legend()
+
+ plt.tight_layout()
+ plt.show()
if __name__ == "__main__":
client = GetJointStateClient()
try:
- client.send(interval=0.5)
+ client.send(interval=0.005)
finally:
client.close()
diff --git a/clients/hand_client.py b/clients/hand_client.py
index 9c74bb2..dc212e4 100644
--- a/clients/hand_client.py
+++ b/clients/hand_client.py
@@ -14,16 +14,16 @@ handshake = [
{"id": 1, "value": 0.0},
{"id": 2, "value": 0.0},
{"id": 3, "value": 0.0},
- {"id": 4, "value": 0.1},
- {"id": 5, "value": 0.0},
+ {"id": 4, "value": 0.0},
+ {"id": 5, "value": 1.0},
]
hand_open = [
- {"id": 0, "value": 0.8},
- {"id": 1, "value": 0.8},
- {"id": 2, "value": 0.8},
- {"id": 3, "value": 0.8},
- {"id": 4, "value": 0.8},
+ {"id": 0, "value": 0.7},
+ {"id": 1, "value": 0.7},
+ {"id": 2, "value": 0.7},
+ {"id": 3, "value": 0.7},
+ {"id": 4, "value": 0.7},
{"id": 5, "value": 1.0},
]
@@ -104,7 +104,7 @@ if __name__ == "__main__":
# ]
# client.set_angles(hand_open, device_id="hand2")
- client.set_angles(angles, device_id="hand2")
+ client.set_angles(hand_open, device_id="hand2")
# client.set_angles(handshake, device_id="hand1")
# Close client
diff --git a/clients/http_server.py b/clients/http_server.py
new file mode 100644
index 0000000..9489167
--- /dev/null
+++ b/clients/http_server.py
@@ -0,0 +1,133 @@
+from __future__ import annotations
+
+import time
+from threading import Lock
+from fastapi import FastAPI
+from pydantic import BaseModel
+from typing import Optional
+
+from clients.movej_client import MoveJClient, action_music,action_air,action_drive_comfort,action_drive_economy,action_hello
+
+
+app = FastAPI(title="Humanoid Robot Control API (Blocking + Mutex + Ignore)")
+
+class ActionResponse(BaseModel):
+ code: int
+ msg: str
+ action: Optional[str] = None
+ ignored: bool = False
+
+class StatusResponse(BaseModel):
+ running: bool
+ current_action: Optional[str]
+ last_action: Optional[str]
+ last_finished_at: Optional[float]
+
+
+# 🔒 全局唯一互斥锁：保证同一时间只能执行一个 action
+action_lock = Lock()
+
+# 状态变量（简单可用；多线程更严谨可再加锁，这里够用）
+running = False
+current_action = None
+last_action = None
+last_finished_at = None
+
+
+def run_action_blocking_ignore_if_busy(action_func, action_name: str) -> ActionResponse:
+ """
+ 通用执行模板：
+ - 忙：直接忽略（立即返回，不执行）
+ - 空闲：阻塞执行，执行完再返回
+ """
+ global running, current_action, last_action, last_finished_at
+
+ # 忙就忽略（静默丢弃动作）
+ if not action_lock.acquire(blocking=False):
+ return ActionResponse(
+ code=0,
+ msg=f"{action_name} ignored: robot busy",
+ action=action_name,
+ ignored=True,
+ )
+
+ client = MoveJClient()
+ # client = 0
+ running = True
+ current_action = action_name
+
+ try:
+ action_func(client) # ✅ 阻塞到动作执行结束
+ last_action = action_name
+ last_finished_at = time.time()
+ return ActionResponse(
+ code=0,
+ msg=f"{action_name} finished",
+ action=action_name,
+ ignored=False,
+ )
+ except Exception as e:
+ return ActionResponse(
+ code=1,
+ msg=f"{action_name} failed: {e}",
+ action=action_name,
+ ignored=False,
+ )
+ finally:
+ try:
+ # print("closing action")
+ client.close()
+ finally:
+ running = False
+ current_action = None
+ action_lock.release()
+
+
+# @app.post("/action/one", response_model=ActionResponse)
+# def call_action_one():
+# return run_action_blocking_ignore_if_busy(action_one, "action_one")
+
+@app.post("/action/music", response_model=ActionResponse)
+def call_action_music():
+ return run_action_blocking_ignore_if_busy(action_music, "action_music")
+
+@app.post("/action/air", response_model=ActionResponse)
+def call_action_air():
+ return run_action_blocking_ignore_if_busy(action_air, "action_air")
+
+@app.post("/action/hello", response_model=ActionResponse)
+def call_action_hello():
+ return run_action_blocking_ignore_if_busy(action_hello, "action_hello")
+# @app.post("/action/drive_comfort", response_model=ActionResponse)
+# def call_action_drive_comfort():
+# return run_action_blocking_ignore_if_busy(action_drive_comfort, "action_drive_comfort")
+#
+# @app.post("/action/drive_economy", response_model=ActionResponse)
+# def call_action_drive_economy():
+# return run_action_blocking_ignore_if_busy(action_drive_economy, "action_drive_economy")
+#
+# @app.post("/action/two", response_model=ActionResponse)
+# def call_action_two():
+# return run_action_blocking_ignore_if_busy(action_two, "action_two")
+# @app.post("/action/two", response_model=ActionResponse)
+# def call_action_two():
+# return run_action_blocking_ignore_if_busy(action_two, "action_two")
+
+@app.get("/status", response_model=StatusResponse)
+def get_status():
+ return StatusResponse(
+ running=running,
+ current_action=current_action,
+ last_action=last_action,
+ last_finished_at=last_finished_at,
+ )
+
+
+if __name__ == "__main__":
+ import uvicorn
+ uvicorn.run(
+ "http_server:app",
+ host="0.0.0.0",
+ port=8000,
+ reload=False
+ )
\ No newline at end of file
diff --git a/clients/main.py b/clients/main.py
new file mode 100644
index 0000000..68da937
--- /dev/null
+++ b/clients/main.py
@@ -0,0 +1,141 @@
+# import time
+#
+# from hand_client import DexHandClient
+#
+#
+# from movej_client import MoveJClient
+#
+# import atexit
+#
+# hand_open = [
+# {"id": 0, "value": 0.8},
+# {"id": 1, "value": 0.8},
+# {"id": 2, "value": 0.8},
+# {"id": 3, "value": 0.8},
+# {"id": 4, "value": 0.8},
+# {"id": 5, "value": 1.0},
+# ]
+#
+#
+# hand_touch = [
+# {"id": 0, "value": 0},
+# {"id": 1, "value": 0},
+# {"id": 2, "value": 0},
+# {"id": 3, "value": 1.0},
+# {"id": 4, "value": 0.3},
+# {"id": 5, "value": 0.3},
+# ]
+#
+# handshake = [
+# {"id": 0, "value": 0.0},
+# {"id": 1, "value": 0.0},
+# {"id": 2, "value": 0.0},
+# {"id": 3, "value": 0.0},
+# {"id": 4, "value": 0.0},
+# {"id": 5, "value": 1.0},
+# ]
+#
+#
+# left_joint_list = [
+#
+# {'joint_name': 'L_SHOULDER_P', 'rad': -0.230949540675},
+# {'joint_name': 'L_SHOULDER_R', 'rad': -1.0384195613},
+# {'joint_name': 'L_SHOULDER_Y', 'rad': -15.045008286377},
+# {'joint_name': 'L_ELBOW_R', 'rad': -1.615503893099},
+# {'joint_name': 'L_WRIST_P', 'rad': 14.787740913187},
+# {'joint_name': 'L_WRIST_Y', 'rad': -0.353138324696},
+# {'joint_name': 'L_WRIST_R', 'rad': 0.28},
+# ]
+#
+# right_joint_list = [
+#
+# {'joint_name': 'L_SHOULDER_P', 'rad': -0.038843127376},
+# {'joint_name': 'L_SHOULDER_R', 'rad': -0.782479233371},
+# {'joint_name': 'L_SHOULDER_Y', 'rad': -14.639593111463},
+# {'joint_name': 'L_ELBOW_R', 'rad': -2.009983759425},
+# {'joint_name': 'L_WRIST_P', 'rad': 14.79076046324},
+# {'joint_name': 'L_WRIST_Y', 'rad': -0.022087994402},
+# {'joint_name': 'L_WRIST_R', 'rad': 0.28},
+#
+#
+# ]
+# start_joint_list = [
+# {'joint_name': 'L_SHOULDER_P', 'rad': -0.029367758425},
+# {'joint_name': 'L_SHOULDER_R', 'rad': -1.448823970756},
+# {'joint_name': 'L_SHOULDER_Y', 'rad': -14.5604203382},
+# {'joint_name': 'L_ELBOW_R', 'rad': -0.184456443516},
+# {'joint_name': 'L_WRIST_P', 'rad': 14.629440930445},
+# {'joint_name': 'L_WRIST_Y', 'rad': -0.021720636379},
+# {'joint_name': 'L_WRIST_R', 'rad': -0.120802885537},
+# ]
+#
+# single_joint = [
+# {'joint_name': 'R_SHOULDER_Y', 'rad': 0},
+# ]
+#
+#
+# first_joint_list = [
+# {'joint_name': 'L_SHOULDER_P', 'rad': 0.013117624077},
+# {'joint_name': 'L_SHOULDER_R', 'rad': -1.153729162914},
+# {'joint_name': 'L_SHOULDER_Y', 'rad': -14.255272070875},
+# {'joint_name': 'L_ELBOW_R', 'rad': -1.477644963752},
+# {'joint_name': 'L_WRIST_P', 'rad': 15.28202635377},
+# {'joint_name': 'L_WRIST_Y', 'rad': -0.000359764059},
+# {'joint_name': 'L_WRIST_R', 'rad': 0.124166062475},
+# ]
+#
+# second_joint_list=[
+# {'joint_name': 'L_SHOULDER_P', 'rad': -0.563525308548},
+# {'joint_name': 'L_SHOULDER_R', 'rad': -1.290596130675},
+# {'joint_name': 'L_SHOULDER_Y', 'rad': -14.162486167364},
+# {'joint_name': 'L_ELBOW_R', 'rad': -1.021621712276},
+# {'joint_name': 'L_WRIST_P', 'rad': 15.432362166456},
+# {'joint_name': 'L_WRIST_Y', 'rad': 0.018426754365},
+# {'joint_name': 'L_WRIST_R', 'rad': -0.005352795583},
+# ]
+#
+# def action_hello():
+# # client.send(start_joint_list, vel=2.0, acc=2.0)
+# hand_client.set_angles(hand_open, device_id="hand1")
+# client.send(right_joint_list, vel=2.0, acc=2.0)
+# # time.sleep(1)
+# while True:
+# client.send(right_joint_list, vel=3.0, acc=3.0)
+# client.send(left_joint_list, vel=3.0, acc=3.0)
+# # client.send(start_joint_list, vel=1.0, acc=1.0)
+#
+# def action_touch():
+# # client.send(start_joint_list, vel=2.0, acc=2.0)
+# hand_client.set_angles(hand_touch, device_id="hand1")
+# client.send(first_joint_list, vel=2.0, acc=2.0)
+# # time.sleep(1)
+# while True:
+# client.send(first_joint_list, vel=5.0, acc=3.0)
+# client.send(second_joint_list, vel=5.0, acc=3.0)
+# # client.send(start_joint_list, vel=1.0, acc=1.0)
+#
+#
+# def cleanup():
+# time.sleep(3)
+# hand_client.set_angles(handshake, device_id="hand1")
+# client.send(start_joint_list, vel=2.0, acc=2.0)
+# if hand_client is not None:
+# hand_client.close()
+# if client is not None:
+# client.close()
+# print("程序结束：已执行清理并打印此信息。")
+#
+# atexit.register(cleanup)
+#
+#
+# if __name__ == "__main__":
+# # Initialize client
+# global hand_client
+# global client
+#
+# hand_client = DexHandClient()
+# client = MoveJClient()
+#
+# action_touch()
+#
+#
diff --git a/clients/movej_client.py b/clients/movej_client.py
index fda57e2..e6c3440 100644
--- a/clients/movej_client.py
+++ b/clients/movej_client.py
@@ -3,6 +3,8 @@ import time
from google.protobuf import timestamp_pb2
+from clients.hand_client import DexHandClient
+
sys.path.append("../generated")
from generated.cmvr.api import humanoid_robot_command_pb2 as pb
@@ -13,6 +15,7 @@ import math
from typing import List, Dict, Literal
+
class MoveJClient(RobotClientBase):
"""Client to send MoveJ commands to the robot"""
@@ -29,20 +32,7 @@ class MoveJClient(RobotClientBase):
device_id: device ID
"""
# Construct JointCmd list
- # cmds = [pb.JointCmd(joint_name=j["joint_name"], rad=j["rad"], vel=vel) for j in joint_list]
-
- offset = {
- "R_WRIST_Y": -0.0,
- }
-
- cmds = [
- pb.JointCmd(
- joint_name=j["joint_name"],
- rad=j["rad"] + offset.get(j["joint_name"], 0),
- vel=vel
- )
- for j in joint_list
- ]
+ cmds = [pb.JointCmd(joint_name=j["joint_name"], rad=j["rad"], vel=vel) for j in joint_list]
# Construct request header
header = common_pb2.CommandHeader.Request()
@@ -73,56 +63,327 @@ class MoveJClient(RobotClientBase):
print("MoveJ RPC call failed:", e)
-zero_joint_list = [
+# init_joint_list = [
+# {'joint_name': 'R_SHOULDER_P', 'rad': -0.003768504782},
+# {'joint_name': 'R_SHOULDER_R', 'rad': -0.256264020657},
+# {'joint_name': 'R_SHOULDER_Y', 'rad': 2.526736892626},
+# {'joint_name': 'R_ELBOW_R', 'rad': -1.542952107456},
+# {'joint_name': 'R_WRIST_P', 'rad': 0.117851681162},
+# {'joint_name': 'R_WRIST_Y', 'rad': 0.097711538603},
+# {'joint_name': 'R_WRIST_R', 'rad': 0.021115966972},
+# ]
- {'joint_name': 'R_WRIST_R', 'rad': 0},
- {'joint_name': 'R_WRIST_Y', 'rad': 0},
- {'joint_name': 'R_WRIST_P', 'rad': 0},
- {'joint_name': 'R_ELBOW_R', 'rad': 0},
- {'joint_name': 'R_SHOULDER_Y', 'rad': 0},
- {'joint_name': 'R_SHOULDER_R', 'rad': 0},
- {'joint_name': 'R_SHOULDER_P', 'rad': 0},
+init_joint_list = [
+{'joint_name': 'R_SHOULDER_P', 'rad': -0.062633119026},
+{'joint_name': 'R_SHOULDER_R', 'rad': -0.013667237243},
+{'joint_name': 'R_SHOULDER_Y', 'rad': 2.563999526183},
+{'joint_name': 'R_ELBOW_R', 'rad': -0.275263170082},
+{'joint_name': 'R_WRIST_P', 'rad': 0.21726806475},
+{'joint_name': 'R_WRIST_Y', 'rad': -0.044427538871},
+{'joint_name': 'R_WRIST_R', 'rad': -0.065047050427},
]
start_joint_list = [
-
- {'joint_name': 'R_WRIST_R', 'rad': -0.182982265197},
- {'joint_name': 'R_WRIST_Y', 'rad': 0.057211977764},
- {'joint_name': 'R_WRIST_P', 'rad': -2.450283707826},
- {'joint_name': 'R_ELBOW_R', 'rad': 1.691315387916},
- {'joint_name': 'R_SHOULDER_Y', 'rad': 1.741483963795},
- {'joint_name': 'R_SHOULDER_R', 'rad': 1.000908225412},
- {'joint_name': 'R_SHOULDER_P', 'rad': -0.275362840863},
+{'joint_name': 'R_SHOULDER_P', 'rad': 0.212733518819},
+{'joint_name': 'R_SHOULDER_R', 'rad': -0.391965314899},
+{'joint_name': 'R_SHOULDER_Y', 'rad': 2.564591855398},
+{'joint_name': 'R_ELBOW_R', 'rad': -1.503521397093},
+{'joint_name': 'R_WRIST_P', 'rad': 0.217252876821},
+{'joint_name': 'R_WRIST_Y', 'rad': 0.164711187099},
+{'joint_name': 'R_WRIST_R', 'rad': 0.227967960443},
]
-end_joint_list = [
+air_in_joint_list= [
+{'joint_name': 'R_SHOULDER_P', 'rad': 0.467093352929},
+{'joint_name': 'R_SHOULDER_R', 'rad': -0.397918033661},
+{'joint_name': 'R_SHOULDER_Y', 'rad': 2.332349442301},
+{'joint_name': 'R_ELBOW_R', 'rad': -1.108740619931},
+{'joint_name': 'R_WRIST_P', 'rad': 0.184619713748},
+{'joint_name': 'R_WRIST_Y', 'rad': 0.165660432636},
+{'joint_name': 'R_WRIST_R', 'rad': 0.013328010806},
+]
- {'joint_name': 'R_WRIST_R', 'rad': 0.590444918686},
- {'joint_name': 'R_WRIST_Y', 'rad': 0.133274073403},
- {'joint_name': 'R_WRIST_P', 'rad': -2.832957807407},
- {'joint_name': 'R_ELBOW_R', 'rad': 0.722278081411},
- {'joint_name': 'R_SHOULDER_Y', 'rad': 1.454527987188},
- {'joint_name': 'R_SHOULDER_R', 'rad': 1.319798720377},
- {'joint_name': 'R_SHOULDER_P', 'rad': 0.886766195809},
+air_open_joint_list= [
+{'joint_name': 'R_SHOULDER_P', 'rad': 0.562264710474},
+{'joint_name': 'R_SHOULDER_R', 'rad': -0.375382944612},
+{'joint_name': 'R_SHOULDER_Y', 'rad': 2.390867581924},
+{'joint_name': 'R_ELBOW_R', 'rad': -1.143580778878},
+{'joint_name': 'R_WRIST_P', 'rad': 0.19485637762},
+{'joint_name': 'R_WRIST_Y', 'rad': 0.159855796177},
+{'joint_name': 'R_WRIST_R', 'rad': 0.26605484086},
+]
+
+air_close_joint_list= air_open_joint_list
+
+go_home_joint_list = [
+{'joint_name': 'R_SHOULDER_P', 'rad': 0.451632041621},
+{'joint_name': 'R_SHOULDER_R', 'rad': -0.307911521083},
+{'joint_name': 'R_SHOULDER_Y', 'rad': 2.13536865317},
+{'joint_name': 'R_ELBOW_R', 'rad': -1.294201365225},
+{'joint_name': 'R_WRIST_P', 'rad': 0.378274346518},
+{'joint_name': 'R_WRIST_Y', 'rad': 0.037015483938},
+{'joint_name': 'R_WRIST_R', 'rad': 0.148664191295},
+]
+
+empty_joint_list = [
+{'joint_name': 'R_SHOULDER_P', 'rad': 0.255359389661},
+{'joint_name': 'R_SHOULDER_R', 'rad': -0.283898456732},
+{'joint_name': 'R_SHOULDER_Y', 'rad': 2.498425644483},
+{'joint_name': 'R_ELBOW_R', 'rad': -1.261196097901},
+{'joint_name': 'R_WRIST_P', 'rad': 0.159945974503},
+{'joint_name': 'R_WRIST_Y', 'rad': 0.124704284694},
+{'joint_name': 'R_WRIST_R', 'rad': 0.089323055792},
]
+music_in_joint_list= [
+{'joint_name': 'R_SHOULDER_P', 'rad': 0.534755574811},
+{'joint_name': 'R_SHOULDER_R', 'rad': -0.370206708698},
+{'joint_name': 'R_SHOULDER_Y', 'rad': 2.318657524675},
+{'joint_name': 'R_ELBOW_R', 'rad': -1.133308992921},
+{'joint_name': 'R_WRIST_P', 'rad': 0.217250029084},
+{'joint_name': 'R_WRIST_Y', 'rad': 0.151281261241},
+{'joint_name': 'R_WRIST_R', 'rad': 0.087528290167},
+]
-if __name__ == "__main__":
- # Initialize client
- client = MoveJClient()
+music_open_joint_list = [
+{'joint_name': 'R_SHOULDER_P', 'rad': 0.562264710474},
+{'joint_name': 'R_SHOULDER_R', 'rad': -0.375382944612},
+{'joint_name': 'R_SHOULDER_Y', 'rad': 2.390867581924},
+{'joint_name': 'R_ELBOW_R', 'rad': -1.143580778878},
+{'joint_name': 'R_WRIST_P', 'rad': 0.19485637762},
+{'joint_name': 'R_WRIST_Y', 'rad': 0.149855796177},
+{'joint_name': 'R_WRIST_R', 'rad': 0.25605484086},
+]
+music_close_joint_list = music_open_joint_list
+set_in_joint_list = [
+{'joint_name': 'R_SHOULDER_P', 'rad': 0.494828409031},
+{'joint_name': 'R_SHOULDER_R', 'rad': -0.305430193249},
+{'joint_name': 'R_SHOULDER_Y', 'rad': 2.207381267348},
+{'joint_name': 'R_ELBOW_R', 'rad': -1.228615143333},
+{'joint_name': 'R_WRIST_P', 'rad': 0.262071504853},
+{'joint_name': 'R_WRIST_Y', 'rad': 0.097687807464},
+{'joint_name': 'R_WRIST_R', 'rad': 0.069247461928},
+]
+dirve_mode_joint_list = [
+{'joint_name': 'R_SHOULDER_P', 'rad': 0.563493983445},
+{'joint_name': 'R_SHOULDER_R', 'rad': -0.162294407962},
+{'joint_name': 'R_SHOULDER_Y', 'rad': 2.249594216381},
+{'joint_name': 'R_ELBOW_R', 'rad': -1.289937354272},
+{'joint_name': 'R_WRIST_P', 'rad': 0.142454226992},
+{'joint_name': 'R_WRIST_Y', 'rad': 0.000561004112},
+{'joint_name': 'R_WRIST_R', 'rad': 0.167736130893},
+]
- R_WRIST_R_joint = [
- {'joint_name': 'R_WRIST_R', 'rad': 0.0},
+dirve_soft_joint_list = [
+
+{'joint_name': 'R_SHOULDER_P', 'rad': 0.535399163285},
+{'joint_name': 'R_SHOULDER_R', 'rad': -0.308316848927},
+{'joint_name': 'R_SHOULDER_Y', 'rad': 2.505504168453},
+{'joint_name': 'R_ELBOW_R', 'rad': -1.088469481487},
+{'joint_name': 'R_WRIST_P', 'rad': -0.006021064442},
+{'joint_name': 'R_WRIST_Y', 'rad': 0.165673722074},
+{'joint_name': 'R_WRIST_R', 'rad': 0.062983648315},
+]
+
+dirve_jie_joint_list = [
+
+{'joint_name': 'R_SHOULDER_P', 'rad': 0.511487668206},
+{'joint_name': 'R_SHOULDER_R', 'rad': -0.330508311093},
+{'joint_name': 'R_SHOULDER_Y', 'rad': 2.33301771116},
+{'joint_name': 'R_ELBOW_R', 'rad': -1.139994529239},
+{'joint_name': 'R_WRIST_P', 'rad': 0.228347658658},
+{'joint_name': 'R_WRIST_Y', 'rad': 0.164826995055},
+{'joint_name': 'R_WRIST_R', 'rad': 0.075367851372},
+]
+
+head_front_joint_list = [
+ {'joint_name': 'HEAD_P', 'rad': -0.15311278316},
+ {'joint_name': 'HEAD_Y', 'rad': -0.04022955268},
+ {'joint_name': 'HEAD_R', 'rad': 0.050031017646},
+]
+
+head_screen_joint_list = [
+ {'joint_name': 'HEAD_P', 'rad': -0.307521160933},
+ {'joint_name': 'HEAD_Y', 'rad': -0.474696809681},
+ {'joint_name': 'HEAD_R', 'rad': 0.074742644342},
+]
+
+singal_joint_list = [
+{'joint_name': 'R_WRIST_P', 'rad': 0.217252876821},
+]
+
+hand_touch = [
+ {"id": 0, "value": 0},
+ {"id": 1, "value": 0},
+ {"id": 2, "value": 0},
+ {"id": 3, "value": 1.0},
+ {"id": 4, "value": 0.3},
+ {"id": 5, "value": 0.3},
+]
+
+hand_open = [
+ {"id": 0, "value": 0.8},
+ {"id": 1, "value": 0.8},
+ {"id": 2, "value": 0.8},
+ {"id": 3, "value": 0.8},
+ {"id": 4, "value": 0.8},
+ {"id": 5, "value": 1.0},
]
- # client.send(start_joint_list, 2.0, 1.0)
- # client.send(start_joint_list,2.0,2.0)
- # client.send(end_joint_list, 1.5, 1.5)
- while True:
- client.send(start_joint_list,1.5,1.5)
- client.send(end_joint_list,1.5,1.5)
+hello_l_joint_list = [
+ # {'joint_name': 'R_SHOULDER_P', 'rad': 0.399221347784},
+ # {'joint_name': 'R_SHOULDER_R', 'rad': -0.287301501983},
+ # {'joint_name': 'R_SHOULDER_Y', 'rad': 2.066446782461},
+ # {'joint_name': 'R_ELBOW_R', 'rad': -2.153093915083},
+ # {'joint_name': 'R_WRIST_P', 'rad': 0.005766666638},
+ # {'joint_name': 'R_WRIST_Y', 'rad': 0.292756816084},
+ # {'joint_name': 'R_WRIST_R', 'rad': -0.466447865964},
+{'joint_name': 'R_SHOULDER_P', 'rad': 0.500229616137},
+{'joint_name': 'R_SHOULDER_R', 'rad': -0.252456596808},
+{'joint_name': 'R_SHOULDER_Y', 'rad': 2.183436548675},
+{'joint_name': 'R_ELBOW_R', 'rad': -2.107785476354},
+{'joint_name': 'R_WRIST_P', 'rad': -0.115909524794},
+{'joint_name': 'R_WRIST_Y', 'rad': 0.10194042747},
+{'joint_name': 'R_WRIST_R', 'rad': -0.283499773607},
+]
+
+hello_r_joint_list = [
+ {'joint_name': 'R_SHOULDER_P', 'rad': 0.406399542535},
+ {'joint_name': 'R_SHOULDER_R', 'rad': -0.299956843483},
+ {'joint_name': 'R_SHOULDER_Y', 'rad': 2.461169655645},
+ {'joint_name': 'R_ELBOW_R', 'rad': -2.049333783919},
+ {'joint_name': 'R_WRIST_P', 'rad': 0.185113321427},
+ {'joint_name': 'R_WRIST_Y', 'rad': -0.44407319941},
+ {'joint_name': 'R_WRIST_R', 'rad': -0.491400683396},
+]
+
+
+
+
+#
+
+
+def action_hello(client):
+ hand_client.set_angles(hand_open, device_id="hand2")
+ client.send(init_joint_list, vel=1.0, acc=1.0)
+ client.send(hello_l_joint_list, vel=2.0, acc=2.0)
+ for _ in range(3):
+ client.send(hello_l_joint_list, vel=3.0, acc=5.0)
+ time.sleep(0.1)
+ client.send(hello_r_joint_list, vel=3.0, acc=5.0)
+ time.sleep(0.1)
+ client.send(init_joint_list, vel=1.5, acc=1.5)
+ hand_client.set_angles(hand_open, device_id="hand2")
+
+def action_air(client):
+ hand_client.set_angles(hand_touch, device_id="hand2")
+ client.send(init_joint_list, vel=2.0, acc=2.0)
+ client.send(head_screen_joint_list, vel=1.0, acc=1.0)
+ client.send(start_joint_list, vel=1.0, acc=1.0)
+ client.send(air_in_joint_list, vel=3.0, acc=3.0)
+ client.send(start_joint_list, vel=3.0, acc=3.0)
+ # # # # #
+ # # time.sleep(1)
+ # # client.send(air_open_joint_list, vel=3.0, acc=3.0)
+ # # client.send(start_joint_list, vel=3.0, acc=3.0)
+ time.sleep(1)
+ client.send(air_close_joint_list, vel=3.0, acc=2.0)
+ client.send(start_joint_list, vel=3.0, acc=2.0)
+ time.sleep(1)
+ client.send(go_home_joint_list, vel=3.0, acc=2.0)
+ client.send(start_joint_list, vel=3.0, acc=3.0)
+ client.send(head_front_joint_list, vel=1.0, acc=1.0)
+ client.send(init_joint_list, vel=2.0, acc=2.0)
+ hand_client.set_angles(hand_open, device_id="hand2")
+
+def action_music(client):
+ # init_joint_list
+ hand_client.set_angles(hand_touch, device_id="hand2")
+ client.send(init_joint_list, vel=2.0, acc=2.0)
+ client.send(head_screen_joint_list, vel=1.0, acc=1.0)
+ client.send(start_joint_list, vel=1.0, acc=1.0)
+ client.send(music_in_joint_list, vel=3.0, acc=3.0)
+ client.send(start_joint_list, vel=3.0, acc=3.0)
+ # # #
+ time.sleep(1)
+ client.send(music_open_joint_list, vel=3.0, acc=3.0)
+ client.send(start_joint_list, vel=3.0, acc=3.0)
+ # time.sleep(2)
+ # client.send(music_close_joint_list, vel=3.0, acc=3.0)
+ # client.send(start_joint_list, vel=3.0, acc=3.0)
+ time.sleep(1)
+ client.send(go_home_joint_list, vel=3.0, acc=2.0)
+ client.send(start_joint_list, vel=3.0, acc=2.0)
+ client.send(head_front_joint_list, vel=1.0, acc=1.0)
+ client.send(init_joint_list, vel=2.0, acc=2.0)
+ hand_client.set_angles(hand_open, device_id="hand2")
+def action_drive_economy(client):
+ # client.send(head_screen_joint_list, vel=1.0, acc=1.0)
+ # client.send(start_joint_list, vel=1.0, acc=1.0)
+ # client.send(set_in_joint_list, vel=3.0, acc=2.0)
+ # client.send(start_joint_list, vel=3.0, acc=2.0)
+ # # #
+ time.sleep(1)
+ # client.send(dirve_mode_joint_list, vel=3.0, acc=2.0)
+ # client.send(start_joint_list, vel=3.0, acc=2.0)
+ #
+ # time.sleep(1)
+ # client.send(dirve_jie_joint_list, vel=3.0, acc=2.0)
+ # client.send(start_joint_list, vel=3.0, acc=2.0)
+ # time.sleep(2)
+ # client.send(go_home_joint_list, vel=3.0, acc=2.0)
+ # client.send(start_joint_list, vel=3.0, acc=2.0)
+ # client.send(head_front_joint_list, vel=1.0, acc=1.0)
+def action_drive_comfort(client):
+ # client.send(head_screen_joint_list, vel=1.0, acc=1.0)
+ # client.send(start_joint_list, vel=1.0, acc=1.0)
+ # client.send(set_in_joint_list, vel=3.0, acc=2.0)
+ # client.send(start_joint_list, vel=3.0, acc=2.0)
+ # # #
+ time.sleep(1)
+ # client.send(dirve_mode_joint_list, vel=3.0, acc=2.0)
+ # client.send(start_joint_list, vel=3.0, acc=2.0)
+ #
+ # time.sleep(1)
+ # client.send(dirve_soft_joint_list, vel=3.0, acc=2.0)
+ # client.send(start_joint_list, vel=3.0, acc=2.0)
+ # time.sleep(2)
+ # client.send(go_home_joint_list, vel=3.0, acc=2)
+ # client.send(start_joint_list, vel=3.0, acc=2)
+ # client.send(head_front_joint_list, vel=1.0, acc=1.0)
+
+ # client.send(init_joint_list, vel=1.0, acc=1.0)
+client = MoveJClient()
+hand_client = DexHandClient()
+if __name__ == "__main__":
+ # Initialize client
+
+ # action_air(client)
+ action_hello(client)
+ # client.send(head_front_joint_list, vel=1.0, acc=1.0)
+ # client.send(go_home_joint_list, vel=3.0, acc=3.0)
+ # action_music(client)
+
+ # action_music(client)
+ # action_air(client)
+ # action_one(client)
+ # action_drive_soft(client)
+ # action_drive_economy(client)
+ # action_drive_comfort(client)
+ # client.send(singal_joint_list, vel=1.0, acc=1.0)
+ # client.send(start_joint_list, vel=1.0, acc=1.0)
+ # while True:
+ # client.send(hello_l_joint_list, vel=1.0, acc=1.0)
+ # time.sleep(0.1)
+ # client.send(hello_r_joint_list, vel=1.0, acc=1.0)
+ # time.sleep(0.1)
+ # # hello_l_joint_list
+
+
+
+ # Close client
client.close()
diff --git a/clients/test.py b/clients/test.py
index f424beb..42991ca 100644
--- a/clients/test.py
+++ b/clients/test.py
@@ -1,5 +1,18 @@
-from google.protobuf import timestamp_pb2
+import requests
-timestamp = timestamp_pb2.Timestamp()
-timestamp.GetCurrentTime()
-print(timestamp)
\ No newline at end of file
+
+#netsh advfirewall firewall add rule name="FastAPI 8000" dir=in action=allow protocol=TCP localport=8000
+
+# "http://192.168.0.131:8000/action/one",
+resp = requests.post(
+ "http://127.0.0.1:8000/action/drive_economy",
+ timeout=80000
+)
+
+
+# resp = requests.post(
+# "http://192.168.0.131:8000/action/one",
+# timeout=800000
+# )
+
+print(resp.json())
diff --git a/generated/__pycache__/__init__.cpython-39.pyc b/generated/__pycache__/__init__.cpython-39.pyc
new file mode 100644
index 0000000..4fc2ab7
Binary files /dev/null and b/generated/__pycache__/__init__.cpython-39.pyc differ
diff --git a/generated/cmvr/__pycache__/__init__.cpython-39.pyc b/generated/cmvr/__pycache__/__init__.cpython-39.pyc
new file mode 100644
index 0000000..526278d
Binary files /dev/null and b/generated/cmvr/__pycache__/__init__.cpython-39.pyc differ
diff --git a/generated/cmvr/api/__pycache__/__init__.cpython-39.pyc b/generated/cmvr/api/__pycache__/__init__.cpython-39.pyc
new file mode 100644
index 0000000..df87170
Binary files /dev/null and b/generated/cmvr/api/__pycache__/__init__.cpython-39.pyc differ
diff --git a/generated/cmvr/api/__pycache__/common_pb2.cpython-39.pyc b/generated/cmvr/api/__pycache__/common_pb2.cpython-39.pyc
new file mode 100644
index 0000000..235d56c
Binary files /dev/null and b/generated/cmvr/api/__pycache__/common_pb2.cpython-39.pyc differ
diff --git a/generated/cmvr/api/__pycache__/dexhand_command_pb2.cpython-39.pyc b/generated/cmvr/api/__pycache__/dexhand_command_pb2.cpython-39.pyc
new file mode 100644
index 0000000..ca3379f
Binary files /dev/null and b/generated/cmvr/api/__pycache__/dexhand_command_pb2.cpython-39.pyc differ
diff --git a/generated/cmvr/api/__pycache__/dexhand_service_pb2_grpc.cpython-39.pyc b/generated/cmvr/api/__pycache__/dexhand_service_pb2_grpc.cpython-39.pyc
new file mode 100644
index 0000000..41cc4b1
Binary files /dev/null and b/generated/cmvr/api/__pycache__/dexhand_service_pb2_grpc.cpython-39.pyc differ
diff --git a/generated/cmvr/api/__pycache__/hlc_command_pb2.cpython-39.pyc b/generated/cmvr/api/__pycache__/hlc_command_pb2.cpython-39.pyc
new file mode 100644
index 0000000..0f420ba
Binary files /dev/null and b/generated/cmvr/api/__pycache__/hlc_command_pb2.cpython-39.pyc differ
diff --git a/generated/cmvr/api/__pycache__/humanoid_robot_command_pb2.cpython-39.pyc b/generated/cmvr/api/__pycache__/humanoid_robot_command_pb2.cpython-39.pyc
new file mode 100644
index 0000000..14150d6
Binary files /dev/null and b/generated/cmvr/api/__pycache__/humanoid_robot_command_pb2.cpython-39.pyc differ
diff --git a/generated/cmvr/api/__pycache__/humanoid_robot_service_pb2_grpc.cpython-39.pyc b/generated/cmvr/api/__pycache__/humanoid_robot_service_pb2_grpc.cpython-39.pyc
new file mode 100644
index 0000000..93203b0
Binary files /dev/null and b/generated/cmvr/api/__pycache__/humanoid_robot_service_pb2_grpc.cpython-39.pyc differ