117 lines
3.9 KiB
Python
117 lines
3.9 KiB
Python
import sys
|
|
from google.protobuf import timestamp_pb2
|
|
|
|
sys.path.append("../generated")
|
|
|
|
from generated.cmvr.api import humanoid_robot_command_pb2 as pb
|
|
from generated.cmvr.api import common_pb2
|
|
from generated.cmvr.api import humanoid_robot_service_pb2_grpc as rpc
|
|
from clients.base_client import RobotClientBase
|
|
import math
|
|
from typing import List, Dict, Literal
|
|
|
|
|
|
|
|
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)
|
|
|
|
|
|
|
|
|
|
|
|
if __name__ == "__main__":
|
|
# Initialize client
|
|
client = MoveJClient()
|
|
|
|
end_joint_list = [
|
|
|
|
{'joint_name': 'R_WRIST_R', 'rad': 0.32396421236},
|
|
{'joint_name': 'R_WRIST_Y', 'rad': 0.098841140792},
|
|
{'joint_name': 'R_WRIST_P', 'rad': -17.626296421526},
|
|
{'joint_name': 'R_ELBOW_R', 'rad': 1.502654735917},
|
|
{'joint_name': 'R_SHOULDER_Y', 'rad': -4.76186081004},
|
|
{'joint_name': 'R_SHOULDER_R', 'rad': -0.484371520195},
|
|
{'joint_name': 'R_SHOULDER_P', 'rad': 0.154464081527},
|
|
]
|
|
|
|
|
|
start_joint_list = [
|
|
{'joint_name': 'R_WRIST_R', 'rad': 0.315772223376},
|
|
{'joint_name': 'R_WRIST_Y', 'rad': 0.08855986238},
|
|
{'joint_name': 'R_WRIST_P', 'rad': -17.291334250373},
|
|
{'joint_name': 'R_ELBOW_R', 'rad': 1.66502982975},
|
|
{'joint_name': 'R_SHOULDER_Y', 'rad': -4.758606796339},
|
|
{'joint_name': 'R_SHOULDER_R', 'rad': -0.830841394993},
|
|
{'joint_name': 'R_SHOULDER_P', 'rad': -0.007418353872},
|
|
]
|
|
|
|
# client.send(start_joint_list, vel=2.0, acc=1.0)
|
|
while True:
|
|
client.send(start_joint_list, vel=2.0, acc=1.0)
|
|
client.send(end_joint_list, vel=1.5, acc=1.5)
|
|
# client.send(end_joint_list, vel=1.5, acc=1.5)
|
|
|
|
# Send MoveJ command
|
|
# client.send(start, vel=0.8, acc=0.8)
|
|
# client.send(end, vel=0.8, acc=0.8)
|
|
# client.send(left_arm, vel=head_speed, acc=0.8)
|
|
# while True:
|
|
# client.send(action_hello_start, vel=5.0, acc=0.8)
|
|
# client.send(action_hello_end, vel=5.0, acc=0.8)
|
|
# client.send(action_touch_start, vel=1.5, acc=0.8)
|
|
# client.send(action_touch_set, vel=1.5, acc=0.8)
|
|
# client.send(action_touch_start, vel=1.5, acc=0.8)
|
|
# client.send(head_dd, vel=1.5, acc=0.8)
|
|
# client.send(joint_list, vel=1.0, acc=0.8)
|
|
# while True:
|
|
# client.send(action_hello_start,vel=5.0, acc=0.8)
|
|
# client.send(action_hello_end,vel=5.0, acc=0.8)
|
|
# client.send(action_hello_start, vel=5.0, acc=0.8)
|
|
|
|
|
|
# Close client
|
|
client.close()
|