zyworkflow 0.0.1__py3-none-any.whl

zyworkflow/utils/logger_config.py

@@ -0,0 +1,50 @@
+import os
+import sys
+from loguru import logger
+
+
+def setup_logger(
+    name: str,
+    log_dir: str = "./logs",
+    level: str = "INFO",
+    retention: str = "7 days",
+    rotation: str = "00:00"
+):
+    os.makedirs(log_dir, exist_ok=True)
+    
+    logger.remove()
+    logger.add(
+        sys.stderr,
+        format="<green>{time:YYYY-MM-DD HH:mm:ss}</green> | <level>{level: <8}</level> | <cyan>{name}</cyan>:<cyan>{function}</cyan>:<cyan>{line}</cyan> - <level>{message}</level>",
+        level=level,
+        colorize=True
+    )
+    
+    log_file = os.path.join(log_dir, f"{name}_{{time:YYYY-MM-DD}}.log")
+    logger.add(
+        log_file,
+        rotation=rotation,
+        retention=retention,
+        format="{time:YYYY-MM-DD HH:mm:ss} | {level: <8} | {name}:{function}:{line} - {message}",
+        level="DEBUG",
+        encoding="utf-8",
+        enqueue=True
+    )
+    
+    return logger
+
+
+def setup_api_server_logger():
+    return setup_logger("api_server", log_dir="/workspace/logs/api_server")
+
+
+def setup_train_client_logger():
+    return setup_logger("train_client", log_dir="/workspace/logs/train_client")
+
+
+def setup_data_collection_logger():
+    return setup_logger("data_collection", log_dir="/workspace/logs/data_collection")
+
+
+def setup_train_pick_policy_logger():
+    return setup_logger("train_pick_policy", log_dir="/workspace/logs/train_pick_policy")

zyworkflow/utils/pose.py

