# ---
# jupyter:
#   jupytext:
#     text_representation:
#       extension: .py
#       format_name: light
#       format_version: '1.5'
#       jupytext_version: 1.14.4
#   kernelspec:
#     display_name: repl
#     language: python
#     name: repl
# ---

# + pycharm={}
# %run my_import

uri = socket.gethostname()+".local:5555"
#uri = "localhost:5555"
#uri = "com-exp.local:5555"
#uri = "advantech.local:5555"
#uri = "hhcm-mio.local:5555"
io = ZmsgIO(uri)

# Right Front Leg 
RFL = [41,42,43,44,45]
# Left Front Leg 
LFL = [71,72,73,74,75]
# Right Rear Leg 
RRL = [51,52,53,54,55]
# Left Rear Leg 
LRL = [61,62,63,64,65]
# Right Arm 
RA = [11,12,13,14,15,16]
# Left Arm 
LA = [21,22,23,24,25,26]
# wheels
WEE = [46,56,66,76] 

legs_ids = RFL + LFL + RRL + LRL
arms_ids = RA + LA
# all 
robot_ids = legs_ids + arms_ids 
#robot_ids = RA
#robot_ids = list(map(lambda x: x[-1],(RFL,LFL,RRL,LRL)))

legs_pitch_ids = RFL[1:-1] + LFL[1:-1] + RRL[1:-1] + LRL[1:-1]

# Right Arm home 
RA_home = [-1.25, 0.25, 0, 1.25, 0, 0]
# Left Arm home
LA_home = [ 0.75,-0.25, 0,-0.75, 0, 0]

homing = {"type": "TRJ_CMD",
          "trajectory_cmd": {"type": "HOMING", "name": "homing", "board_id": 1,
                             "homing_par": {"x": [0,3]}}}

xs = [0,1,2,3,4]
smooth = {"type": "TRJ_CMD",
          "trajectory_cmd": {"type": "SMOOTHER", "name": "smooth", "board_id": 1,
                             "smooth_par": {"x": xs,
                                            "y": [0,-0.5,0,0.5,0]}
                            }
         } 

smooth_11= {"type": "TRJ_CMD",
             "trajectory_cmd": {"type": "SMOOTHER", "name": "smooth", "board_id": 11,
                                "smooth_par": {"x": xs,
                                               "y": [-1.25,-2.0,-1.25,-0.25,-1.25]}
                               }
            } 
smooth_12 = {"type": "TRJ_CMD",
             "trajectory_cmd": {"type": "SMOOTHER", "name": "smooth", "board_id": 12,
                                "smooth_par": {"x": xs,
                                               "y": [0.25,-0.25,0.25,0.75,0.25]}
                               }
            } 
smooth_14 = {"type": "TRJ_CMD",
             "trajectory_cmd": {"type": "SMOOTHER", "name": "smooth", "board_id": 14,
                                "smooth_par": {"x": xs,
                                               "y": [1.25,0.25,1.25,2.0,1.25]}
                               }
            } 
smooth_21= {"type": "TRJ_CMD",
             "trajectory_cmd": {"type": "SMOOTHER", "name": "smooth", "board_id": 21,
                                "smooth_par": {"x": xs,
                                               "y": [0.75,1.5,0.75,-0.25,0.75]}
                               }
            } 
smooth_22 = {"type": "TRJ_CMD",
             "trajectory_cmd": {"type": "SMOOTHER", "name": "smooth", "board_id": 22,
                                "smooth_par": {"x": xs,
                                               "y": [-0.25,0.25,-0.25,-0.75,-0.25]}
                               }
            } 
smooth_24 = {"type": "TRJ_CMD",
             "trajectory_cmd": {"type": "SMOOTHER", "name": "smooth", "board_id": 24,
                                "smooth_par": {"x": xs,
                                               "y": [-0.75,-1.5,-0.75,0,-0.75]}
                               }
            } 
# 24 -2.5 chiuso 0.35 aperto

leg_pitch_pos = {"type": "TRJ_CMD",
                 "trajectory_cmd": {"type": "SMOOTHER",
                                    "name": "leg_pitch_pos",
                                    "board_id": 1,
                                    "smooth_par": {"x": [0,  1, 2],
                                                   "y": [0,1.0, 0]}}} 

