diff --git a/actions.json b/actions.json new file mode 100644 index 0000000..8c740b8 --- /dev/null +++ b/actions.json @@ -0,0 +1,882 @@ +{ + "action_one": { + "1_start_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.274967005474, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.603120933284, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.499983558976, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.492936360109, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.720539063587, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.056068136892, + "vel": 1.0 + } + ] + }, + "2_wait": 2, + "3_set_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.066444339857, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.531186157241, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.340070808067, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.952500297438, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.626298915913, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.342241935176, + "vel": 1.0 + } + ] + }, + "4_start_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.274967005474, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.603120933284, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.499983558976, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.492936360109, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.720539063587, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.056068136892, + "vel": 1.0 + } + ] + }, + "5_wait": 2, + "6_drive_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": -0.011299818873, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.60070130641, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.364414209864, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.980264780151, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.560519997177, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.442999602707, + "vel": 1.0 + } + ] + }, + "7_start_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.274967005474, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.603120933284, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.499983558976, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.492936360109, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.720539063587, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.056068136892, + "vel": 1.0 + } + ] + }, + "8_mode_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.274796141277, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.944670868464, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.136851577265, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.543831108824, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.465282192444, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.415577797633, + "vel": 1.0 + } + ] + }, + "9_start_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.274967005474, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.603120933284, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.499983558976, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.492936360109, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.720539063587, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.056068136892, + "vel": 1.0 + } + ] + }, + "10_home_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.031161140931, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.577257789382, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.381942977952, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -2.055649114477, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.602941780228, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.314926445602, + "vel": 1.0 + } + ] + } + }, + "action_two": { + "1_start_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.274967005474, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.603120933284, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.499983558976, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.492936360109, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.720539063587, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.056068136892, + "vel": 1.0 + } + ] + }, + "2_wait": 2.0, + "3_wyy_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.319859674658, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.645610112769, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.129972394858, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.724514301329, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.557216622708, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.379009062563, + "vel": 1.0 + } + ] + }, + "4_start_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.274967005474, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.603120933284, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.499983558976, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.492936360109, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.720539063587, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.056068136892, + "vel": 1.0 + } + ] + }, + "5_wait": 2.0, + "6_play_music_joint_list": { + "acc": 3.0, + "vel": 3.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.0, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.446466942304, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.205457349211, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.959358596443, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.776803694305, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.372088113353, + "vel": 1.0 + } + ] + }, + "7_start_joint_list": { + "acc": 3.0, + "vel": 3.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.274967005474, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.603120933284, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.499983558976, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.492936360109, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.720539063587, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.056068136892, + "vel": 1.0 + } + ] + }, + "8_wait": 2.0, + "9_play_music_joint_list": { + "acc": 3.0, + "vel": 3.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.0, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.446466942304, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.205457349211, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.959358596443, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.776803694305, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.372088113353, + "vel": 1.0 + } + ] + }, + "10_start_joint_list": { + "acc": 3.0, + "vel": 3.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.274967005474, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.603120933284, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.499983558976, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.492936360109, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.720539063587, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.056068136892, + "vel": 1.0 + } + ] + }, + "11_wait": 2.0, + "12_home_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.031161140931, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.577257789382, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.381942977952, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -2.055649114477, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.602941780228, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.314926445602, + "vel": 1.0 + } + ] + } + }, + "action_three": { + "1_start_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.274967005474, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.603120933284, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.499983558976, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.492936360109, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.720539063587, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.056068136892, + "vel": 1.0 + } + ] + }, + "2_wait": 2.0, + "3_empty_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.065426748642, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.572653948527, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.354382583029, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.857614662799, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.490473270507, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.343988546965, + "vel": 1.0 + } + ] + }, + "4_start_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.274967005474, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.603120933284, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.499983558976, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.492936360109, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.720539063587, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.056068136892, + "vel": 1.0 + } + ] + }, + "5_wait": 2.0, + "6_empty_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.065426748642, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.572653948527, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.354382583029, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.857614662799, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.490473270507, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.343988546965, + "vel": 1.0 + } + ] + }, + "7_start_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.274967005474, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.603120933284, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.499983558976, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.492936360109, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.720539063587, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.056068136892, + "vel": 1.0 + } + ] + }, + "8_wait": 2.0, + "9_nav_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.057895434551, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.659321964551, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.348938659874, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.969583869368, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.586725818719, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.35676823963, + "vel": 1.0 + } + ] + }, + "10_start_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.274967005474, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.603120933284, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.499983558976, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -1.492936360109, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.720539063587, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.056068136892, + "vel": 1.0 + } + ] + }, + "11_wait": 7.0, + "12_home_joint_list": { + "acc": 5.0, + "vel": 5.0, + "cmds": [ + { + "jointName": "R_WRIST_Y", + "rad": 0.031161140931, + "vel": 1.0 + }, + { + "jointName": "R_WRIST_P", + "rad": -20.577257789382, + "vel": 1.0 + }, + { + "jointName": "R_ELBOW_R", + "rad": -1.381942977952, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_Y", + "rad": -2.055649114477, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_R", + "rad": -0.602941780228, + "vel": 1.0 + }, + { + "jointName": "R_SHOULDER_P", + "rad": 0.314926445602, + "vel": 1.0 + } + ] + } + } +} \ No newline at end of file diff --git a/clients/get_joint_state_client.py b/clients/get_joint_state_client.py index 4c18037..f0d8d00 100644 --- a/clients/get_joint_state_client.py +++ b/clients/get_joint_state_client.py @@ -40,13 +40,6 @@ class GetJointStateClient(RobotClientBase): "HEAD_P", "HEAD_Y","HEAD_R" ] - joint_order = [ - "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", - "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R", - - "R_WRIST_R","R_WRIST_Y", "R_WRIST_P", "R_ELBOW_R", "R_SHOULDER_Y","R_SHOULDER_R", "R_SHOULDER_P", - "HEAD_P", "HEAD_Y","HEAD_R" - ] # Map response to dictionary joint_dict = {} diff --git a/clients/movej_client.py b/clients/movej_client.py index fd4ec2a..fda57e2 100644 --- a/clients/movej_client.py +++ b/clients/movej_client.py @@ -1,4 +1,6 @@ import sys +import time + from google.protobuf import timestamp_pb2 sys.path.append("../generated") @@ -11,7 +13,6 @@ import math from typing import List, Dict, Literal - class MoveJClient(RobotClientBase): """Client to send MoveJ commands to the robot""" @@ -28,7 +29,20 @@ 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() @@ -59,6 +73,38 @@ class MoveJClient(RobotClientBase): print("MoveJ RPC call failed:", e) +zero_joint_list = [ + + {'joint_name': 'R_WRIST_R', 'rad': 0}, + {'joint_name': 'R_WRIST_Y', 'rad': 0}, + {'joint_name': 'R_WRIST_P', 'rad': 0}, + {'joint_name': 'R_ELBOW_R', 'rad': 0}, + {'joint_name': 'R_SHOULDER_Y', 'rad': 0}, + {'joint_name': 'R_SHOULDER_R', 'rad': 0}, + {'joint_name': 'R_SHOULDER_P', 'rad': 0}, +] + +start_joint_list = [ + + {'joint_name': 'R_WRIST_R', 'rad': -0.182982265197}, + {'joint_name': 'R_WRIST_Y', 'rad': 0.057211977764}, + {'joint_name': 'R_WRIST_P', 'rad': -2.450283707826}, + {'joint_name': 'R_ELBOW_R', 'rad': 1.691315387916}, + {'joint_name': 'R_SHOULDER_Y', 'rad': 1.741483963795}, + {'joint_name': 'R_SHOULDER_R', 'rad': 1.000908225412}, + {'joint_name': 'R_SHOULDER_P', 'rad': -0.275362840863}, +] + +end_joint_list = [ + + {'joint_name': 'R_WRIST_R', 'rad': 0.590444918686}, + {'joint_name': 'R_WRIST_Y', 'rad': 0.133274073403}, + {'joint_name': 'R_WRIST_P', 'rad': -2.832957807407}, + {'joint_name': 'R_ELBOW_R', 'rad': 0.722278081411}, + {'joint_name': 'R_SHOULDER_Y', 'rad': 1.454527987188}, + {'joint_name': 'R_SHOULDER_R', 'rad': 1.319798720377}, + {'joint_name': 'R_SHOULDER_P', 'rad': 0.886766195809}, +] @@ -66,51 +112,17 @@ 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}, + + R_WRIST_R_joint = [ + {'joint_name': 'R_WRIST_R', 'rad': 0.0}, ] - - 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) + # 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, 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) + client.send(start_joint_list,1.5,1.5) + client.send(end_joint_list,1.5,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() diff --git a/generate_json.py b/generate_json.py new file mode 100644 index 0000000..b6f447f --- /dev/null +++ b/generate_json.py @@ -0,0 +1,298 @@ +import json +import time + +# ===== 关节列表 ===== + +set_joint_list = [ + + {'joint_name': 'R_WRIST_Y', 'rad': 0.066444339857}, + {'joint_name': 'R_WRIST_P', 'rad': -20.531186157241}, + {'joint_name': 'R_ELBOW_R', 'rad': -1.340070808067}, + {'joint_name': 'R_SHOULDER_Y', 'rad': -1.952500297438}, + {'joint_name': 'R_SHOULDER_R', 'rad': -0.626298915913}, + {'joint_name': 'R_SHOULDER_P', 'rad': 0.342241935176}, +] + +drive_joint_list = [ + + {'joint_name': 'R_WRIST_Y', 'rad': -0.011299818873}, + {'joint_name': 'R_WRIST_P', 'rad': -20.60070130641}, + {'joint_name': 'R_ELBOW_R', 'rad': -1.364414209864}, + {'joint_name': 'R_SHOULDER_Y', 'rad': -1.980264780151}, + {'joint_name': 'R_SHOULDER_R', 'rad': -0.560519997177}, + {'joint_name': 'R_SHOULDER_P', 'rad': 0.442999602707}, +] + +mode_joint_list = [ + + {'joint_name': 'R_WRIST_Y', 'rad': 0.274796141277}, + {'joint_name': 'R_WRIST_P', 'rad': -20.944670868464}, + {'joint_name': 'R_ELBOW_R', 'rad': -1.136851577265}, + {'joint_name': 'R_SHOULDER_Y', 'rad': -1.543831108824}, + {'joint_name': 'R_SHOULDER_R', 'rad': -0.465282192444}, + {'joint_name': 'R_SHOULDER_P', 'rad': 0.415577797633}, +] + +home_joint_list = [ + {'joint_name': 'R_WRIST_Y', 'rad': 0.031161140931}, + {'joint_name': 'R_WRIST_P', 'rad': -20.577257789382}, + {'joint_name': 'R_ELBOW_R', 'rad': -1.381942977952}, + {'joint_name': 'R_SHOULDER_Y', 'rad': -2.055649114477}, + {'joint_name': 'R_SHOULDER_R', 'rad': -0.602941780228}, + {'joint_name': 'R_SHOULDER_P', 'rad': 0.314926445602}, +] + +start_joint_list = [ + + {'joint_name': 'R_WRIST_Y', 'rad': 0.274967005474}, + {'joint_name': 'R_WRIST_P', 'rad': -20.603120933284}, + {'joint_name': 'R_ELBOW_R', 'rad': -1.499983558976}, + {'joint_name': 'R_SHOULDER_Y', 'rad': -1.492936360109}, + {'joint_name': 'R_SHOULDER_R', 'rad': -0.720539063587}, + {'joint_name': 'R_SHOULDER_P', 'rad': 0.056068136892}, +] +# start_joint_list = [ +# +# {'joint_name': 'R_WRIST_Y', 'rad': 0.182186797436}, +# {'joint_name': 'R_WRIST_P', 'rad': -20.304780654951}, +# {'joint_name': 'R_ELBOW_R', 'rad': -1.535033501186}, +# {'joint_name': 'R_SHOULDER_Y', 'rad': -1.596287366447}, +# {'joint_name': 'R_SHOULDER_R', 'rad': -1.034838057889}, +# {'joint_name': 'R_SHOULDER_P', 'rad': -0.0965316264}, +# ] + +wyy_joint_list = [ +{'joint_name': 'R_WRIST_Y', 'rad': 0.319859674658}, +{'joint_name': 'R_WRIST_P', 'rad': -20.645610112769}, +{'joint_name': 'R_ELBOW_R', 'rad': -1.129972394858}, +{'joint_name': 'R_SHOULDER_Y', 'rad': -1.724514301329}, +{'joint_name': 'R_SHOULDER_R', 'rad': -0.557216622708}, +{'joint_name': 'R_SHOULDER_P', 'rad': 0.379009062563}, +] + + +play_music_joint_list = [ + + {'joint_name': 'R_WRIST_Y', 'rad': 0.00}, + {'joint_name': 'R_WRIST_P', 'rad': -20.446466942304}, + {'joint_name': 'R_ELBOW_R', 'rad': -1.205457349211}, + {'joint_name': 'R_SHOULDER_Y', 'rad': -1.959358596443}, + {'joint_name': 'R_SHOULDER_R', 'rad': -0.776803694305}, + {'joint_name': 'R_SHOULDER_P', 'rad': 0.372088113353}, +] + + +air_joint_list = [ + + {'joint_name': 'R_WRIST_Y', 'rad': 0.368498066731}, + {'joint_name': 'R_WRIST_P', 'rad': -20.806707522108}, + {'joint_name': 'R_ELBOW_R', 'rad': -1.160211560687}, + {'joint_name': 'R_SHOULDER_Y', 'rad': -1.754356682523}, + {'joint_name': 'R_SHOULDER_R', 'rad': -0.652379437044}, + {'joint_name': 'R_SHOULDER_P', 'rad': 0.325288409885}, +] + +empty_joint_list = [ +{'joint_name': 'R_WRIST_Y', 'rad': 0.065426748642}, +{'joint_name': 'R_WRIST_P', 'rad': -20.572653948527}, +{'joint_name': 'R_ELBOW_R', 'rad': -1.354382583029}, +{'joint_name': 'R_SHOULDER_Y', 'rad': -1.857614662799}, +{'joint_name': 'R_SHOULDER_R', 'rad': -0.490473270507}, +{'joint_name': 'R_SHOULDER_P', 'rad': 0.343988546965}, +] + + +nav_joint_list = [ + + {'joint_name': 'R_WRIST_Y', 'rad': 0.057895434551}, + {'joint_name': 'R_WRIST_P', 'rad': -20.659321964551}, + {'joint_name': 'R_ELBOW_R', 'rad': -1.348938659874}, + {'joint_name': 'R_SHOULDER_Y', 'rad': -1.969583869368}, + {'joint_name': 'R_SHOULDER_R', 'rad': -0.586725818719}, + {'joint_name': 'R_SHOULDER_P', 'rad': 0.35676823963}, +] + + +air_key_joint_list = [ + + {'joint_name': 'R_WRIST_Y', 'rad': 0.394130543969}, + {'joint_name': 'R_WRIST_P', 'rad': -20.514247819877}, + {'joint_name': 'R_ELBOW_R', 'rad': -1.122382227544}, + {'joint_name': 'R_SHOULDER_Y', 'rad': -1.903357855984}, + {'joint_name': 'R_SHOULDER_R', 'rad': -0.756327518825}, + {'joint_name': 'R_SHOULDER_P', 'rad': 0.422019377847}, +] + +def action_one(): + while True: + client.send(start_joint_list, vel=5.0, acc=5.0) + time.sleep(2) + client.send(set_joint_list, vel=5.0, acc=5.0) + client.send(start_joint_list, vel=5.0, acc=5.0) + time.sleep(2) + client.send(drive_joint_list, vel=5.0, acc=5.0) + client.send(start_joint_list, vel=5.0, acc=5.0) + client.send(mode_joint_list, vel=5.0, acc=5.0) + client.send(start_joint_list, vel=5.0, acc=5.0) + client.send(home_joint_list, vel=5.0, acc=5.0) + +def action_two(): + while True: + client.send(start_joint_list, vel=5.0, acc=5.0) + time.sleep(2.0) + client.send(wyy_joint_list, vel=5.0, acc=5.0) + client.send(start_joint_list, vel=5.0, acc=5.0) + time.sleep(2.0) + client.send(play_music_joint_list, vel=3.0, acc=3.0) + client.send(start_joint_list, vel=3.0, acc=3.0) + time.sleep(2.0) + client.send(play_music_joint_list, vel=3.0, acc=3.0) + client.send(start_joint_list, vel=3.0, acc=3.0) + time.sleep(2.0) + client.send(home_joint_list, vel=5.0, acc=5.0) + +def action_three(): + while True: + client.send(start_joint_list, vel=5.0, acc=5.0) + time.sleep(2.0) + client.send(empty_joint_list, vel=5.0, acc=5.0) + client.send(start_joint_list, vel=5.0, acc=5.0) + time.sleep(2.0) + client.send(empty_joint_list, vel=5.0, acc=5.0) + client.send(start_joint_list, vel=5.0, acc=5.0) + time.sleep(2.0) + client.send(nav_joint_list, vel=5.0, acc=5.0) + client.send(start_joint_list, vel=5.0, acc=5.0) + time.sleep(7.0) + client.send(home_joint_list, vel=5.0, acc=5.0) + + +# ===== 录制工具 ===== +JOINT_LISTS = { + 'set_joint_list': set_joint_list, + 'drive_joint_list': drive_joint_list, + 'mode_joint_list': mode_joint_list, + 'home_joint_list': home_joint_list, + 'start_joint_list': start_joint_list, + 'wyy_joint_list': wyy_joint_list, + 'play_music_joint_list': play_music_joint_list, + 'air_joint_list': air_joint_list, + 'air_key_joint_list': air_key_joint_list, + 'empty_joint_list': empty_joint_list, + 'nav_joint_list': nav_joint_list, +} + +def find_joint_list_name(lst): + for name, value in JOINT_LISTS.items(): + if value is lst: + return name + return None + + +class StopRecording(Exception): + pass + + +class RecordingClient: + def __init__(self, max_steps=100): + self.max_steps = max_steps + self.steps = [] + self._send_count = 0 + + def send(self, joint_list, vel=3.0, acc=3.0): + name = find_joint_list_name(joint_list) + self.steps.append(("move", name, vel, acc)) + self._send_count += 1 + + # ① 安全兜底：防止真死循环 + if self._send_count >= self.max_steps: + raise StopRecording + + # ② 关键：第一次回到 home 就结束一轮动作 + if name == "home_joint_list": + raise StopRecording + + def sleep(self, seconds): + self.steps.append(("wait", seconds)) + + + +def record_action(func, max_steps=50): + global client + + real_client = client + real_sleep = time.sleep + + rec_client = RecordingClient(max_steps=max_steps) + + try: + client = rec_client + time.sleep = rec_client.sleep + func() + except StopRecording: + pass + finally: + client = real_client + time.sleep = real_sleep + + return rec_client.steps + + +def joints_to_cmds(joint_list, default_vel=1.0): + return [ + { + "jointName": j["joint_name"], + "rad": j["rad"], + "vel": default_vel + } + for j in joint_list + ] + + +def steps_to_json_obj(steps): + obj = {} + idx = 1 + for s in steps: + kind = s[0] + if kind == "move": + _, list_name, vel, acc = s + key = f"{idx}_{list_name}" + joint_list = JOINT_LISTS[list_name] + obj[key] = { + "acc": acc, + "vel": vel, + "cmds": joints_to_cmds(joint_list, default_vel=1.0), + } + elif kind == "wait": + _, seconds = s + key = f"{idx}_wait" + obj[key] = seconds + idx += 1 + return obj + + +# ===== 主入口：录制并保存到文件 ===== +if __name__ == "__main__": + # 占位真实 client（防止 record_action 里引用失败） + class DummyClient: + def send(self, *args, **kwargs): + pass + + client = DummyClient() + + # 分别录制三个动作 + steps_one = record_action(action_one, max_steps=20) + steps_two = record_action(action_two, max_steps=20) + steps_three = record_action(action_three, max_steps=20) + + data = { + "action_one": steps_to_json_obj(steps_one), + "action_two": steps_to_json_obj(steps_two), + "action_three": steps_to_json_obj(steps_three), + } + + # ✅ 保存到文件 + with open("actions.json", "w", encoding="utf-8") as f: + json.dump(data, f, indent=2, ensure_ascii=False) + + print("已保存到 actions.json")