@@ -0,0 +1,131 @@
+import numpy as np
+from typing import Sequence
+
+
+def get_gripper_pose(end_in_base_pose6: Sequence[float],) -> np.ndarray:
+    T_end_tool = np.array([
+        [1.0, 0.0, 0.0, 0.0 ],
+        [0.0, 1.0, 0.0, 0.0 ],
+        [0.0, 0.0, 1.0, 0.17],
+        [0.0, 0.0, 0.0, 1.0 ],
+    ], dtype=float)
+
+    def Rx(a: float) -> np.ndarray:
+        c, s = np.cos(a), np.sin(a)
+        return np.array([[1,0,0],[0,c,-s],[0,s,c]], dtype=float)
+
+    def Ry(a: float) -> np.ndarray:
+        c, s = np.cos(a), np.sin(a)
+        return np.array([[c,0,s],[0,1,0],[-s,0,c]], dtype=float)
+
+    def Rz(a: float) -> np.ndarray:
+        c, s = np.cos(a), np.sin(a)
+        return np.array([[c,-s,0],[s,c,0],[0,0,1]], dtype=float)
+
+    def euler_zyx_to_R(rx: float, ry: float, rz: float) -> np.ndarray:
+        return Rz(rz) @ Ry(ry) @ Rx(rx)
+
+    def make_T(R: np.ndarray, t: Sequence[float]) -> np.ndarray:
+        T = np.eye(4, dtype=float)
+        T[:3, :3] = np.asarray(R, dtype=float).reshape(3, 3)
+        T[:3, 3]  = np.asarray(t, dtype=float).reshape(3)
+        return T
+
+    def R_to_euler_zyx(R: np.ndarray) -> tuple[float, float, float]:
+        R = np.asarray(R, dtype=float).reshape(3, 3)
+
+        sy = float(np.clip(-R[2, 0], -1.0, 1.0))
+        ry = float(np.arcsin(sy))
+        cy = float(np.cos(ry))
+
+        if abs(cy) > 1e-8:
+            rx = float(np.arctan2(R[2, 1], R[2, 2]))
+            rz = float(np.arctan2(R[1, 0], R[0, 0]))
+        else:
+            rz = 0.0
+            rx = float(np.arctan2(-R[0, 1], R[1, 1]))
+
+        return rx, ry, rz
+
+    end6 = np.asarray(end_in_base_pose6, dtype=float).reshape(6)
+    x, y, z, rx, ry, rz = end6.tolist()
+
+    T_base_end = make_T(euler_zyx_to_R(rx, ry, rz), [x, y, z])
+    T_base_tool = T_base_end @ T_end_tool
+
+    t_bt = T_base_tool[:3, 3]
+    rx_t, ry_t, rz_t = R_to_euler_zyx(T_base_tool[:3, :3])
+    return np.array([t_bt[0], t_bt[1], t_bt[2], rx_t, ry_t, rz_t], dtype=float)
+
+
+def get_target_pose(end_in_base_pose6: Sequence[float], obj_in_cam_pose7: Sequence[float],) -> np.ndarray:
+    # TODO: 需要根据实际情况调整, 未来要传参数进来
+    T_EE_CAM = np.array([
+        [-0.9801513, -0.12894636, -0.15058639,  0.04160245],
+        [ 0.19782898, -0.68568308, -0.70050154,  0.15992103],
+        [ -0.01292742, -0.71638785, 0.69758249,  0.11156246],
+        [ 0.0,         0.0,         0.0,         1.0       ],
+    ], dtype=float)
+
+    def Rx(a):
+        c, s = np.cos(a), np.sin(a)
+        return np.array([[1,0,0],[0,c,-s],[0,s,c]], dtype=float)
+
+    def Ry(a):
+        c, s = np.cos(a), np.sin(a)
+        return np.array([[c,0,s],[0,1,0],[-s,0,c]], dtype=float)
+
+    def Rz(a):
+        c, s = np.cos(a), np.sin(a)
+        return np.array([[c,-s,0],[s,c,0],[0,0,1]], dtype=float)
+
+    def euler_zyx_to_R(rx, ry, rz):
+        return Rz(rz) @ Ry(ry) @ Rx(rx)
+
+    def quat_xyzw_to_R(q):
+        q = np.asarray(q, dtype=float).reshape(4)
+        q = q / (np.linalg.norm(q) + 1e-12)
+        x, y, z, w = q
+        return np.array([
+            [1 - 2*(y*y + z*z),     2*(x*y - z*w),     2*(x*z + y*w)],
+            [    2*(x*y + z*w), 1 - 2*(x*x + z*z),     2*(y*z - x*w)],
+            [    2*(x*z - y*w),     2*(y*z + x*w), 1 - 2*(x*x + y*y)],
+        ], dtype=float)
+
+    def R_to_euler_zyx(R):
+        R = np.asarray(R, dtype=float).reshape(3, 3)
+
+        sy = float(np.clip(-R[2, 0], -1.0, 1.0))
+        ry = float(np.arcsin(sy))
+        cy = float(np.cos(ry))
+
+        if abs(cy) > 1e-8:
+            rx = float(np.arctan2(R[2, 1], R[2, 2]))
+            rz = float(np.arctan2(R[1, 0], R[0, 0]))
+        else:
+            rz = 0.0
+            rx = float(np.arctan2(-R[0, 1], R[1, 1]))
+
+        return rx, ry, rz
+
+    def make_T(R, t):
+        T = np.eye(4, dtype=float)
+        T[:3, :3] = np.asarray(R, dtype=float).reshape(3, 3)
+        T[:3, 3] = np.asarray(t, dtype=float).reshape(3)
+        return T
+
+    end6 = np.asarray(end_in_base_pose6, dtype=float).reshape(6)
+    ex, ey, ez, erx, ery, erz = end6.tolist()
+    T_base_ee = make_T(euler_zyx_to_R(erx, ery, erz), [ex, ey, ez])
+
+    obj7 = np.asarray(obj_in_cam_pose7, dtype=float).reshape(7)
+    t_co = obj7[:3]
+    q_co = obj7[3:]
+    T_cam_obj = make_T(quat_xyzw_to_R(q_co), t_co)
+
+    T_base_obj = T_base_ee @ T_EE_CAM @ T_cam_obj
+
+    t_bo = T_base_obj[:3, 3]
+    rx, ry, rz = R_to_euler_zyx(T_base_obj[:3, :3])
+
+    return np.array([t_bo[0], t_bo[1], t_bo[2], rx, ry, rz], dtype=float)

