From 54f9b038da0919413a671c46489f227156d0eef6 Mon Sep 17 00:00:00 2001
From: Agnel Wang <59766821+Agnel-Wang@users.noreply.github.com>
Date: Thu, 8 Dec 2022 10:58:55 +0800
Subject: [PATCH] Create unitreeArmTools.py

---
 unitreeArmTools.py | 53 ++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 53 insertions(+)
 create mode 100644 unitreeArmTools.py

diff --git a/unitreeArmTools.py b/unitreeArmTools.py
new file mode 100644
index 0000000..56b82a0
--- /dev/null
+++ b/unitreeArmTools.py
@@ -0,0 +1,53 @@
+# Set the IP address of the z1 robot
+# Before running the program, please check the network with the robot
+import re
+import os
+import sys
+import socket
+import struct
+
+def check_ip(ipAddr):
+    compile_ip = re.compile('192.168.123.1(\d{2})')
+    if compile_ip.match(ipAddr):
+        return True
+    else:
+        print("[ERROR] Please input the valid IP address")
+        return False
+
+def ping_ip(ip):
+    cmd = "ping " + str(ip) + " -c 1 -w 1 >/dev/null"  #send once, wait 1s
+    if  os.system(cmd):
+        print("[ERROR]IP: "+ip+" Destination Host Unreachable")
+        return False
+    else:
+        return True
+
+if __name__ == '__main__':
+    # 1. check arm valid
+    if not ping_ip('192.168.123.110'):
+        sys.exit(1)
+
+    server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    address = ("0.0.0.0", 8881)
+    armAddr = ("192.168.123.110", 8880)
+    server_socket.bind(address)
+    pack_format = struct.Struct('6B')
+
+    # 2. read IP address input
+    print("Please enter the IP address to set, which should be input as 192.168.123.1** ")
+    ipAddr = ''
+    while True:
+        ipAddr = input("IP to set:   ")
+        if check_ip(ipAddr):
+            break
+    ipset = re.split('\.', ipAddr)
+    
+    # 3. send udp socket to set IP address
+    send_data = [7, 1, 192, 168, 123, int(ipset[3])]
+    send_data = pack_format.pack(*send_data)
+    server_socket.sendto(send_data, armAddr)
+    data, client = server_socket.recvfrom(6)
+    data = pack_format.unpack(data)
+    ipAddr = str(data[2])+'.'+str(data[3])+'.'+str( data[4])+'.'+str(data[5])
+    print("Set correctly! Current robot IP is", ipAddr)
+    print("Please repower the robot and change the IP setting in the z1_controller/config/config.xml")