Metadata-Version: 2.4
Name: hex_util_ros
Version: 0.0.1a1
Summary: HEXFELLOW Robotics Utilities
Author-email: Dong Zhaorui <dzr159@gmail.com>
Maintainer-email: jecjune <zejun.chen@hexfellow.com>, Dong Zhaorui <dzr159@gmail.com>
License-Expression: Apache-2.0
Project-URL: Homepage, https://github.com/hexfellow/hex_util_robot
Project-URL: Repository, https://github.com/hexfellow/hex_util_robot.git
Project-URL: Bug Tracker, https://github.com/hexfellow/hex_util_robot/issues
Project-URL: Documentation, https://github.com/hexfellow/hex_util_robot/wiki
Keywords: hex,util,robot
Classifier: Development Status :: 4 - Beta
Classifier: Intended Audience :: Developers
Classifier: Intended Audience :: Science/Research
Classifier: Topic :: Software Development :: Libraries :: Python Modules
Classifier: Programming Language :: Python :: 3
Classifier: Programming Language :: Python :: 3.8
Classifier: Programming Language :: Python :: 3.9
Classifier: Programming Language :: Python :: 3.10
Classifier: Programming Language :: Python :: 3.11
Classifier: Programming Language :: Python :: 3.12
Classifier: Operating System :: POSIX :: Linux
Requires-Python: >=3.8
Description-Content-Type: text/markdown
License-File: LICENSE
Requires-Dist: hex_dynamic>=0.1.3
Requires-Dist: numpy>=1.17.4
Requires-Dist: opencv-python>=4.2
Dynamic: license-file

<h1 align="center">HEXFELLOW ROBOTICS UTILITIES</h1>

<p align="center">
    <a href="https://github.com/hexfellow/hex_util_robot/stargazers">
        <img src="https://img.shields.io/github/stars/hexfellow/hex_util_robot?style=flat-square&logo=github" />
    </a>
    <a href="https://github.com/hexfellow/hex_util_robot/forks">
        <img src="https://img.shields.io/github/forks/hexfellow/hex_util_robot?style=flat-square&logo=github" />
    </a>
    &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;
    <a href="https://github.com/hexfellow/hex_util_robot/issues">
        <img src="https://img.shields.io/github/issues/hexfellow/hex_util_robot?style=flat-square&logo=github" />
    </a>
    &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;
    <img src="https://img.shields.io/badge/python-≥3.8-blue?style=flat-square&logo=python" />
    &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;
    <img src="https://img.shields.io/badge/platform-Linux-orange?style=flat-square" />
</p>

---

# 📖 Overview

## What is `hex_util_ros`

`hex_util_ros` is a lightweight Python utility library providing reusable robotic primitives for kinematics, dynamics, control, state estimation, motion planning, and more. It is part of the HEXFELLOW ecosystem and serves as the algorithmic middleware layer for robot system development.

| Module         | Primary Purpose                                                       | Key Components                                                                                                  |
| -------------- | --------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------- |
| **math_utils** | 3D rotation & transformation (SO(3), SE(3), quaternion, Euler angles) | `hat`, `rot2quat`, `euler2rot`, `trans_inv`, `quat_slerp`                                                       |
| **dyn_util**   | Rigid-body dynamics (forward/inverse kinematics, ID, Coriolis)        | `HexDynUtil`, `HexDynUtilY6`                                                                                    |
| **ctrl_util**  | Joint-level and workspace-level controllers (MIT, PID, integral)      | `HexCtrlUtilMitJoint`, `HexCtrlUtilPidJoint`, `HexCtrlUtilIntJoint`, `HexCtrlUtilMitWork`, `HexCtrlUtilIntWork` |
| **intx_util**  | Interaction utilities (mirroring, friction, feedback)                 | `HexMirrorUtil`, `HexFricUtil`, `HexFeedbackUtil`                                                               |
| **cam_util**   | Camera & depth-image utilities                                        | `depth_to_cmap`                                                                                                 |
| **basic_util** | General-purpose numeric helpers                                       | `deadzone`, `remap`, `time_interp`, `time_nearest`                                                              |
| **robot_util** | Robot-specific utility functions                                      | `interp_joint`, `arm_pos_limit`, `grip_pos_limit`, `state_interp`                                               |

## What problem it solves

- **Unified math library**: Provide a consistent, well-tested set of functions for SO(3), SE(3), quaternion, and Euler angle conversions, all with batched (vectorized) support and uniform input/output conventions.
- **Robot dynamics abstraction**: Wrap `hex_dynamic` (Pinocchio-based) behind a clean API that handles flange-to-end-effector transforms, analytical IK for Y6 arms, and Coriolis/gravity compensation.
- **Reusable interaction primitives**: Mirroring, tanh-based friction modeling, and PD feedback control as callable objects — ready to plug into any robot control loop.

