Skip to content
Snippets Groups Projects
Commit b727e500 authored by Sitarz, Michal A (UG - Comp Sci & Elec Eng)'s avatar Sitarz, Michal A (UG - Comp Sci & Elec Eng)
Browse files

Moving the robotic arm with keypoints; currently successful only in z-axis

parent cc720b39
No related branches found
No related tags found
No related merge requests found
<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>
int32 landmark
float64 x
float64 y
float64 z
float64 visibility
\ No newline at end of file
Keypoint[] keypoints
\ No newline at end of file
float64 distance
float64 theta
float64 phi
\ No newline at end of file
############################## 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}")
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)
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment