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)