## Target users

- Robotics engineers building low-level control loops for manipulators.
- Developers working with the HEXFELLOW ecosystem (`hex_util_runtime`, `hex_driver_mujoco`, etc.).
- Researchers prototyping kinematics, dynamics, and motion planning algorithms.
- Anyone needing a clean, vectorized math library for 3D rotations and transformations.

## Project structure

```
hex_util_ros/
├── hex_util_ros/
│   ├── __init__.py         # Public exports (__all__)
│   ├── math_utils.py       # 3D math: quaternion, rotation, pose, Euler angles
│   ├── dyn_util.py         # Rigid-body dynamics: HexDynUtil, HexDynUtilY6
│   ├── ctrl_util.py        # Control utilities: joint/work controllers (MIT, PID, integral)
│   ├── intx_util.py        # Interaction utilities: HexMirrorUtil, HexFricUtil, HexFeedbackUtil
│   ├── cam_util.py         # Camera utilities: depth-to-colormap
│   ├── basic_util.py       # Basic utilities: deadzone, remap, time interpolation
│   └── robot_util.py       # Robot helpers: joint interpolation, position limits
├── test/
│   └── test_math_utils.py  # Unit tests for math_utils
├── docs/
│   └── api.md               # Full API reference
└── pyproject.toml            # Project metadata & build configuration
```

---

# 📦 Installation

## Requirements

- **Python** ≥ 3.8
- **OS**: Linux
- **Dependencies**:
  - `hex_dynamic ≥ 0.1.3`
  - `numpy ≥ 1.17.4`
  - `opencv-python ≥ 4.2`

## Install from PyPI

```bash
pip install hex_util_ros
```

## Install from Source

We use [**uv**](https://github.com/astral-sh/uv) to manage the Python environment. Please install it first.

1. Clone and install in editable mode:

```bash
git clone https://github.com/hexfellow/hex_util_robot.git
cd hex_util_robot
./venv.sh
```

2. Activate before using:

```bash
source .venv/bin/activate
```

---

# ⚡ Quick Start

```python
import numpy as np
from hex_util_ros import hat, rot2quat, quat_slerp, trans_inv
from hex_util_ros import euler2rot, angle_norm

# SO(3) hat operator
omega = np.array([0.1, 0.2, 0.3])
omega_hat = hat(omega)  # 3x3 skew-symmetric matrix

# Rotation matrix to quaternion
rot = euler2rot(np.array([0.1, 0.2, 0.3]), format='xyz')
quat = rot2quat(rot)  # [w, x, y, z]

# Quaternion slerp
q1 = np.array([1.0, 0.0, 0.0, 0.0])
q2 = np.array([0.707, 0.707, 0.0, 0.0])
q_interp = quat_slerp(q1, q2, 0.5)  # halfway quaternion

# Transform inverse
T = np.eye(4)
T[:3, 3] = [1.0, 2.0, 3.0]
T_inv = trans_inv(T)

# Normalise angle to [-pi, pi]
theta = angle_norm(3.5)  # -2.783 rad
```

```python
# Joint-level MIT controller
from hex_util_ros import HexCtrlUtilMitJoint

ctrl = HexCtrlUtilMitJoint(ctrl_limit=np.array([1.0] * 6))
tau = ctrl(kp=10.0, kd=0.5, q_tar=np.zeros(6), dq_tar=np.zeros(6),
           q_cur=np.array([0.1]*6), dq_cur=np.zeros(6), tau_comp=np.zeros(6))
```

```python
# Depth-to-colormap for visualization
from hex_util_ros import depth_to_cmap
import cv2

depth = np.random.rand(480, 640).astype(np.float32) * 5000
depth_color = depth_to_cmap(depth, depth_range=(70, 1000))
```

---

# 📑 Documentation

- [**API Reference**](docs/api.md) — Detailed documentation of all classes and functions.

---

# 📄 License

Apache License 2.0. See [LICENSE](LICENSE).

---

# 👥 Authors & Maintainers

| Role           | Name                 | Email                    |
| -------------- | -------------------- | ------------------------ |
| **Author**     | Dong Zhaorui         | dzr159@gmail.com |
| **Maintainer** | jecjune (Chen Zejun) | zejun.chen@hexfellow.com |
| **Maintainer** | Dong Zhaorui         | dzr159@gmail.com |

---

# 🌟 Star History

[![Star History Chart](https://api.star-history.com/svg?repos=hexfellow/hex_util_robot&type=Date)](https://star-history.com/#hexfellow/hex_util_robot&Date)

---

# 👥 Contributors

<a href="https://github.com/hexfellow/hex_util_robot/graphs/contributors">
    <img src="https://contrib.rocks/image?repo=hexfellow/hex_util_robot" />
</a>
