112 lines
2.8 KiB
Python
112 lines
2.8 KiB
Python
|
import sys
|
||
|
|
from google.protobuf import timestamp_pb2
|
||
|
|
|
||
|
|
sys.path.append("../generated")
|
||
|
|
|
||
|
|
|
||
|
|
from generated.cmvr.api import dexhand_command_pb2 as pb
|
||
|
|
from generated.cmvr.api import common_pb2
|
||
|
|
from clients.base_client import RobotClientBase
|
||
|
|
|
||
|
|
|
||
|
|
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.1},
|
||
|
|
{"id": 5, "value": 0.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": 5, "value": 1.0},
|
||
|
|
]
|
||
|
|
|
||
|
|
handshake_1 = [
|
||
|
|
{"id": 0, "value": 0.0},
|
||
|
|
{"id": 1, "value": 0.0},
|
||
|
|
{"id": 2, "value": 0.0},
|
||
|
|
{"id": 3, "value": 0.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},
|
||
|
|
]
|
||
|
|
|
||
|
|
class DexHandClient(RobotClientBase):
|
||
|
|
"""DexHand control client"""
|
||
|
|
|
||
|
|
def set_angles(self, angles: list, device_id="hc01"):
|
||
|
|
"""
|
||
|
|
angles: list of dicts, e.g.,
|
||
|
|
[
|
||
|
|
{"id": 0, "value": 0.5},
|
||
|
|
{"id": 1, "value": 0.8},
|
||
|
|
...
|
||
|
|
]
|
||
|
|
device_id: device ID
|
||
|
|
"""
|
||
|
|
# Construct request header
|
||
|
|
header = common_pb2.CommandHeader.Request()
|
||
|
|
header.device_id = device_id
|
||
|
|
ts = timestamp_pb2.Timestamp()
|
||
|
|
ts.GetCurrentTime()
|
||
|
|
header.timestamp.CopyFrom(ts)
|
||
|
|
|
||
|
|
# Construct FreedomValue list
|
||
|
|
values = [pb.FreedomValue(id=a["id"], value=a["value"]) for a in angles]
|
||
|
|
|
||
|
|
# Construct request
|
||
|
|
req = pb.SetDexHandAnglesCommand.Request(
|
||
|
|
header=header,
|
||
|
|
values=values
|
||
|
|
)
|
||
|
|
|
||
|
|
# Call RPC
|
||
|
|
try:
|
||
|
|
resp = self.stub_hand.SetDexHandAngle(req, timeout=10)
|
||
|
|
print("SetDexHandAngle RPC call succeeded")
|
||
|
|
print(f"Success: {resp.header.success}")
|
||
|
|
print(f"Error message: {resp.header.error_message}")
|
||
|
|
print(f"Timestamp: {resp.header.timestamp.seconds}")
|
||
|
|
except Exception as e:
|
||
|
|
print("SetDexHandAngle RPC call failed:", e)
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
# Initialize client
|
||
|
|
client = DexHandClient()
|
||
|
|
|
||
|
|
# Example: set finger angles
|
||
|
|
angles = [
|
||
|
|
{"id": 0, "value": 0.0},
|
||
|
|
{"id": 1, "value": 0.0},
|
||
|
|
{"id": 2, "value": 0.0},
|
||
|
|
{"id": 3, "value": 1.0},
|
||
|
|
|
||
|
|
]
|
||
|
|
|
||
|
|
# angles = [
|
||
|
|
#
|
||
|
|
# {"id": 4, "value": 0.0},
|
||
|
|
# {"id": 5, "value": 1.0},
|
||
|
|
#
|
||
|
|
# ]
|
||
|
|
|
||
|
|
# client.set_angles(hand_open, device_id="hand2")
|
||
|
|
client.set_angles(angles, device_id="hand2")
|
||
|
|
# client.set_angles(handshake, device_id="hand1")
|
||
|
|
|
||
|
|
# Close client
|
||
|
|
client.close()
|