lgv/grpc_client
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
6b9bf3be30
grpc_client/clients/movej_client.py
lgv 6b9bf3be30 feat:add readme.txt
2026-02-03 13:36:52 +08:00

402 lines
14 KiB
Python
Raw Blame History

import math
import time
from typing import Dict, List, Literal
from clients._path_setup import ensure_paths
ensure_paths()
from google.protobuf import timestamp_pb2
from clients.base_client import RobotClientBase
from clients.hand_client import DexHandClient
from cmvr.api import common_pb2
from cmvr.api import humanoid_robot_command_pb2 as pb
from cmvr.api import humanoid_robot_service_pb2_grpc as rpc
class MoveJClient(RobotClientBase):
"""Client to send MoveJ commands to the robot"""
def send(self, joint_list, vel=1.0, acc=0.5, device_id="hc01"):
"""
joint_list: list of dicts, e.g.,
[
{"joint_name": "L_SHOULDER_P", "rad": 0.0},
{"joint_name": "L_SHOULDER_R", "rad": -1.31873},
...
]
vel: velocity
acc: acceleration
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]
# Construct request header
header = common_pb2.CommandHeader.Request()
header.device_id = device_id
ts = timestamp_pb2.Timestamp()
ts.GetCurrentTime()
header.timestamp.CopyFrom(ts)
# Construct MoveJ request
req = pb.MoveJ.Request(
header=header,
vel=vel,
acc=acc,
cmds=cmds
)
# Call RPC
try:
resp = self.stub.moveJ(req, timeout=10)
success = getattr(resp.header, "success", None)
error_msg = getattr(resp.header, "error_message", "")
timestamp_sec = getattr(resp.header.timestamp, "seconds", 0)
print("MoveJ RPC call succeeded")
print(f"Success: {success}")
print(f"Error message: {error_msg}")
print(f"Timestamp: {timestamp_sec}")
except Exception as e:
print("MoveJ RPC call failed:", e)
# 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},
# ]
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_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},
]
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},
]
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},
]
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': 'L_SHOULDER_R', 'rad': 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},
]
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},
]
test_joint_list = [
# {'joint_name': 'R_SHOULDER_P', 'rad':0},
{'joint_name': 'R_SHOULDER_R', 'rad': 0},
# {'joint_name': 'R_SHOULDER_Y', 'rad': 0},
# {'joint_name': 'R_ELBOW_R', 'rad':0},
# {'joint_name': 'R_WRIST_P', 'rad': 0},
# {'joint_name': 'R_WRIST_Y', 'rad': 0},
# {'joint_name': 'R_WRIST_R', 'rad': 0},
]
#
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(test_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()
Reference in New Issue View Git Blame Copy Permalink