leg_pitch_neg = {"type": "TRJ_CMD",
                 "trajectory_cmd": {"type": "SMOOTHER",
                                    "name": "leg_pitch_neg",
                                    "board_id": 1,
                                    "smooth_par": {"x": [0,   1, 2],
                                                   "y": [0,-1.0, 0]}
                                   }
                } 



add_homing = {"type": "TRJ_QUEUE_CMD",
              "trj_queue_cmd": {"type": "PUSH_QUE", "trj_names": ["homing"]}}

add_smooth = {"type": "TRJ_QUEUE_CMD",
              "trj_queue_cmd": {"type": "PUSH_QUE", "trj_names": ["smooth"]}}

add_leg_pitch = {"type": "TRJ_QUEUE_CMD",
                 "trj_queue_cmd": {"type": "PUSH_QUE", "trj_names": ["leg_pitch_pos","leg_pitch_neg"]}}

io.debug=0

# + pycharm={}
reply = io.doit(master_cmd_get_slave_descr)
slave_descr = yaml.safe_load(reply['msg'])
scan_ids = slave_descr.keys()
scan_ids

# + pycharm={}
slave_descr[31]

# + pycharm={}
[g for g in io.doit4ids(robot_ids,ctrl_cmd_set_home)]

for iD,h in zip(RA,RA_home) :
    io.doit(ctrl_cmd_set_home.set_value(h).set_bid(iD))

for iD,h in zip(LA,LA_home) :
    io.doit(ctrl_cmd_set_home.set_value(h).set_bid(iD))

# + pycharm={}
[g for g in io.doit4ids(robot_ids,ctrl_cmd_fan.set_value(1))]

# + pycharm={}
[g for g in io.doit4ids(robot_ids,ctrl_cmd_start().set_value(0x3B))]

# + pycharm={}
# define trajectories
# reset trj names ???
[g for g in io.doit4ids(robot_ids,homing)]
[g for g in io.doit4ids(legs_pitch_ids,leg_pitch_pos)]
[g for g in io.doit4ids(legs_pitch_ids,leg_pitch_neg)]
[g for g in io.doit4ids(legs_ids,smooth)]
[g for g in io.doit4ids(RA,smooth)]
io.doit(smooth_11)
io.doit(smooth_12)
io.doit(smooth_14)
[g for g in io.doit4ids(LA,smooth)]
io.doit(smooth_21)
io.doit(smooth_22)
io.doit(smooth_24)

# + pycharm={}
reply = io.doit(add_homing)

# + pycharm={}
reply = io.doit(add_smooth)

# + pycharm={}
reply = io.doit(add_leg_pitch)

# + pycharm={}
for i in range(100):
    reply = io.doit(add_smooth)
    reply = io.doit(add_leg_pitch)
    print ("CICLO ", i)
    time.sleep(10)


# + pycharm={}
[g for g in io.doit4ids(robot_ids,ctrl_cmd_stop)]
# -

for iD in robot_ids:
    reply = io.doit(SdoCmd(rd_sdo=['motor_pos','motor_vel','link_pos','fault'],wr_sdo={}).set_bid(iD))
    yaml_msg = yaml.safe_load(reply['msg'])
    print(iD,yaml_msg)



io.debug=0
gains = {}
gains['type'] = "POSITION"
gains['pos_kp'] = 200
gains['pos_kd'] = 10
gains['tor_kp'] = 0
gains['tor_kd'] = 0
gains['tor_ki'] = 0
[g for g in io.doit4ids(robot_ids,ctrl_cmd_start().set_value(0x3B).set_gains(u'POSITION',200,15,0,0,0))]
#[g for g in io.doit4ids(robot_ids,ctrl_cmd_start().set_value(0x3B).set_gains(*gains))]

io.debug=0
for iD in robot_ids:
    reply = io.doit(SdoCmd(rd_sdo=['gain_0','gain_1','gain_2','gain_','gain_4'],wr_sdo={}).set_bid(iD))
    yaml_msg = yaml.safe_load(reply['msg'])
    print(iD,yaml_msg)



legs_pitch_ids

rf_pitch = RFL[1:-1]
rf_pitch

list(zip(RA,RA_home))

list(map(lambda x: x[-1],(RFL,LFL,RRL,LRL)))





reply = io.doit(SdoInfo(u'SDO_NAME').set_bid(robot_ids[0]))
yaml_msg = yaml.safe_load(reply['msg'])
sdo_names = yaml_msg
sdo_names



StarcoderdataJupyterScriptsDedupFiltered