zyworkflow/utils/utils.py

@@ -0,0 +1,264 @@
+# coding: utf-8
+import io
+import cv2
+import requests
+import collections
+import numpy as np
+from PIL import Image
+from zyworkflow.utils.logger_config import setup_api_server_logger
+logger = setup_api_server_logger()
+
+
+try:
+    collectionsAbc = collections.abc
+except AttributeError:
+    collectionsAbc = collections
+callback_url = "http://192.168.1.103:8888/api/v1/task/end"
+rgb_image_url = "http://192.168.1.102:8660/api/robot/rgb_image?dev=top_camera"
+depth_image_url = "http://192.168.1.102:8660/api/robot/depth_image?dev=top_camera"
+arm_status = "http://192.168.1.102:8660/api/arm/pose"
+arm_movej = "http://192.168.1.102:8660/api/arm/move-j"
+arm_movel = "http://192.168.1.102:8660/api/arm/move-l"
+arm_stop = "http://192.168.1.102:8660/api/arm/stop"
+gripper_move = "http://192.168.1.102:8660/api/arm/gripper-position"
+gripper_status = "http://192.168.1.102:8660/api/gripper/status"
+dof = "http://192.168.112.161:58896/api/call-service/calculate"
+find_basket = "http://192.168.112.161:58896/api/call-service/find_basket"
+
+
+class AttrDict(dict):
+    def __init__(self, **kwargs):
+        super(AttrDict, self).__init__()
+        super(AttrDict, self).update(kwargs)
+
+    def __getattr__(self, key):
+        if key in self:
+            return self[key]
+        raise AttributeError("没有找到key:'{}'".format(key))
+
+
+global_config = AttrDict()
+
+
+def preprocess_image(img):
+    img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
+    img = img.transpose(2, 0, 1)
+    img = img.astype(np.float32) / 255.0
+    return img
+
+
+def register(name=None):
+    if callable(name):
+        cls = name
+        return register()(cls)
+
+    def wrapper(cls):
+        nonlocal name
+        if isinstance(cls, type) and cls.__name__ in global_config:
+            raise ValueError(f"该类{cls.__name__}已经被注册过")
+        if not isinstance(cls, type) and callable(cls):
+            func = cls
+            cls = type(
+                cls.__name__, (object,), {
+                    '__call__': lambda _, *arg, **kws: func(*arg, **kws),
+                    '__doc__': func.__doc__,
+                })
+        if name:
+            global_config[name] = cls
+        else:
+            global_config[cls.__name__] = cls
+
+        return cls
+
+    return wrapper
+
+
+def create(cls_or_name, args=None, kws=None):
+    assert type(cls_or_name) in [type, str], "必须是类或类的名字"
+    name = type(cls_or_name) == str and cls_or_name or cls_or_name.__name__
+    cls = global_config[name]
+    if args is None:
+        args = ()
+    if kws is None:
+        kws = {}
+
+    return cls(*args, **kws)
+
+
+def get_image(url):
+    try:
+        response = requests.get(url, timeout=10)
+        response.raise_for_status()
+        jpg_bytes = response.content
+
+        image_stream = io.BytesIO(jpg_bytes)
+        image = Image.open(image_stream)
+        image_array = np.array(image)
+
+        return image_array, ""
+
+    except requests.exceptions.RequestException as e:
+        logger.error(f"获取URL图片失败：{str(e)}")
+        return None, f"获取URL图片失败：{str(e)}"
+    except Exception as e:
+        logger.error(f"转换为numpy数组失败：{str(e)}")
+        return None, f"转换为numpy数组失败：{str(e)}"
+
+
+def get_arm_status():
+    try:
+        response = requests.get(arm_status, timeout=10)
+        response.raise_for_status()
+        result = response.json()
+
+        status = result.get('status')
+        return status, ""
+
+    except requests.exceptions.RequestException as e:
+        logger.error(f"获取URL机械臂状态失败：{str(e)}")
+        return None, f"获取URL机械臂状态失败：{str(e)}"
+    except Exception as e:
+        logger.error(f"读取机械臂状态失败：{str(e)}")
+        return None, f"读取机械臂状态失败：{str(e)}"
+
+
+def get_gripper_status():
+    try:
+        response = requests.get(gripper_status, timeout=10)
+        response.raise_for_status()
+        result = response.json()
+
+        grip_state = result.get('grip_state')
+        position = result.get('position')
+        set_position = result.get('set_position')
+        return grip_state, set_position, position, ""
+
+    except requests.exceptions.RequestException as e:
+        logger.error(f"获取URL夹爪状态失败：{str(e)}")
+        return None, None, None, f"获取URL夹爪状态失败：{str(e)}"
+    except Exception as e:
+        logger.error(f"读取机械臂夹爪失败：{str(e)}")
+        return None, None, None, f"读取机械臂夹爪失败：{str(e)}"
+
+
+def post_arm_movej(joints, speed=10):
+    try:
+        data = {'joints': [float(joint) for joint in joints], "speed": speed}
+        response = requests.post(arm_movej, json=data, timeout=100)
+        response.raise_for_status()
+        result = response.json()
+
+        code = result['code']
+        return code, ""
+
+    except requests.exceptions.RequestException as e:
+        logger.error(f"获取URL机械臂movej失败：{str(e)}")
+        return None, f"获取URL机械臂movej失败：{str(e)}"
+    except Exception as e:
+        logger.error(f"解析movej返回值失败：{str(e)}")
+        return None, f"解析movej返回值失败：{str(e)}"
+
+
+def post_arm_movel(poses, speed=10):
+    try:
+        data = {'pose': [float(pose) for pose in poses], "speed": speed}
+        response = requests.post(arm_movel, json=data, timeout=100)
+        response.raise_for_status()
+        result = response.json()
+
+        code = result['code']
+        return code, ""
+
+    except requests.exceptions.RequestException as e:
+        logger.error(f"获取URL机械臂movel失败：{str(e)}")
+        return None, f"获取URL机械臂movel失败：{str(e)}"
+    except Exception as e:
+        logger.error(f"解析movel返回值失败：{str(e)}")
+        return None, f"解析movel返回值失败：{str(e)}"
+
+
+def post_gripper_move(pos):
+    try:
+        data = {'pos': int(pos)}
+        response = requests.post(gripper_move, json=data, timeout=100)
+        response.raise_for_status()
+        result = response.text
+
+        return result, ""
+
+    except requests.exceptions.RequestException as e:
+        logger.error(f"获取URL夹爪move失败：{str(e)}")
+        return None, f"获取URL夹爪move失败：{str(e)}"
+    except Exception as e:
+        logger.error(f"解析夹爪move返回值失败：{str(e)}")
+        return None, f"解析夹爪move返回值失败：{str(e)}"
+
+
+def post_6dof(sku):
+    try:
+        data = {'class_id': sku}
+        response = requests.post(dof, json=data, timeout=100)
+        response.raise_for_status()
+        result = response.json()
+
+        status = result['data']['ok']
+        try:
+            xyz = result['data']['xyz']
+            quat_xyzw = result['data']['quat_xyzw']
+            logger.info(f"xyz：{xyz}, quat_xyz: {quat_xyzw}")
+        except Exception as e:
+            logger.error(f"JSON解析失败: {e}")
+            return None, None, ""
+
+        return xyz, quat_xyzw, ""
+
+    except requests.exceptions.RequestException as e:
+        logger.error(f"获取URL 6dof失败：{str(e)}")
+        return None, None, f"获取URL 6dof失败：{str(e)}"
+    except Exception as e:
+        logger.error(f"解析6dof返回值失败：{str(e)}")
+        return None, None, f"解析6dof返回值失败：{str(e)}"
+
+
+def post_find_basket():
+    try:
+        data = {}
+        response = requests.post(find_basket, json=data, timeout=100)
+        response.raise_for_status()
+        result = response.json()
+
+        status = result['data']['ok']
+        try:
+            xyz = result['data']['xyz']
+            quat_xyzw = result['data']['quat_xyzw']
+            logger.info(f"xyz：{xyz}, quat_xyz: {quat_xyzw}")
+        except Exception as e:
+            logger.error(f"JSON解析失败: {e}")
+            return None, None, ""
+
+        return xyz, quat_xyzw, ""
+
+    except requests.exceptions.RequestException as e:
+        logger.error(f"获取URL 6dof失败：{str(e)}")
+        return None, None, f"获取URL 6dof失败：{str(e)}"
+    except Exception as e:
+        logger.error(f"解析6dof返回值失败：{str(e)}")
+        return None, None, f"解析6dof返回值失败：{str(e)}"
+
+
+def post_arm_stop():
+    try:
+        data = {}
+        response = requests.post(arm_stop, json=data, timeout=100)
+        response.raise_for_status()
+        result = response.json()
+
+        code = result['code']
+        return code, ""
+
+    except requests.exceptions.RequestException as e:
+        logger.error(f"获取URL机械臂stop失败：{str(e)}")
+        return None, f"获取URL机械臂stop失败：{str(e)}"
+    except Exception as e:
+        logger.error(f"解析stop返回值失败：{str(e)}")
+        return None, f"解析stop返回值失败：{str(e)}"

