Surface Painting Robot (3D Perception, Planning, Inverse Kinematics)

About This Project

The system scans an arbitrary curved surface, automatically plans a coverage path, and executes the trajectory while keeping the paint brush perpendicular to the surface.

This project was part of ENPM662 - Introduction to Robot Modeling course at University of Maryland. I worked on this with teammates Pranav, Akshun and Mathew. The goal was to build a complete perception-to-execution pipeline for surface painting using a UR5 robot arm.

GitHub: paint_cloud

Quick Demo

---

System Architecture

The system follows a Sense → Plan → Execute pipeline:

1. Sense: 3D LiDAR scans the environment and generates a point cloud
2. Plan: Compute surface normals, generate 2D coverage pattern, project onto 3D surface
3. Execute: Solve Inverse Kinematics to guide the end-effector along the path

PointCloud (LiDAR) → Surface Normals (Open3D) → Coverage Path (Zig-Zag) 
    → 3D Projection (KD-Tree) → IK Solver (Jacobian) → Joint Trajectory

---

Path Planning Node

The paint_cloud node handles perception and path generation.

Point Cloud Processing

The node subscribes to LiDAR point cloud data and transforms it to the world frame:

# Transform points from sensor frame to world frame
trans = self.tf_buffer.lookup_transform('world', msg.header.frame_id, rclpy.time.Time())

# Apply transform: P_world = R * P_sensor + T
rotation_matrix = tf_transformations.quaternion_matrix(quaternion)[:3, :3]
new_points = np.dot(new_points, rotation_matrix.T) + translation

The code supports both RealSense depth cameras (24-byte point format) and LiDAR sensors (16-byte format with intensity).

Surface Normal Estimation

Using Open3D, we estimate normals at each point:

# Create Open3D PointCloud and estimate normals
self.pcd = o3d.geometry.PointCloud()
self.pcd.points = o3d.utility.Vector3dVector(self.points)

self.pcd.estimate_normals(
    search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.15, max_nn=30)
)

# Orient all normals to point upward (positive Z)
self.pcd.orient_normals_to_align_with_direction(
    orientation_reference=np.array([0.0, 0.0, 1.0])
)

Coverage Path Generation

The path planner generates a zig-zag pattern based on brush radius and overlap factor:

# Calculate step between lines: step = 2*radius / overlap_factor
step_x = (2.0 * brush_radius) / overlap_factor

# Generate zig-zag waypoints
while current_x <= path_x_max:
    if direction_up:
        waypoints.append((current_x, path_y_min))
        waypoints.append((current_x, path_y_max))
    else:
        waypoints.append((current_x, path_y_max))
        waypoints.append((current_x, path_y_min))
    
    direction_up = not direction_up
    current_x += step_x

3D Projection of 2D Path

The 2D path is projected onto the actual curved surface using nearest-neighbor search:

# Build KD-Tree from point cloud (x, y only)
self.kd_tree = cKDTree(self.points[:, :2])

for p in final_points:
    # Find nearest surface point
    dist, idx = self.kd_tree.query([p[0], p[1]])
    
    # Get surface height and normal
    surface_point = self.points[idx]
    normal = normals[idx]
    
    # Offset position along normal by brush radius
    offset_pos = surface_point + normal * brush_radius
    
    # Compute quaternion to align tool with surface normal
    orientation = quaternion_from_normal(normal)

---

Inverse Kinematics Controller

The jacobian_trajectory node solves IK and controls the UR5 arm.

UR5 DH Parameters

We use the standard Denavit-Hartenberg parameters for the UR5:

# UR5 DH Parameters: [a, alpha, d, theta_offset]
self.dh_params = [
    [0,         np.pi/2,  0.089159, 0],  # Joint 1
    [-0.425,    0,        0,        0],  # Joint 2
    [-0.39225,  0,        0,        0],  # Joint 3
    [0,         np.pi/2,  0.10915,  0],  # Joint 4
    [0,        -np.pi/2,  0.09465,  0],  # Joint 5
    [0,         0,        0.0823,   0]   # Joint 6
]

Jacobian-Based IK Solver

We implemented Newton-Raphson iteration with the Jacobian pseudo-inverse:

def solve_ik(self, target_pos, initial_joints, tolerance=0.01, max_iter=100):
    q = np.array(initial_joints, dtype=float)
    
    for _ in range(max_iter):
        current_pos, transforms = self.forward_kinematics(q)
        error_pos = target_pos - current_pos
        
        # Check convergence
        if np.linalg.norm(error_pos) < tolerance:
            return q
        
        # Compute Jacobian and its pseudo-inverse
        J = self.compute_jacobian(transforms)
        J_pinv = np.linalg.pinv(J[:3, :])
        
        # Update joint angles
        delta_q = np.dot(J_pinv, error_pos)
        q += delta_q * 0.5  # Damping factor
    
    return q

Geometric Jacobian Computation

The 6×6 Jacobian relates joint velocities to end-effector velocities:

def compute_jacobian(self, transforms):
    J = np.zeros((6, 6))
    p_e = transforms[-1][:3, 3]  # End-effector position
    
    z_prev = np.array([0, 0, 1])  # Base Z-axis
    p_prev = np.array([0, 0, 0])  # Base origin
    
    for i in range(6):
        # Geometric Jacobian for revolute joints
        J[:3, i] = np.cross(z_prev, (p_e - p_prev))  # Linear velocity
        J[3:, i] = z_prev                             # Angular velocity
        
        z_prev = transforms[i][:3, 2]
        p_prev = transforms[i][:3, 3]
    
    return J

---

ROS 2 Service Interface

The nodes communicate via a custom ROS 2 service:

# paint_cloud_msgs/srv/GetPaintPath.srv
---
geometry_msgs/PoseArray poses

The IK controller calls this service to get the path:

# Request path from planning node
req = GetPaintPath.Request()
future = self.cli.call_async(req)
rclpy.spin_until_future_complete(self, future)

# Extract waypoints from response
for pose in response.poses.poses:
    points.append([pose.position.x, pose.position.y, pose.position.z])

---

Running the Code

# Clone and build
mkdir -p ~/paint_ws && cd ~/paint_ws
git clone https://github.com/AakashDammala/paint_cloud.git src/
rosdep install --from-paths src --ignore-src -r -y
pip3 install open3d scipy
colcon build && source install/setup.bash

# Launch simulation (Gazebo + RViz + UR5)
ros2 launch paint_cloud classic.launch.py

# Run path planning node
ros2 run paint_cloud paint_cloud

# Execute trajectory with IK solver
ros2 run my_ik_controller jacobian_mover

---

Configurable Parameters

Parameter	Default	Description
downsample_factor	50	Point cloud downsampling ratio
brush_radius	0.05	Brush radius in meters
overlap_factor	1.0	Path overlap (1.0 = no gap)
surface_length_x	0.15	Surface length in X
surface_width_y	0.3	Surface width in Y

---

What I Learned

1. Jacobian IK — The Newton-Raphson method with pseudo-inverse converges quickly for reasonable targets
2. Surface normals — Keeping the tool perpendicular to the surface is critical for painting applications
3. KD-Trees — Nearest neighbor search on point clouds is essentially O(log n)

Check out the GitHub repo for the full code!