104 lines
3.1 KiB
Python
104 lines
3.1 KiB
Python
|
import argparse
|
||
|
|
import sys
|
||
|
|
|
||
|
|
from clients._path_setup import ensure_paths
|
||
|
|
|
||
|
|
ensure_paths()
|
||
|
|
|
||
|
|
|
||
|
|
def parse_args() -> argparse.Namespace:
|
||
|
|
parser = argparse.ArgumentParser(description="Run robot client actions.")
|
||
|
|
parser.add_argument(
|
||
|
|
"action",
|
||
|
|
choices=["movej", "get_pose", "torque_on", "torque_off"],
|
||
|
|
help="Action to run",
|
||
|
|
)
|
||
|
|
parser.add_argument("--host", default="192.168.0.222", help="gRPC server host")
|
||
|
|
parser.add_argument("--port", type=int, default=50052, help="gRPC server port")
|
||
|
|
parser.add_argument("--device-id", default="hc01", help="Device ID")
|
||
|
|
parser.add_argument("--interval", type=float, default=0.05, help="Interval seconds")
|
||
|
|
parser.add_argument("--base-link", default="PELVIS_S", help="Base link for pose")
|
||
|
|
parser.add_argument("--ee-link", default="R_FINGER_TIP", help="End-effector link for pose")
|
||
|
|
parser.add_argument("--vel", type=float, default=1.0, help="MoveJ velocity")
|
||
|
|
parser.add_argument("--acc", type=float, default=0.5, help="MoveJ acceleration")
|
||
|
|
parser.add_argument(
|
||
|
|
"--joint-list",
|
||
|
|
default="",
|
||
|
|
help="MoveJ joint list format: JOINT:RAD;JOINT:RAD",
|
||
|
|
)
|
||
|
|
return parser.parse_args()
|
||
|
|
|
||
|
|
|
||
|
|
def main() -> int:
|
||
|
|
args = parse_args()
|
||
|
|
address = f"{args.host}:{args.port}"
|
||
|
|
|
||
|
|
if args.action == "movej":
|
||
|
|
from clients.movej_client import MoveJClient, test_joint_list
|
||
|
|
|
||
|
|
joint_list = test_joint_list
|
||
|
|
if args.joint_list:
|
||
|
|
parsed = []
|
||
|
|
for item in args.joint_list.split(";"):
|
||
|
|
if not item:
|
||
|
|
continue
|
||
|
|
if ":" not in item:
|
||
|
|
continue
|
||
|
|
name, rad_str = item.split(":", 1)
|
||
|
|
try:
|
||
|
|
rad = float(rad_str)
|
||
|
|
except ValueError:
|
||
|
|
rad = 0.0
|
||
|
|
parsed.append({"joint_name": name, "rad": rad})
|
||
|
|
if parsed:
|
||
|
|
joint_list = parsed
|
||
|
|
|
||
|
|
client = MoveJClient(address=address)
|
||
|
|
try:
|
||
|
|
client.send(joint_list, vel=args.vel, acc=args.acc, device_id=args.device_id)
|
||
|
|
print(joint_list)
|
||
|
|
finally:
|
||
|
|
client.close()
|
||
|
|
return 0
|
||
|
|
|
||
|
|
if args.action == "get_pose":
|
||
|
|
from clients.get_pose_client import GetPoseClient
|
||
|
|
|
||
|
|
client = GetPoseClient(address=address)
|
||
|
|
try:
|
||
|
|
client.send(
|
||
|
|
interval=args.interval,
|
||
|
|
device_id=args.device_id,
|
||
|
|
base_link=args.base_link,
|
||
|
|
ee_link=args.ee_link,
|
||
|
|
)
|
||
|
|
finally:
|
||
|
|
client.close()
|
||
|
|
return 0
|
||
|
|
|
||
|
|
if args.action == "torque_on":
|
||
|
|
from clients.torque_on_client import TorqueOnClient
|
||
|
|
|
||
|
|
client = TorqueOnClient(address=address)
|
||
|
|
try:
|
||
|
|
client.send(device_id=args.device_id)
|
||
|
|
finally:
|
||
|
|
client.close()
|
||
|
|
return 0
|
||
|
|
|
||
|
|
if args.action == "torque_off":
|
||
|
|
from clients.torque_off_client import TorqueOffClient
|
||
|
|
|
||
|
|
client = TorqueOffClient(address=address)
|
||
|
|
try:
|
||
|
|
client.send(device_id=args.device_id)
|
||
|
|
finally:
|
||
|
|
client.close()
|
||
|
|
return 0
|
||
|
|
|
||
|
|
return 1
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
sys.exit(main())
|