zyworkflow-0.0.1.dist-info/METADATA

@@ -0,0 +1,11 @@
+Metadata-Version: 2.4
+Name: zyworkflow
+Version: 0.0.1
+Summary: Need Python3.9+
+License: Apache License 2.0
+Requires-Dist: psutil
+Requires-Dist: Cython
+Requires-Dist: aiohttp
+Dynamic: license
+Dynamic: requires-dist
+Dynamic: summary

zyworkflow-0.0.1.dist-info/RECORD

@@ -0,0 +1,19 @@
+zyworkflow/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+zyworkflow/api_server.py,sha256=hcLuwI2vmquHkgQ6EzTwfD_iERMANSramL7hP_pSIWc,26684
+zyworkflow/data/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+zyworkflow/data/collection.py,sha256=3aj8n7w31muccCfRdin5DzEbNZaTMC4hmWlWc6T5wUk,48507
+zyworkflow/data/process.py,sha256=eK1nrtPOIlOA_x6dTjIIMiQEfvIiY7UpELkiGHDgtBw,2626
+zyworkflow/doc/api.md,sha256=rqV9eDsHMRyTbg3T6QhLdG4j9R_bMfDGehMt2APkNNw,12061
+zyworkflow/example/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+zyworkflow/example/train_client.py,sha256=JMdYX-FwNcGOU8u6ykyqlvkXF4KCUkd8iOK9ABNVzxQ,13317
+zyworkflow/example/train_client_example.py,sha256=Y2W4rUsppoyVeovqE3yH1OkQ81mkqizTuTk9YKVY5w8,965
+zyworkflow/policy/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+zyworkflow/policy/train_pick_policy.py,sha256=-QjM9CMokQ4_uI8Y4Va-9BefelvCgxCPQ3-HeLD6tRk,31929
+zyworkflow/utils/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+zyworkflow/utils/logger_config.py,sha256=oaEd204gMoj9591CtQFEITStJzSPr2PzF015rEgJDX4,1372
+zyworkflow/utils/pose.py,sha256=JXjmxVCvlq0jXTFykGJDiHlzehkursZMBY-99UipM6U,4417
+zyworkflow/utils/utils.py,sha256=yRLzUwWP1sPbfRoghnm2kgTvo0tBOHW1wRuF1NXstN8,8709
+zyworkflow-0.0.1.dist-info/METADATA,sha256=TfNviS3rH2G62rzAVvQje0SHyp9n2ATTgNt-EhV-cJ0,231
+zyworkflow-0.0.1.dist-info/WHEEL,sha256=wUyA8OaulRlbfwMtmQsvNngGrxQHAvkKcvRmdizlJi0,92
+zyworkflow-0.0.1.dist-info/top_level.txt,sha256=Hc13aPsoEw0Zc0BhJjR2vl8AvTqsyemSK_mYUJRttUk,11
+zyworkflow-0.0.1.dist-info/RECORD,,

zyworkflow-0.0.1.dist-info/WHEEL

@@ -0,0 +1,5 @@
+Wheel-Version: 1.0
+Generator: setuptools (80.10.2)
+Root-Is-Purelib: true
+Tag: py3-none-any
+

zyworkflow-0.0.1.dist-info/top_level.txt

@@ -0,0 +1 @@
+zyworkflow