# gripper_controller

**Repository Path**: bole233/gripper_controller

## Basic Information

- **Project Name**: gripper_controller
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: main
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2026-01-15
- **Last Updated**: 2026-04-20

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# Gripper Controller

ROS2 C++ package for controlling gripper via serial port using **ros2_control** framework.

## Architecture

This package follows the [ros2_robotiq_gripper](https://github.com/PickNikRobotics/ros2_robotiq_gripper) architecture pattern, integrating with the ros2_control framework:

```
┌─────────────────────────────────────────┐
│     ros2_control Framework              │
│  ┌────────────────────────────────┐     │
│  │  Controller Manager            │     │
│  │  ┌──────────────────────────┐  │     │
│  │  │ Forward Command          │  │     │
│  │  │ Controller               │  │     │
│  │  └──────────────────────────┘  │     │
│  │  ┌──────────────────────────┐  │     │
│  │  │ Activation Controller    │  │     │
│  │  └──────────────────────────┘  │     │
│  └────────────────────────────────┘     │
│              ▲                           │
│              │ Command/State Interfaces  │
│              ▼                           │
│  ┌────────────────────────────────┐     │
│  │ Hardware Interface             │     │
│  │ (GripperHardwareInterface)     │     │
│  └────────────────────────────────┘     │
└──────────────┬──────────────────────────┘
               │
               ▼
   ┌───────────────────────┐
   │ Driver Layer          │
   │ (DefaultDriver)       │
   └───────────────────────┘
               │
               ▼
   ┌───────────────────────┐
   │ Serial Layer          │
   │ (DefaultSerial)       │
   └───────────────────────┘
               │
               ▼
         Gripper Hardware
```

## Features

- **ros2_control integration**: Full hardware_interface implementation
- **Command interfaces**: Position control (velocity and force as parameters)
- **State interfaces**: Position and velocity feedback
- **Activation controller**: Service for gripper reactivation
- **Modular design**: Separate layers for serial, driver, and hardware interface
- **Platform**: Linux support
- **Modbus RTU protocol**: Standard industrial communication
- **Register monitor**: Tool for reading gripper registers in real-time

## Dependencies

- ROS2 (Humble or later)
- hardware_interface
- controller_interface
- controller_manager
- robot_state_publisher
- joint_state_broadcaster
- forward_command_controller
- rosidl_default_runtime

## Installation

```bash
cd ~/ros2_ws/src
# Clone this package
cd ~/ros2_ws
colcon build --packages-select hkv_gripper_controller gripper_controller_interfaces
source install/setup.bash
```

## Usage

### Launch with ros2_control (Recommended)

```bash
ros2 launch hkv_gripper_controller gripper_control.launch.py \
    serial_port:=/dev/ttyUSB0 \
    baud_rate:=1000000
```

## Control Interface

### 1. Topic-based Control (via forward_command_controller)

```bash
# Move to position 0.05 (half open)
ros2 topic pub --once /tg9801_gripper_controller/commands std_msgs/msg/Float64MultiArray "data: [0.05]"

# Fully close (0.1)
ros2 topic pub --once /tg9801_gripper_controller/commands std_msgs/msg/Float64MultiArray "data: [0.1]"

# Fully open (0.0)
ros2 topic pub --once /tg9801_gripper_controller/commands std_msgs/msg/Float64MultiArray "data: [0.0]"
```

### 2. Python Client (Persistent Connection)

```python
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray

class GripperClient(Node):
    def __init__(self):
        super().__init__('gripper_client')
        self.pub = self.create_publisher(
            Float64MultiArray,
            '/tg9801_gripper_controller/commands',
            10
        )
    
    def move_to(self, position):
        """Move gripper to position (0.0=open, 0.1=closed)"""
        msg = Float64MultiArray()
        msg.data = [position]
        self.pub.publish(msg)
        self.get_logger().info(f'Moving to: {position}')

def main():
    rclpy.init()
    client = GripperClient()
    
    # Open gripper
    client.move_to(0.0)
    
    # Half close
    client.move_to(0.05)
    
    # Full close
    client.move_to(0.1)
    
    rclpy.spin(client)

if __name__ == '__main__':
    main()
```

### 3. C++ Client

```cpp
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>

class GripperClient : public rclcpp::Node {
public:
  GripperClient() : Node("gripper_client") {
    publisher_ = create_publisher<std_msgs::msg::Float64MultiArray>(
      "/tg9801_gripper_controller/commands", 10);
  }

  void moveTo(double position) {
    auto msg = std_msgs::msg::Float64MultiArray();
    msg.data = {position};
    publisher_->publish(msg);
    RCLCPP_INFO(get_logger(), "Moving to: %f", position);
  }

private:
  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_;
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto client = std::make_shared<GripperClient>();
  client->moveTo(0.05);  // Half open
  rclcpp::spin(client);
  return 0;
}
```

### 4. Gripper Reactivation

Use the activation controller service to re-calibrate:

```bash
ros2 service call /tg9801_activation_controller/reactivate_gripper std_srvs/srv/Trigger
```

### 5. Monitor Joint State

```bash
# View joint states
ros2 topic echo /joint_states

# List controllers
ros2 control list_controllers

# List hardware interfaces
ros2 control list_hardware_interfaces
```

## Configuration

### Hardware Parameters (URDF/xacro)

Edit [urdf/gripper.urdf.xacro](urdf/gripper.urdf.xacro):

```xml
<hardware>
  <plugin>hkv_gripper_controller/GripperHardwareInterface</plugin>
  <param name="serial_port">/dev/ttyUSB0</param>
  <param name="baud_rate">1000000</param>
  <param name="timeout">1000</param>
  <param name="slave_address">1</param>
  <param name="gripper_model">TG-9801</param>
  <param name="gripper_closed_position">0.1</param>
  <param name="position_mode_speed_register">1000</param>
  <param name="target_force_percent">50</param>
</hardware>
```

### Controller Parameters

Edit [config/gripper_controllers.yaml](config/gripper_controllers.yaml):

```yaml
controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

tg9801_gripper_controller:
  ros__parameters:
    joints:
      - gripper_joint
    interface_name: position
```

## Package Structure

```
hkv_gripper_controller/
├── include/hkv_gripper_controller/
│   ├── serial.hpp                         # Serial interface (abstract)
│   ├── default_serial.hpp                # Serial implementation
│   ├── driver.hpp                         # Driver interface (abstract)
│   ├── default_driver.hpp                # Modbus RTU driver
│   ├── driver_factory.hpp               # Factory interface
│   ├── default_driver_factory.hpp        # Factory implementation
│   ├── modbus_utils.hpp                   # Modbus protocol utilities
│   ├── gripper_hardware_interface.hpp      # ros2_control hardware interface
│   ├── gripper_activation_controller.hpp   # Activation controller
│   ├── gripper_controller_node.hpp      # Service-based node
│   └── register_monitor_node.hpp      # Register monitor node
├── src/
│   ├── serial.cpp
│   ├── default_serial.cpp
│   ├── modbus_utils.cpp
│   ├── default_driver.cpp
│   ├── default_driver_factory.cpp
│   ├── gripper_hardware_interface.cpp
│   ├── gripper_activation_controller.cpp
│   ├── gripper_controller_node.cpp
│   ├── register_monitor_node.cpp
│   ├── register_monitor_main.cpp
│   └── main.cpp
├── msg/
│   └── GripperRegisters.msg               # Register data message
├── srv/
│   ├── GripperCommand.srv              # Grip command service
│   └── ReadFingerState.srv             # Read finger state service
├── urdf/
│   └── gripper.urdf.xacro              # Robot description
├── config/
│   └── gripper_controllers.yaml         # Controller config
├── launch/
│   ├── gripper_control.launch.py         # ros2_control launch
│   ├── gripper_launch.py             # Service-based launch
│   └── register_monitor.launch.py  # Register monitor launch
├── nodes/
│   └── rviz_gripper_cmd_marker.py    # RViz marker tool
├── rviz/
│   └── gripper_control.rviz        # RViz config
├── meshes/
│   ├── base_body.stl
│   ├── rail_155.stl
│   ├── slider.stl
│   ├── finger.stl
│   └── ...
├── CMakeLists.txt
├── package.xml
└── README.md
```

## Comparison with ros2_robotiq_gripper

| Feature | ros2_robotiq_gripper | This Package |
|---------|---------------------|--------------|
| Framework | ros2_control | ros2_control |
| Hardware Interface | SystemInterface | SystemInterface |
| Command Interfaces | position, velocity, force | position only |
| State Interfaces | position, velocity | position, velocity |
| Activation | Service via controller | Service via controller |
| Communication | Modbus RTU | Modbus RTU |
| Platform | Linux | Linux |
| Gripper Model | Robotiq 2F-85/140 | TG-9801 |

## Troubleshooting

### Serial Port Permissions (Linux)

```bash
# Check port
ls -l /dev/ttyUSB0

# Add user to dialout group
sudo usermod -a -G dialout $USER
# Logout and login again

# Or temporary fix
sudo chmod 666 /dev/ttyUSB0
```


### Controllers Not Loading

```bash
# Check controller manager
ros2 control list_controllers

# Check hardware interfaces
ros2 control list_hardware_interfaces

# View logs
ros2 run controller_manager ros2_control_node --ros-args --log-level debug
```

### Hardware Interface Plugin Not Found

```bash
# Verify plugin registration
ros2 pkg list | grep hkv_gripper_controller

# List hardware_interface plugins
ros2 run pluginlib list_plugins hardware_interface

# Check for hkv_gripper_controller/GripperHardwareInterface
```

## Service Interface

The package provides service-based control as an alternative to topic-based control:

```bash
# Launch service node
ros2 launch hkv_gripper_controller gripper_launch.py serial_port:=/dev/ttyUSB0 baud_rate:=1000000

# Use services
ros2 service call /gripper_command hkv_gripper_controller/srv/GripperCommand "{command: true}"
ros2 service call /read_finger_state hkv_gripper_controller/srv/ReadFingerState
```

## Register Monitor

The package includes a register monitor node for reading gripper registers in real-time:

```bash
# Launch register monitor
ros2 launch hkv_gripper_controller register_monitor.launch.py serial_port:=/dev/ttyUSB0 baud_rate:=1000000

# View register data (position: index 8, current: index 9)
ros2 topic echo /gripper/registers
```

### Register Mapping

| Index | Address | Name | Range | Description |
|-------|---------|------|-------|------------|
| 0 | 0x0000 | X1 | -32767~32767 | Finger 1 X displacement |
| 1 | 0x0001 | Y1 | -32767~32768 | Finger 1 Y displacement |
| 2 | 0x0002 | Z1 | -32767~32769 | Finger 1 Z displacement |
| 3 | 0x0003 | X2 | -32767~32770 | Finger 2 X displacement |
| 4 | 0x0004 | Y2 | -32767~32771 | Finger 2 Y displacement |
| 5 | 0x0005 | Z2 | -32767~32772 | Finger 2 Z displacement |
| 6 | 0x0006 | Status | 0/1/2/3 | 0=idle, 1=gripping, 2=waiting, 3=held |
| 7 | 0x0007 | Hardness | 0~6000 | Object hardness detection |
| 8 | 0x0008 | Position | 0~100 | Real-time position feedback |
| 9 | 0x0009 | Current | 0~1400 | Current feedback |

### Combined Launch

Launch both controller and monitor simultaneously:

```bash
ros2 launch hkv_gripper_controller gripper_control_with_monitor.launch.py serial_port:=/dev/ttyUSB0 baud_rate:=1000000
```

## Development

### Building

```bash
cd ~/ros2_ws
colcon build --packages-select hkv_gripper_controller gripper_controller_interfaces --symlink-install
```

## References

- [ros2_robotiq_gripper](https://github.com/PickNikRobotics/ros2_robotiq_gripper) - Architecture reference
- [ros2_control documentation](https://control.ros.org/)
- [hardware_interface documentation](https://control.ros.org/master/doc/getting_started/getting_started.html)

## License

AGPL-3.0