Forward kinematics was used to compute the end-effector pose from joint angles, enabling verification of inverse kinematics solutions and ensuring accurate execution of pick-and-place motions. In this project, the screw axes (w_i, v_i) define each UR3 joint’s rotation axis and linear component in the space frame, enabling forward kinematics to be computed as implemented in the lab kinematics functions.

Inverse kinematics was used whenever the robot needed to move the end-effector to a target block location or placement position defined in world coordinates. After detecting block positions from the camera and transforming them into the world frame, the project computed an elbow-up inverse kinematics solution for the UR3 arm based on its mechanical schematic.

Using the known lengths and offsets of each UR3 link, the wrist center was first calculated by compensating for the gripper geometry and desired end-effector yaw. The shoulder and elbow joint angles were then solved geometrically using trigonometric relationships derived from the robot’s link lengths, and the remaining wrist joints were computed to satisfy the target orientation, enabling accurate and repeatable pick-and-place motions.

Block perception was implemented using a camera-based vision pipeline in Python to detect colored blocks and localize them for manipulation. Camera images were processed using OpenCV and NumPy, where images were converted to the HSV color space and thresholded to segment target block colors. Blob detection was then applied using OpenCV’s SimpleBlobDetector to extract the centroids of valid block regions while filtering out noise and small artifacts.

raw_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
cv_image = cv2.flip(raw_image, -1)

xw_yw_G = blob_search(cv_image, "green")
xw_yw_Y = blob_search(cv_image, "yellow")

def blob_search(image_raw, color):

    # Setup SimpleBlobDetector parameters.
    params = cv2.SimpleBlobDetector_Params()

    # Filter by Area.
    params.filterByArea = True
    params.minArea = 50

    # Create a detector with the parameters
    detector = cv2.SimpleBlobDetector_create(params)

    # Convert the image into the HSV color space
    hsv_image = cv2.cvtColor(image_raw, cv2.COLOR_BGR2HSV)

    if color == "yellow":
        lower = (20, 100, 100)
        upper = (30, 255, 255)

    if color == "green":
        lower = (45, 60, 55)
        upper = (70, 255, 255)

    mask_image = cv2.inRange(hsv_image, lower, upper)

    keypoints = detector.detect(mask_image)

    # Find blob centers in the image coordinates
    blob_image_center = []
    num_blobs = len(keypoints)
    for i in range(num_blobs):
        blob_image_center.append((keypoints[i].pt[0], keypoints[i].pt[1]))

The detected block centroids in pixel coordinates were subsequently transformed into world-frame coordinates using a calibrated image-to-world mapping. This geometric transformation enabled the vision system to interface directly with the robot’s inverse kinematics solver, providing accurate target positions for autonomous pick-and-place execution.

# Params for camera calibration
theta = 0.026660348374597458
beta = 0.001332267944962386
tx = 271 * beta + 0.01  # m
ty = 44 * beta          # m

# Function that converts image coord to world coord
def IMG2W(col, row):
    Or = row
    Oc = col

    xc = beta * Or
    yc = beta * Oc

    Rcw = np.array([[np.cos(theta), -np.sin(theta), 0],
                    [np.sin(theta),  np.cos(theta), 0],
                    [0,              0,             1]])

    xc = xc - ty
    yc = yc - tx

    pw = Rcw @ np.array([xc, yc, 0])
    return [pw[0], pw[1]]

The ROS software architecture was designed to decouple perception, kinematics, and control to support real-time operation and robust data flow.

• Used ROS image callbacks to process camera data independently from arm motion, enabling smooth and reliable execution.
• Ensured continuous data flow from perception through kinematics to motion execution without blocking or race conditions

During testing, there were moments when a poorly chosen joint trajectory caused the robot arm to swing uncomfortably close to surrounding lab equipment, including the monitor. That experience made it clear to me that solving kinematics alone is not enough, but how the robot moves between poses is just as important as reaching the target safely.

I also realized that certain arm configurations unintentionally blocked the overhead camera’s view of the workspace, which disrupted block detection. This taught me that effective manipulation requires coordinating motion planning with perception constraints, and it motivated me to learn more about efficient, collision-aware trajectory planning to make the system both faster and more reliable.