[jsk_pr2_accessories] Generate april-tag image and plate stl file for hand calibration

[HiroIshida]

Motivation

PR2's calibrations (intrinsics/extrinsics/kinematics) are very challenging.
A simple but effective workaround is to attach markers to the PR2's gripper and compare the pose calculated by nominal kinematics with the detected pose.
We have been using yellow tape as markers, but it's difficult to maintain their position and orientation consistently.
Therefore, we decided to create a 3D-printed tag plate for the PR2's gripper.

Requirements

Software:

sudo apt install texlive-base openscad

Hardware:

• スペーサM3オスメス六角10mm (x2) (Recommended to use plastic spacers to not damage the aluminum lid)
• 六角穴付きボルトM3-10mm (x2)

Assebly instructions

• Then run make command in this directory.
• Print the generated PDF file under build/april_tag_print.pdf
• 3D print the generated STL file under build/tag_plate.stl
• Remove the two bolts from the lid of PR2's gripper's back side. Don't remove the lid.
• Attach the printed tag plate to the lid using the two bolts with the spacers.
• Paste the printed tag on the tag plate. The printed tag size is supposed to be exactly equal to the tag plate size.

See the images here for the reference:

Example usage

• launch example/kinect_april_tag_detection.launch inside PR2's machine.
• Check tf tree to see apriltag frame and apriltag_fk frame to see the nominal and actual april tag pose
  

Node example

* Because april tag's detection's orientation's fluctuates a lot, we need to filter it

import time
from typing import List, Optional, Tuple

import numpy as np
import rospy
import tf2_ros
from skrobot.coordinates.math import quaternion2rpy


class PoseQueue:
    queue: List[Tuple[np.ndarray, rospy.Time]]
    window: float = 5.0

    def __init__(self, max_size: int, position_only: bool):
        self.queue = []
        self.max_size = max_size
        self.position_only = position_only  # because rotation is super noisy

    def push(self, pose: np.ndarray, timestamp: rospy.Time):
        if self.position_only:
            assert len(pose) == 3
        if len(self.queue) >= self.max_size:
            self.queue.pop(0)
        self.queue.append((pose, timestamp))

    def get_mean(
        self, timestamp: rospy.Time, outlier_th_sigma: float = 1.0
    ) -> Optional[np.ndarray]:
        oldest_timestamp = self.queue[0][1]
        elapsed_from_oldest = (timestamp - oldest_timestamp).to_sec()
        is_too_old = elapsed_from_oldest > self.window
        if is_too_old:
            rospy.logwarn(f"(get_mean) data is too old. elapsed: {elapsed_from_oldest}")
            return None

        poses = np.array([pose for pose, _ in self.queue])

        # Because, calibration error is not so big, we can remove
        # outlier by simple thresholding.
        if self.position_only:
            is_outlier = np.any(np.abs(poses) > 0.1, axis=1)
        else:
            is_pos_outlier = np.any(np.abs(poses[:, :3]) > 0.1, axis=1)
            is_ypr_outlier = np.any(np.abs(poses[:, 3:]) > np.deg2rad(30), axis=1)
            is_outlier = np.logical_or(is_pos_outlier, is_ypr_outlier)

        rate_of_outlier = np.sum(is_outlier) / len(is_outlier)
        if rate_of_outlier > 0.6:
            rospy.logwarn(f"rate of outlier is too high: {rate_of_outlier}")
            return None

        poses = poses[~is_outlier]
        std = np.std(poses, axis=0)
        if self.position_only:
            is_stable = np.all(std < 0.003)
        else:
            position_std, ypr_std = std[:3], std[3:]
            is_stable = np.all(position_std < 0.003) and np.all(ypr_std < np.deg2rad(3))

        if not is_stable:
            rospy.logwarn(
                f"(get_mean) data is not stable. position_std: {position_std}, ypr_std: {ypr_std}"
            )
            return None

        mean = np.mean(poses, axis=0)
        return mean


class AprilOffsetDetector:
    def __init__(
        self,
        tf_lb: Optional[Tuple[tf2_ros.Buffer, tf2_ros.TransformListener]] = None,
        position_only: bool = False,
    ):
        if tf_lb is None:
            buffer = tf2_ros.Buffer()
            listener = tf2_ros.TransformListener(buffer)
            self.tf_buffer = buffer
            self.tf_listener = listener
        else:
            self.tf_buffer = tf_lb[0]
            self.tf_listener = tf_lb[1]
        self.tf_listener.unregister
        self.pose_queue = PoseQueue(30, position_only=position_only)
        self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_callback)
        self.position_only = position_only

    def timer_callback(self, event):
        transform = self.tf_buffer.lookup_transform(
            "apriltag_fk", "apriltag", rospy.Time(0), rospy.Duration(1.0)
        )
        pose = transform.transform
        translation = np.array([pose.translation.x, pose.translation.y, pose.translation.z])
        if self.position_only:
            self.pose_queue.push(translation, transform.header.stamp)
        else:
            quaternion = np.array(
                [
                    pose.rotation.w,
                    pose.rotation.x,
                    pose.rotation.y,
                    pose.rotation.z,
                ]
            )
            ypr = quaternion2rpy(quaternion)[0]
            vec = np.hstack([translation, ypr])
            self.pose_queue.push(vec, transform.header.stamp)

    def get_gripper_offset(self) -> Tuple[np.ndarray, np.ndarray]:
        timeout = 5.0
        ts = time.time()
        while True:
            mean = self.pose_queue.get_mean(rospy.Time.now())
            if mean is not None:
                break
            rospy.sleep(0.1)
            if time.time() - ts > timeout:
                rospy.logerr("Timeout")
                raise TimeoutError
        if self.position_only:
            position = mean
            rpy = np.zeros(3)
        else:
            position = mean[:3]
            ypr = mean[3:]
            rpy = ypr[::-1]
        return position, rpy


if __name__ == "__main__":
    rospy.init_node("gripper_offset_detector")
    detector = AprilOffsetDetector(position_only=False)
    time.sleep(2)
    print("call")
    ts = time.time()
    position, rpy = detector.get_gripper_offset()
    print(f"time: {time.time() - ts}")