feat:add http server

.idea/grpc_client.iml

@@ -4,7 +4,7 @@
     <content url="file://$MODULE_DIR$">
       <excludeFolder url="file://$MODULE_DIR$/.venv" />
     </content>
-    <orderEntry type="jdk" jdkName="Python 3.10" jdkType="Python SDK" />
+    <orderEntry type="inheritedJdk" />
     <orderEntry type="sourceFolder" forTests="false" />
   </component>
 </module>

.idea/misc.xml

@@ -3,5 +3,5 @@
   <component name="Black">
     <option name="sdkName" value="Python 3.8 (grpc_client)" />
   </component>
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.10" project-jdk-type="Python SDK" />
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.9" project-jdk-type="Python SDK" />
 </project>

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
+from pathlib import Path
-sys.path.append("/home/lgv/cmvr/0-workspace/grpc_client/generated/cmvr")  # 指向 cmvr 顶层
+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
 

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
+                # ====== 1. 同时取 position 和 velocity ======
-                joint_order = [
+                joint_pos_dict = {}
-                    "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
+                joint_vel_dict = {}
-                    "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 = {}
                 for state in resp.state:
-                    for name, pos in zip(state.name, state.position):
+                    # 这里就用你说的三元 zip
-                        joint_dict[name] = round(pos, 12)
+                    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)}
+                # ====== 2. 记录到历史数据里，用于之后画图 ======
-                              for name in joint_order]
+                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()

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": 4, "value": 0.0},
-        {"id": 5, "value": 0.0},
+        {"id": 5, "value": 1.0},
     ]
 
 hand_open = [
-        {"id": 0, "value": 0.8},
+        {"id": 0, "value": 0.7},
-        {"id": 1, "value": 0.8},
+        {"id": 1, "value": 0.7},
-        {"id": 2, "value": 0.8},
+        {"id": 2, "value": 0.7},
-        {"id": 3, "value": 0.8},
+        {"id": 3, "value": 0.7},
-        {"id": 4, "value": 0.8},
+        {"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

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
+    )

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()
+#
+#

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]
+        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
-        ]
 
         # 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},
+init_joint_list = [
-    {'joint_name': 'R_WRIST_Y', 'rad': 0},
+{'joint_name': 'R_SHOULDER_P', 'rad': -0.062633119026},
-    {'joint_name': 'R_WRIST_P', 'rad': 0},
+{'joint_name': 'R_SHOULDER_R', 'rad': -0.013667237243},
-    {'joint_name': 'R_ELBOW_R', 'rad': 0},
+{'joint_name': 'R_SHOULDER_Y', 'rad': 2.563999526183},
-    {'joint_name': 'R_SHOULDER_Y', 'rad': 0},
+{'joint_name': 'R_ELBOW_R', 'rad': -0.275263170082},
-    {'joint_name': 'R_SHOULDER_R', 'rad': 0},
+{'joint_name': 'R_WRIST_P', 'rad': 0.21726806475},
-    {'joint_name': 'R_SHOULDER_P', 'rad': 0},
+{'joint_name': 'R_WRIST_Y', 'rad': -0.044427538871},
+{'joint_name': 'R_WRIST_R', 'rad': -0.065047050427},
 ]
 
 start_joint_list = [
-
+{'joint_name': 'R_SHOULDER_P', 'rad': 0.212733518819},
-    {'joint_name': 'R_WRIST_R', 'rad': -0.182982265197},
+{'joint_name': 'R_SHOULDER_R', 'rad': -0.391965314899},
-    {'joint_name': 'R_WRIST_Y', 'rad': 0.057211977764},
+{'joint_name': 'R_SHOULDER_Y', 'rad': 2.564591855398},
-    {'joint_name': 'R_WRIST_P', 'rad': -2.450283707826},
+{'joint_name': 'R_ELBOW_R', 'rad': -1.503521397093},
-    {'joint_name': 'R_ELBOW_R', 'rad': 1.691315387916},
+{'joint_name': 'R_WRIST_P', 'rad': 0.217252876821},
-    {'joint_name': 'R_SHOULDER_Y', 'rad': 1.741483963795},
+{'joint_name': 'R_WRIST_Y', 'rad': 0.164711187099},
-    {'joint_name': 'R_SHOULDER_R', 'rad': 1.000908225412},
+{'joint_name': 'R_WRIST_R', 'rad': 0.227967960443},
-    {'joint_name': 'R_SHOULDER_P', 'rad': -0.275362840863},
 ]
 
-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},
+air_open_joint_list= [
-    {'joint_name': 'R_WRIST_Y', 'rad': 0.133274073403},
+{'joint_name': 'R_SHOULDER_P', 'rad': 0.562264710474},
-    {'joint_name': 'R_WRIST_P', 'rad': -2.832957807407},
+{'joint_name': 'R_SHOULDER_R', 'rad': -0.375382944612},
-    {'joint_name': 'R_ELBOW_R', 'rad': 0.722278081411},
+{'joint_name': 'R_SHOULDER_Y', 'rad': 2.390867581924},
-    {'joint_name': 'R_SHOULDER_Y', 'rad': 1.454527987188},
+{'joint_name': 'R_ELBOW_R', 'rad': -1.143580778878},
-    {'joint_name': 'R_SHOULDER_R', 'rad': 1.319798720377},
+{'joint_name': 'R_WRIST_P', 'rad': 0.19485637762},
-    {'joint_name': 'R_SHOULDER_P', 'rad': 0.886766195809},
+{'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},
+]
+
+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},
+]
+
+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},
+    ]
+
+
+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
-    client = MoveJClient()
+
+    # 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
 
 
 
-    R_WRIST_R_joint = [
+    # Close client
-        {'joint_name': 'R_WRIST_R', 'rad': 0.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)
-
     client.close()

clients/test.py

@@ -1,5 +1,18 @@
-from google.protobuf import timestamp_pb2
+import requests
 
-timestamp = timestamp_pb2.Timestamp()
+
-timestamp.GetCurrentTime()
+#netsh advfirewall firewall add rule name="FastAPI 8000" dir=in action=allow protocol=TCP localport=8000
-print(timestamp)
+
+# "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())