diff --git a/pose_estimation/launch/pose_estimation.launch b/pose_estimation/launch/pose_estimation.launch new file mode 100644 index 0000000000000000000000000000000000000000..083ab2efc072d6bae3344c18f4575adddddf7647 --- /dev/null +++ b/pose_estimation/launch/pose_estimation.launch @@ -0,0 +1,4 @@ +<launch> + <node name="arm_keypoint_capture" pkg="pose_estimation" type="arm_keypoint_capture.py" output="screen"/> + <node name="pose_estimation_control" pkg="pose_estimation" type="pose_estimation_control.py" output="screen"/> +</launch> diff --git a/pose_estimation/msg/Keypoint.msg b/pose_estimation/msg/Keypoint.msg new file mode 100644 index 0000000000000000000000000000000000000000..a2918cdd3d9a555afe1590b321ed760e4bd5703a --- /dev/null +++ b/pose_estimation/msg/Keypoint.msg @@ -0,0 +1,5 @@ +int32 landmark +float64 x +float64 y +float64 z +float64 visibility \ No newline at end of file diff --git a/pose_estimation/msg/Keypoints.msg b/pose_estimation/msg/Keypoints.msg new file mode 100644 index 0000000000000000000000000000000000000000..e5f9f39c82415cd6c43c6e33b34c2aedebceff81 --- /dev/null +++ b/pose_estimation/msg/Keypoints.msg @@ -0,0 +1 @@ +Keypoint[] keypoints \ No newline at end of file diff --git a/pose_estimation/msg/SphericalCoordinates.msg b/pose_estimation/msg/SphericalCoordinates.msg new file mode 100644 index 0000000000000000000000000000000000000000..7e14a368a901bfbc4913ec75a16b0e1e2ac7e86e --- /dev/null +++ b/pose_estimation/msg/SphericalCoordinates.msg @@ -0,0 +1,3 @@ +float64 distance +float64 theta +float64 phi \ No newline at end of file diff --git a/pose_estimation/scripts/arm_keypoint_capture.py b/pose_estimation/scripts/arm_keypoint_capture.py new file mode 100755 index 0000000000000000000000000000000000000000..782138acc7b1aad393ac0e44039580daada6d303 --- /dev/null +++ b/pose_estimation/scripts/arm_keypoint_capture.py @@ -0,0 +1,218 @@ +############################## TODO ################################ +# 1) the angle closer to the table not accurate + + + +# TESTS: +# - y (depth) -> poor accuracy +# - z (height) -> better accuracy but gets out of range +# - x (width) -> the range is too small + +### VERIFY: spherical to coordinates + +# IMPROVEMENTS: How often do we update the arm? How long to move the arm? Which IKs won't be solved +# GENERAL: Global variables, comments and structure +#################################################################### + + +import mediapipe as mp +import numpy as np +import rospy +import math +import cv2 +import vg +import logging + +from mediapipe.framework.formats import landmark_pb2 + +### Importing messages +from pose_estimation.msg import SphericalCoordinates + +### Mediapipe drawing +mp_drawing = mp.solutions.drawing_utils +mp_drawing_styles = mp.solutions.drawing_styles +mp_ds = mp_drawing_styles.get_default_pose_landmarks_style() + +### Mediapipe Pose Estimation +mp_pose = mp.solutions.pose + +### Unit axis vectors +x_axis = np.array([1, 0, 0]) +y_axis = np.array([0, 1, 0]) +z_axis = np.array([0, 0, 1]) + +# TODO: Define in the other file (prove mathematically) +### Ranges +mp_distance_range = (0.0,0.53) +robot_distance_range = (0.14, 0.46) +# robot_distance_range = (0.14, 0.46) + +### Threshold +VISIBILITY_THRESHOLD = 0.2 + +### Arm Landmarks +wrist_keypoint = mp_pose.PoseLandmark.RIGHT_WRIST.value +shoulder_keypoint = mp_pose.PoseLandmark.RIGHT_SHOULDER.value + +arm_landmarks = [ + shoulder_keypoint, + mp_pose.PoseLandmark.RIGHT_ELBOW.value, + wrist_keypoint, +] + +### Checks if the keypoint is relatively visible +def verify_visibility(keypoint, visibility_threshold): + if keypoint.visibility <= visibility_threshold: + return False + + return True + +### Adjusts values to a new range +def adjust_range(old_min, old_max, new_min, new_max, old_value): + old_range = old_max - old_min + new_range = new_max - new_min + + return (((old_value - old_min) * new_range) / old_range) + new_min + + +def euclid_dist(vec1, vec2): + if vec1.size != vec2.size: + print("Size of the two points doesn't match") + return None + + sum_ = 0 + for i in range(vec1.size): + squared_difference = (vec1[i] - vec2[i])**2 + sum_ += squared_difference + + return math.sqrt(sum_) + + +### Convert keypoint locations to a numpy array +def get_keypoint_as_np(keypoint): + return np.array([keypoint.x, keypoint.y, keypoint.z]) + +### Method used for publishing the spherical coordinates +def publish_keypoint(pub, results): + shoulder = np.array([]) + spherical_coordinates = None + + for idx, keypoint in enumerate(results.pose_world_landmarks.landmark): + if idx == shoulder_keypoint: + if not verify_visibility(keypoint=keypoint, visibility_threshold = VISIBILITY_THRESHOLD): + rospy.logwarn("Shoulder keypoint not visible") + break + + shoulder = get_keypoint_as_np(keypoint) + + if idx == wrist_keypoint and shoulder.size != 0: + if not verify_visibility(keypoint=keypoint, visibility_threshold = VISIBILITY_THRESHOLD): + rospy.logwarn("Wrist keypoint not visible") + break + + wrist = get_keypoint_as_np(keypoint) + + shifted_keypoint = wrist - shoulder + # Checking if the keypoint is to the left side of the shoulder, i.e. if x is positive (because the image is inverted, otherwise it should be positive) + if shifted_keypoint[0] > 0: + rospy.logerr("Wrist has to be to the right of the shoulder") + break + + + # Calculating the angle between the keypoint and the z-axis (depth) + theta = vg.angle(shifted_keypoint, z_axis) + # Calculating the angle between the keypoint and the y-axis (height) + # Scaling the angle which defines the height to mimic the robotic arm's range of movement + + phi = vg.angle(shifted_keypoint, y_axis) + ## TODO: Global variable + if phi < 30 or phi > 150: + rospy.logwarn("Invalid angle") + break + + phi = adjust_range(60, 140, 30, 80, phi) + + + distance = euclid_dist(np.array([0,0,0]), shifted_keypoint) + + # Scaling the range of the distance from mediapipe to manipulator + dist = adjust_range(mp_distance_range[0], mp_distance_range[1], + robot_distance_range[0], robot_distance_range[1], + distance) + + + print(f"Calculated distance: {distance}") + + spherical_coordinates = SphericalCoordinates() + spherical_coordinates.theta = math.radians(theta) + spherical_coordinates.phi = math.radians(phi) + spherical_coordinates.distance = dist + + elif idx == wrist_keypoint and shoulder.size == 0: + rospy.logwarn("Shoulder keypoint missing") + + if spherical_coordinates: + pub.publish(spherical_coordinates) + + +def skeleton_estimation_pose(pub, img, pose, data_collection = True): + results = pose.process(cv2.cvtColor(img, cv2.COLOR_BGR2RGB)) + + + pose_landmarks = results.pose_landmarks + try: + arm = [results.pose_landmarks.landmark[arm_keypoint] for arm_keypoint in arm_landmarks] + pose_landmarks = landmark_pb2.NormalizedLandmarkList(landmark = arm) + except Exception as e: + pass + + mp_drawing.draw_landmarks( + img, + pose_landmarks, + landmark_drawing_spec=mp_ds + ) + + cv2.imshow('Pose', cv2.flip(img,1)) + + if data_collection and results.pose_world_landmarks: + publish_keypoint(pub=pub, results=results) + + +def video_capture(): + node = 'arm_keypoint_capture' + rospy.init_node(node) + pub = rospy.Publisher(node, SphericalCoordinates, queue_size=50) + + video = cv2.VideoCapture(0) + data_collection = False + + with mp_pose.Pose(static_image_mode=False, min_detection_confidence=0.5, model_complexity=2, enable_segmentation=True) as pose: + + print("Press 'E' to capture the keypoints") + + while not rospy.is_shutdown(): + _, capture = video.read() + + skeleton_estimation_pose(pub, capture, pose, data_collection) + + key = cv2.waitKey(1) + if key == ord('e'): + data_collection = not data_collection + if data_collection: + rospy.loginfo("Publishing the data on 'arm_keypoint_capture'") + else: + rospy.loginfo("Publishing of data stopped") + + # TODO: Quit ROS + if key == ord('q'): + rospy.logwarn("Closing the program") + break + + video.release() + cv2.destroyAllWindows() + +if __name__ == "__main__": + try: + video_capture() + except rospy.ROSInterruptException as e: + rospy.logfatal(f"Following exception caused the program to stop: {e}") diff --git a/pose_estimation/scripts/pose_estimation_control.py b/pose_estimation/scripts/pose_estimation_control.py new file mode 100755 index 0000000000000000000000000000000000000000..38579a59aa4a203653ff97dedf6fdbae2f55b5bf --- /dev/null +++ b/pose_estimation/scripts/pose_estimation_control.py @@ -0,0 +1,110 @@ +import rospy +import math +import itertools +import numpy as np + +from math import sin, cos + +### Importing the messages for OpenManipulator's joint movement +from open_manipulator_msgs.srv import SetJointPosition +from open_manipulator_msgs.msg import JointPosition + +from geometry_msgs.msg import Pose +from open_manipulator_msgs.srv import SetKinematicsPose +from open_manipulator_msgs.msg import KinematicsPose + +### Importing the messages for Spherical Coordinates +from pose_estimation.msg import SphericalCoordinates + + +### Defining available joints by name +JOINT_NAMES = ["joint1", "joint2", "joint3", "joint4"] + +### Variables for moving the gripper +END_EFFECTOR_NAME = "gripper" +PATH_TIME = 2 + +# ### Define starting joint positions +START_LOCATION = { + 'x': 0.286, + 'y': 0.0, + 'z': 0.204, +} + +# Calculating the distance between two points +def dis(point1, point2): + return math.sqrt((point2[0] - point1[0])**2 + (point2[1] - point1[1])**2) + + +# SOURCES: https://math.libretexts.org/Bookshelves/Calculus/Book%3A_Calculus_(OpenStax)/12%3A_Vectors_in_Space/12.07%3A_Cylindrical_and_Spherical_Coordinates +def spherical_to_cartesian(d, theta, phi): + # TODO: Prove why change of cos and sin + # x = d * sin(phi) * cos(theta) + # y = d * sin(phi) * sin(theta) + # z = d * cos(phi) + x = d * sin(phi) * sin(theta) + y = d * sin(phi) * cos(theta) + z = d * sin(phi) + + print(f"Distance: {d}") + coordinates = { + "x" : START_LOCATION['x'], + "y" : START_LOCATION['y'], + "z" : z + } + + return coordinates + + +def set_position(target_location): + set_kinematics_pose = rospy.ServiceProxy('goal_task_space_path_position_only', SetKinematicsPose) + + pose = Pose() + pose.position.x = target_location['x'] + pose.position.y = target_location['y'] + pose.position.z = target_location['z'] + + kinematics_pose = KinematicsPose() + kinematics_pose.pose = pose + + resp = set_kinematics_pose(end_effector_name = END_EFFECTOR_NAME, kinematics_pose=kinematics_pose, path_time = PATH_TIME) + + if not resp.is_planned: + rospy.logerr("Failed to sovle IK equation") + print(f"X: {target_location['x']}; Y: {target_location['y']}; Z: {target_location['z']}") + + +def video_capture(coordinate): + # TODO: Exception and stop program if connections fails + rospy.wait_for_service('goal_task_space_path_position_only', 2) + + # Converting the spherical coordiantes received from pose estimation to cartesian coordinates + target_location = spherical_to_cartesian(coordinate.distance, coordinate.theta, coordinate.phi) + + try: + # Setting the location of the robotic arm + set_position(target_location) + except rospy.ServiceException as e: + print("Service call failed %s"%e) + + # Sleep equal to the time needed for the arm to move to the desired location + rospy.sleep(PATH_TIME) + +def pose_estimation(): + rospy.init_node('pose_estimation_control') + + # Moving the arm to the starting position + rospy.loginfo("Moving the arm to the starting location") + set_position(START_LOCATION) + + print("Subscribing to `arm_keypoint_capture`") + rospy.Subscriber("arm_keypoint_capture", SphericalCoordinates, video_capture, queue_size = 1) + + rospy.spin() + + +if __name__ == '__main__': + try: + pose_estimation() + except rospy.ROSInterruptException as e: + print("Exception: " + e)