From 9f77c9c43f50ccd3d214113820d2e2b3a455d57a Mon Sep 17 00:00:00 2001 From: Chenzui Li Date: Thu, 7 Nov 2024 06:47:24 +0000 Subject: [PATCH 1/2] Add human ergonomic and manipulability calculation --- examples/visualab/example_ergo_manip.py | 74 ++++++++++ rofunc/__init__.py | 2 +- rofunc/utils/ergolab/__init__.py | 9 ++ .../utils/ergolab/reba_calculate/__init__.py | 0 .../reba_calculate/degree2reba/__init__.py | 0 .../degree2reba/lower_arm_reba_score.py | 49 +++++++ .../degree2reba/trunk_reba_score.py | 50 +++++++ .../degree2reba/upper_arm_reba_score.py | 137 ++++++++++++++++++ .../reba_calculate/pose2degree/Util.py | 53 +++++++ .../reba_calculate/pose2degree/__init__.py | 0 .../pose2degree/body_part_sorting.py | 30 ++++ .../pose2degree/lower_arm_degree.py | 23 +++ .../pose2degree/trunk_degree.py | 64 ++++++++ .../pose2degree/upper_arm_degree.py | 116 +++++++++++++++ rofunc/utils/maniplab/__init__.py | 2 + .../maniplab/manipulability_calculator.py | 53 +++++++ rofunc/utils/maniplab/read_skeleton_motion.py | 5 + rofunc/utils/robolab/kinematics/__init__.py | 2 +- rofunc/utils/visualab/__init__.py | 1 + rofunc/utils/visualab/ergo_manip_plot.py | 52 +++++++ 20 files changed, 720 insertions(+), 2 deletions(-) create mode 100644 examples/visualab/example_ergo_manip.py create mode 100644 rofunc/utils/ergolab/reba_calculate/__init__.py create mode 100644 rofunc/utils/ergolab/reba_calculate/degree2reba/__init__.py create mode 100644 rofunc/utils/ergolab/reba_calculate/degree2reba/lower_arm_reba_score.py create mode 100644 rofunc/utils/ergolab/reba_calculate/degree2reba/trunk_reba_score.py create mode 100644 rofunc/utils/ergolab/reba_calculate/degree2reba/upper_arm_reba_score.py create mode 100644 rofunc/utils/ergolab/reba_calculate/pose2degree/Util.py create mode 100644 rofunc/utils/ergolab/reba_calculate/pose2degree/__init__.py create mode 100644 rofunc/utils/ergolab/reba_calculate/pose2degree/body_part_sorting.py create mode 100644 rofunc/utils/ergolab/reba_calculate/pose2degree/lower_arm_degree.py create mode 100644 rofunc/utils/ergolab/reba_calculate/pose2degree/trunk_degree.py create mode 100644 rofunc/utils/ergolab/reba_calculate/pose2degree/upper_arm_degree.py create mode 100644 rofunc/utils/maniplab/__init__.py create mode 100644 rofunc/utils/maniplab/manipulability_calculator.py create mode 100644 rofunc/utils/maniplab/read_skeleton_motion.py create mode 100644 rofunc/utils/visualab/ergo_manip_plot.py diff --git a/examples/visualab/example_ergo_manip.py b/examples/visualab/example_ergo_manip.py new file mode 100644 index 000000000..de175a9a3 --- /dev/null +++ b/examples/visualab/example_ergo_manip.py @@ -0,0 +1,74 @@ +import numpy as np +import rofunc as rf +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation + + +def update(frame): + ax.clear() + ax.set_xlim(-1, 1) + ax.set_ylim(-1, 1) + ax.set_zlim(0, 2) + ax.set_xlabel('X') + ax.set_ylabel('Y') + ax.set_zlabel('Z') + joint_rotations = skeleton_joint[frame] + global_positions, global_rotations = rf.maniplab.forward_kinematics(skeleton_joint_local_translation, joint_rotations, skeleton_parent_indices) + rf.visualab.plot_skeleton(ax, global_positions, skeleton_parent_indices) + right_hand_index = 5 + left_hand_index = 8 + jacobian_right = np.squeeze(jacobians_right[frame]) + eigenvalues_right, eigenvectors_right = rf.maniplab.calculate_manipulability(jacobian_right) + rf.visualab.plot_ellipsoid(ax, eigenvalues_right, eigenvectors_right, global_positions[right_hand_index], 'b') + jacobian_left = np.squeeze(jacobians_left[frame]) + eigenvalues_left, eigenvectors_left = rf.maniplab.calculate_manipulability(jacobian_left) + rf.visualab.plot_ellipsoid(ax, eigenvalues_left, eigenvectors_left, global_positions[left_hand_index], 'r') + + rf.visualab.plot_skeleton(ax, global_positions, skeleton_parent_indices) + + # Define a structure to map joint types to their corresponding functions and indices + joint_analysis = { + 'upper_arm': { + 'indices': [skeleton_joint_name.index('right_upper_arm'), skeleton_joint_name.index('left_upper_arm')], + 'degree_func': rf.ergolab.UpperArmDegree(global_positions).upper_arm_degrees, + 'reba_func': rf.ergolab.UAREBA, + 'score_func': lambda reba: reba.upper_arm_reba_score(), + 'color_func': rf.visualab.ua_get_color + }, + 'lower_arm': { + 'indices': [skeleton_joint_name.index('right_lower_arm'), skeleton_joint_name.index('left_lower_arm')], + 'degree_func': rf.ergolab.LADegrees(global_positions).lower_arm_degree, + 'reba_func': rf.ergolab.LAREBA, + 'score_func': lambda reba: reba.lower_arm_score(), + 'color_func': rf.visualab.la_get_color + }, + 'trunk': { + 'indices': [skeleton_joint_name.index('pelvis')], + 'degree_func': rf.ergolab.TrunkDegree(global_positions, global_rotations).trunk_degrees, + 'reba_func': rf.ergolab.TrunkREBA, + 'score_func': lambda reba: reba.trunk_reba_score(), + 'color_func': rf.visualab.trunk_get_color + } + } + + # Process each joint type with its corresponding REBA analysis and coloring + for joint_type, settings in joint_analysis.items(): + degrees = settings['degree_func']() + reba = settings['reba_func'](degrees) + scores = settings['score_func'](reba) + for i, idx in enumerate(settings['indices']): + score = scores[i] + color = settings['color_func'](score) + ax.scatter(*global_positions[idx], color=color, s=100) # Use scatter for single points + + +if __name__ == '__main__': + skeleton_joint_name, skeleton_joint, skeleton_parent_indices, skeleton_joint_local_translation = rf.maniplab.read_skeleton_motion('/home/ubuntu/Ergo-Manip/data/demo_2_test_chenzui_only_optitrack2hotu.npy') + skeleton_joint = skeleton_joint[::40, :, :] + jacobians_left = np.load('/home/ubuntu/Ergo-Manip/data/jacobian_data/jacobians_left_hand_2.npy', allow_pickle=True)[::10] + jacobians_right = np.load('/home/ubuntu/Ergo-Manip/data/jacobian_data/jacobians_right_hand_2.npy', allow_pickle=True)[::10] + fig = plt.figure() + ax = fig.add_subplot(111, projection='3d') + ani = FuncAnimation(fig, update, frames=len(skeleton_joint), repeat=True) + # ani.save('/home/ubuntu/Ergo-Manip/data/gif/demo_2_andrew.gif', writer='imagemagick', fps=3) + plt.show() \ No newline at end of file diff --git a/rofunc/__init__.py b/rofunc/__init__.py index a22a61fdd..bdbe1fe8c 100644 --- a/rofunc/__init__.py +++ b/rofunc/__init__.py @@ -22,7 +22,7 @@ from .learning import ml from .learning import RofuncIL, RofuncRL from .planning_control import lqt, lqr -from .utils import visualab, robolab, logger, oslab +from .utils import visualab, robolab, logger, oslab, ergolab, maniplab from .utils.datalab import primitive, data_generator from . import config diff --git a/rofunc/utils/ergolab/__init__.py b/rofunc/utils/ergolab/__init__.py index e69de29bb..559e16035 100644 --- a/rofunc/utils/ergolab/__init__.py +++ b/rofunc/utils/ergolab/__init__.py @@ -0,0 +1,9 @@ +from .reba_calculate.degree2reba.upper_arm_reba_score import * +from .reba_calculate.degree2reba.trunk_reba_score import * +from .reba_calculate.degree2reba.lower_arm_reba_score import * + +from .reba_calculate.pose2degree.trunk_degree import * +from .reba_calculate.pose2degree.lower_arm_degree import * +from .reba_calculate.pose2degree.upper_arm_degree import * +from .reba_calculate.pose2degree.Util import * +from .reba_calculate.pose2degree.body_part_sorting import * diff --git a/rofunc/utils/ergolab/reba_calculate/__init__.py b/rofunc/utils/ergolab/reba_calculate/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/rofunc/utils/ergolab/reba_calculate/degree2reba/__init__.py b/rofunc/utils/ergolab/reba_calculate/degree2reba/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/rofunc/utils/ergolab/reba_calculate/degree2reba/lower_arm_reba_score.py b/rofunc/utils/ergolab/reba_calculate/degree2reba/lower_arm_reba_score.py new file mode 100644 index 000000000..917ac3f70 --- /dev/null +++ b/rofunc/utils/ergolab/reba_calculate/degree2reba/lower_arm_reba_score.py @@ -0,0 +1,49 @@ +class LAREBA: + # For calculating REBA score based on degrees + def __init__(self, LA_degrees): + self.LA_degrees = LA_degrees + + def lower_arm_score_overall(self): + degree = self.LA_degrees + right_degree = degree[0] + left_degree = degree[1] + lower_arm_reba_score = 0 + if right_degree >= left_degree: + if 0 <= right_degree < 60: + lower_arm_reba_score = lower_arm_reba_score + 2 + if 60 <= right_degree < 100: + lower_arm_reba_score = lower_arm_reba_score + 1 + if 100 <= right_degree: + lower_arm_reba_score = lower_arm_reba_score + 1 + if right_degree < left_degree: + if 0 <= left_degree < 60: + lower_arm_reba_score = lower_arm_reba_score + 2 + if 60 <= left_degree < 100: + lower_arm_reba_score = lower_arm_reba_score + 1 + if 100 <= left_degree: + lower_arm_reba_score = lower_arm_reba_score + 1 + + return lower_arm_reba_score + + def lower_arm_score(self): + degree = self.LA_degrees + right_degree = degree[0] + left_degree = degree[1] + + right_lower_arm_reba_score = 0 + if 0 <= right_degree < 60: + right_lower_arm_reba_score = right_lower_arm_reba_score + 2 + if 60 <= right_degree < 100: + right_lower_arm_reba_score = right_lower_arm_reba_score + 1 + if 100 <= right_degree: + right_lower_arm_reba_score = right_lower_arm_reba_score + 1 + + left_lower_arm_reba_score = 0 + if 0 <= left_degree < 60: + left_lower_arm_reba_score = left_lower_arm_reba_score + 2 + if 60 <= left_degree < 100: + left_lower_arm_reba_score = left_lower_arm_reba_score + 1 + if 100 <= left_degree: + left_lower_arm_reba_score = left_lower_arm_reba_score + 1 + + return right_lower_arm_reba_score, left_lower_arm_reba_score \ No newline at end of file diff --git a/rofunc/utils/ergolab/reba_calculate/degree2reba/trunk_reba_score.py b/rofunc/utils/ergolab/reba_calculate/degree2reba/trunk_reba_score.py new file mode 100644 index 000000000..59288afb4 --- /dev/null +++ b/rofunc/utils/ergolab/reba_calculate/degree2reba/trunk_reba_score.py @@ -0,0 +1,50 @@ +class TrunkREBA: + # For calculating REBA score based on degrees + def __init__(self, trunk_degrees): + self.trunk_degrees = trunk_degrees + + def trunk_reba_score(self): + + trunk_flex_degree = self.trunk_degrees[0] + trunk_side_bending_degree = self.trunk_degrees[1] + trunk_torsion_degree = self.trunk_degrees[2] + + trunk_reba_score = 0 + trunk_flex_score = 0 + trunk_side_score = 0 + trunk_torsion_score = 0 + + if trunk_flex_degree >= 0: + # means flexion + if 0 <= trunk_flex_degree < 5: + trunk_reba_score += 1 + trunk_flex_score += 1 + elif 5 <= trunk_flex_degree < 20: + trunk_reba_score += 2 + trunk_flex_score += 2 + elif 20 <= trunk_flex_degree < 60: + trunk_reba_score += 3 + trunk_flex_score += 3 + elif 60 <= trunk_flex_degree: + trunk_reba_score += 4 + trunk_flex_score += 4 + else: + # means extension + if 0 <= abs(trunk_flex_degree) < 5: + trunk_reba_score += 1 + trunk_flex_score += 1 + elif 5 <= abs(trunk_flex_degree) < 20: + trunk_reba_score += 2 + trunk_flex_score += 2 + elif 20 <= abs(trunk_flex_degree): + trunk_reba_score += 3 + trunk_flex_score += 3 + + if abs(trunk_side_bending_degree) >= 3: + trunk_reba_score += 1 + trunk_side_score += 1 + if abs(trunk_torsion_degree) >= 1: + trunk_reba_score += 1 + trunk_torsion_score += 1 + + return [trunk_reba_score, trunk_flex_score, trunk_side_score, trunk_torsion_score] \ No newline at end of file diff --git a/rofunc/utils/ergolab/reba_calculate/degree2reba/upper_arm_reba_score.py b/rofunc/utils/ergolab/reba_calculate/degree2reba/upper_arm_reba_score.py new file mode 100644 index 000000000..ae1b4dd79 --- /dev/null +++ b/rofunc/utils/ergolab/reba_calculate/degree2reba/upper_arm_reba_score.py @@ -0,0 +1,137 @@ +class UAREBA: + # For calculating REBA score based on degrees + def __init__(self, UA_degrees): + self.UA_degrees = UA_degrees + + def upper_arm_reba_score_overall(self): + upper_arm_reba_score = 0 + upper_arm_flex_score = 0 + upper_arm_side_score = 0 + upper_arm_shoulder_rise = 0 + + + right_flexion = self.UA_degrees[0] + left_flexion = self.UA_degrees[1] + + right_side = self.UA_degrees[2] + left_side = self.UA_degrees[3] + + right_shoulder_rise = self.UA_degrees[4] + left_shoulder_rise = self.UA_degrees[5] + + if right_flexion >= left_flexion: + if -20 <= right_flexion < 20: + upper_arm_reba_score += 1 + upper_arm_flex_score += 1 + if 20 <= right_flexion < 45: + upper_arm_reba_score += 2 + upper_arm_flex_score += 2 + if right_flexion < -20: + upper_arm_reba_score += 2 + upper_arm_flex_score += 2 + if 45 <= right_flexion < 90: + upper_arm_reba_score += 3 + upper_arm_flex_score += 3 + if 90 <= right_flexion: + upper_arm_reba_score += 4 + upper_arm_flex_score += 4 + if right_flexion < left_flexion: + if -20 <= left_flexion < 20: + upper_arm_reba_score += 1 + upper_arm_flex_score += 1 + if left_flexion < -20: + upper_arm_reba_score += 2 + upper_arm_flex_score += 2 + if 20 <= left_flexion < 45: + upper_arm_reba_score += 2 + upper_arm_flex_score += 2 + if 45 <= left_flexion < 90: + upper_arm_reba_score += 3 + upper_arm_flex_score += 3 + if 90 <= left_flexion: + upper_arm_reba_score += 4 + upper_arm_flex_score += 4 + + # for side bending + if abs(right_side) > 2 or abs(left_side) > 2: + upper_arm_reba_score += 1 + upper_arm_side_score += 1 + + # for shoulder rise + + if right_shoulder_rise > 90 or left_shoulder_rise > 90: + upper_arm_reba_score += 1 + upper_arm_shoulder_rise += 1 + return [upper_arm_reba_score, upper_arm_flex_score, upper_arm_side_score, upper_arm_shoulder_rise] + + def upper_arm_reba_score(self): + right_upper_arm_reba_score = 0 + right_upper_arm_flex_score = 0 + right_upper_arm_side_score = 0 + right_upper_arm_shoulder_rise = 0 + left_upper_arm_reba_score = 0 + left_upper_arm_flex_score = 0 + left_upper_arm_side_score = 0 + left_upper_arm_shoulder_rise = 0 + + right_flexion = self.UA_degrees[0] + left_flexion = self.UA_degrees[1] + + right_side = self.UA_degrees[2] + left_side = self.UA_degrees[3] + + right_shoulder_rise = self.UA_degrees[4] + left_shoulder_rise = self.UA_degrees[5] + + if -20 <= right_flexion < 20: + right_upper_arm_reba_score += 1 + right_upper_arm_flex_score += 1 + if 20 <= right_flexion < 45: + right_upper_arm_reba_score += 2 + right_upper_arm_flex_score += 2 + if right_flexion < -20: + right_upper_arm_reba_score += 2 + right_upper_arm_flex_score += 2 + if 45 <= right_flexion < 90: + right_upper_arm_reba_score += 3 + right_upper_arm_flex_score += 3 + if 90 <= right_flexion: + right_upper_arm_reba_score += 4 + right_upper_arm_flex_score += 4 + + if -20 <= left_flexion < 20: + left_upper_arm_reba_score += 1 + left_upper_arm_flex_score += 1 + if 20 <= left_flexion < 45: + left_upper_arm_reba_score += 2 + left_upper_arm_flex_score += 2 + if left_flexion < -20: + left_upper_arm_reba_score += 2 + left_upper_arm_flex_score += 2 + if 45 <= left_flexion < 90: + left_upper_arm_reba_score += 3 + left_upper_arm_flex_score += 3 + if 90 <= left_flexion: + left_upper_arm_reba_score += 4 + left_upper_arm_flex_score += 4 + + # for side bending + if abs(right_side) > 2: + right_upper_arm_reba_score += 1 + right_upper_arm_side_score += 1 + + if abs(left_side) > 2: + left_upper_arm_reba_score += 1 + left_upper_arm_side_score += 1 + + # for shoulder rise + + if right_shoulder_rise > 90: + right_upper_arm_reba_score += 1 + right_upper_arm_shoulder_rise += 1 + + if left_shoulder_rise > 90: + left_upper_arm_reba_score += 1 + left_upper_arm_shoulder_rise += 1 + + return [right_upper_arm_reba_score, left_upper_arm_reba_score] \ No newline at end of file diff --git a/rofunc/utils/ergolab/reba_calculate/pose2degree/Util.py b/rofunc/utils/ergolab/reba_calculate/pose2degree/Util.py new file mode 100644 index 000000000..45ebeca2d --- /dev/null +++ b/rofunc/utils/ergolab/reba_calculate/pose2degree/Util.py @@ -0,0 +1,53 @@ +import math +import numpy as np + + + # multiplication of two quaternion(representing the orientation of joints) +def multiply_two_quaternion(q1, q2): + a = q1[0] + b = q1[1] + c = q1[2] + d = q1[3] + e = q2[0] + f = q2[1] + g = q2[2] + h = q2[3] + + m0 = round(a * e - b * f - c * g - d * h, 1) + m1 = round(b * e + a * f + c * h - d * g, 1) + m2 = round(a * g - b * h + c * e + d * f, 1) + m3 = round(a * h + b * g - c * f + d * e, 1) + return [m0, m1, m2, m3] + + +def get_angle_between_degs(v1, v2): + len_v1 = math.sqrt(v1[0] ** 2 + v1[1] ** 2 + v1[2] ** 2) + len_v2 = math.sqrt(v2[0] ** 2 + v2[1] ** 2 + v2[2] ** 2) + + result = math.acos(round(np.dot(v1, v2) / (len_v1 * len_v2), 3)) * 180 / math.pi + return result + + +def get_distance_between(p1, p2): + result = [x + y for x, y in zip(p2, np.dot(p1, -1))] + return math.sqrt(result[0] ** 2 + result[1] ** 2 + result[2] ** 2) + + +def normalization(vector): + l = math.sqrt(vector[0] ** 2 + vector[1] ** 2 + vector[2] ** 2) + if l == 0: + l += 0.01 + normal_vector = [vector[0] / l, vector[1] / l, vector[2] / l] + return normal_vector + + +def find_rotation_quaternion(outer_quaternion, inner_quaternion): + conjucate = [outer_quaternion[0], -outer_quaternion[1], -outer_quaternion[2], -outer_quaternion[3]] + length = math.sqrt(outer_quaternion[0] ** 2 + outer_quaternion[1] ** 2 + + outer_quaternion[2] ** 2 + outer_quaternion[3] ** 2) + if length ==0: + inverse =[0,0,0,-1] + else: + inverse = np.dot(conjucate, (1 / length)) + rotation = multiply_two_quaternion(inner_quaternion, inverse) + return rotation \ No newline at end of file diff --git a/rofunc/utils/ergolab/reba_calculate/pose2degree/__init__.py b/rofunc/utils/ergolab/reba_calculate/pose2degree/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/rofunc/utils/ergolab/reba_calculate/pose2degree/body_part_sorting.py b/rofunc/utils/ergolab/reba_calculate/pose2degree/body_part_sorting.py new file mode 100644 index 000000000..f928a3bce --- /dev/null +++ b/rofunc/utils/ergolab/reba_calculate/pose2degree/body_part_sorting.py @@ -0,0 +1,30 @@ +class body_part_number: + def neck(self): + return [2, 1] + + def trunk_upper_body(self): + return [0, 3, 2, 6] + + def right_arm(self): + return [3, 4, 5] + + def left_arm(self): + return [6, 7, 8] + + # def trunk_lower_body(self): + # return [0, 14, 19, 11] + + def trunk_whole_body(self): + return [9, 12, 2, 0] + + def right_leg(self): + return [9, 10, 11] + + def left_leg(self): + return [12, 13, 14] + + def right_shoulder(self): + return [2, 3] + + def left_shoulder(self): + return [2, 6] \ No newline at end of file diff --git a/rofunc/utils/ergolab/reba_calculate/pose2degree/lower_arm_degree.py b/rofunc/utils/ergolab/reba_calculate/pose2degree/lower_arm_degree.py new file mode 100644 index 000000000..4cc49178e --- /dev/null +++ b/rofunc/utils/ergolab/reba_calculate/pose2degree/lower_arm_degree.py @@ -0,0 +1,23 @@ +import rofunc as rf + + +class LADegrees: + def __init__(self, joints_position): + self.joints_position = joints_position + + def lower_arm_degree(self): + m_body_number = rf.ergolab.body_part_number() + right_arm_joint_numbers = m_body_number.right_arm() + left_arm_joint_numbers = m_body_number.left_arm() + + right_shoulder_elbow_vector = self.joints_position[right_arm_joint_numbers[1]] - self.joints_position[right_arm_joint_numbers[0]] + left_shoulder_elbow_vector = self.joints_position[left_arm_joint_numbers[1]] - self.joints_position[left_arm_joint_numbers[0]] + + right_elbow_wrist_vector = self.joints_position[right_arm_joint_numbers[2]] - self.joints_position[right_arm_joint_numbers[1]] + left_elbow_wrist_vector = self.joints_position[left_arm_joint_numbers[2]] - self.joints_position[left_arm_joint_numbers[1]] + + # right and left arm degree in saggital plane + right_degree = rf.ergolab.get_angle_between_degs(right_shoulder_elbow_vector, right_elbow_wrist_vector) + left_degree = rf.ergolab.get_angle_between_degs(left_shoulder_elbow_vector, left_elbow_wrist_vector) + + return [right_degree,left_degree] \ No newline at end of file diff --git a/rofunc/utils/ergolab/reba_calculate/pose2degree/trunk_degree.py b/rofunc/utils/ergolab/reba_calculate/pose2degree/trunk_degree.py new file mode 100644 index 000000000..a059a0718 --- /dev/null +++ b/rofunc/utils/ergolab/reba_calculate/pose2degree/trunk_degree.py @@ -0,0 +1,64 @@ +import numpy as np +import math +import transformations as tf +import rofunc as rf + + +class TrunkDegree: + def __init__(self, joints_position,joints_orientation): + self.joints_position = joints_position + self.joints_orientation = joints_orientation + + def trunk_plane(self): + m_body_number = rf.ergolab.body_part_number() + trunk_joint_numbers = m_body_number.trunk_upper_body() + + # finding a plane of upper body + u = self.joints_position[trunk_joint_numbers[1]] - self.joints_position[trunk_joint_numbers[0]] + v = self.joints_position[trunk_joint_numbers[3]] - self.joints_position[trunk_joint_numbers[0]] + + normal_plane = np.cross(u, v) + return normal_plane + + def trunk_flex_calculator(self): + normal_plane = self.trunk_plane() + z_vector = np.array([0, 0, 1]) + + trunk_flex = rf.ergolab.get_angle_between_degs(z_vector, normal_plane) - 90 + return trunk_flex + + def trunk_side_calculator(self): + m_body_number = rf.ergolab.body_part_number() + trunk_joint_numbers = m_body_number.trunk_upper_body() + + normal_plane_xz = np.array([1, 0, 0]) + z_vector = np.array([0, 0, 1]) + spine_vector = self.joints_position[trunk_joint_numbers[2]] - self.joints_position[trunk_joint_numbers[0]] + project_spine_on_xz_plane = spine_vector - np.dot(spine_vector, normal_plane_xz) * normal_plane_xz + + trunk_side_bending = rf.ergolab.get_angle_between_degs(z_vector, project_spine_on_xz_plane) + + return trunk_side_bending + + def trunk_twist_calculator(self): + # In here the rotor needed to transfer orientation frame of core joint to neck joint is calculated + # this considered as twist + m_body_number = rf.ergolab.body_part_number() + trunk_joint_numbers = m_body_number.trunk_upper_body() + q1 = np.eye(4) + q2 = np.eye(4) + q1[:3, :3] = self.joints_orientation[trunk_joint_numbers[2]]# neck + q2[:3, :3] = self.joints_orientation[trunk_joint_numbers[0]]# core + q1 = tf.quaternion_from_matrix(q1) + q2 = tf.quaternion_from_matrix(q2) + # finding the rotor that express rotation between two orientational frame(between outer and inner joint) + rotor = rf.ergolab.find_rotation_quaternion(q1, q2) + trunk_twist = math.acos(abs(rotor[0])) * 2 * (180 / np.pi) + return trunk_twist + + def trunk_degrees(self): + trunk_flex_degree = self.trunk_flex_calculator() + trunk_side_bending_degree = self.trunk_side_calculator() + trunk_torsion_degree = self.trunk_twist_calculator() + + return [trunk_flex_degree,trunk_side_bending_degree,trunk_torsion_degree] \ No newline at end of file diff --git a/rofunc/utils/ergolab/reba_calculate/pose2degree/upper_arm_degree.py b/rofunc/utils/ergolab/reba_calculate/pose2degree/upper_arm_degree.py new file mode 100644 index 000000000..654b6523d --- /dev/null +++ b/rofunc/utils/ergolab/reba_calculate/pose2degree/upper_arm_degree.py @@ -0,0 +1,116 @@ +import numpy as np +import rofunc as rf + + +class UpperArmDegree: + def __init__(self, joints_position): + self.joints_position = joints_position + + def trunk_plane(self): + m_body_number = rf.ergolab.body_part_number() + trunk_joint_numbers = m_body_number.trunk_upper_body() + + # finding a plane of upper body + u = self.joints_position[trunk_joint_numbers[1]] - self.joints_position[trunk_joint_numbers[0]] + v = self.joints_position[trunk_joint_numbers[3]] - self.joints_position[trunk_joint_numbers[0]] + + normal_plane = np.cross(u, v) + return normal_plane + + def upper_arm_flex(self): + m_body_number = rf.ergolab.body_part_number() + right_upper_arm_joint_numbers = m_body_number.right_arm() + left_upper_arm_joint_numbers = m_body_number.left_arm() + trunk_joint_numbers = m_body_number.trunk_whole_body() + + right_upper_arm_vector = self.joints_position[right_upper_arm_joint_numbers[1]] - self.joints_position[ + right_upper_arm_joint_numbers[0]] + + left_upper_arm_vector = self.joints_position[left_upper_arm_joint_numbers[1]] - self.joints_position[ + left_upper_arm_joint_numbers[0]] + + normal_trunk_plane = self.trunk_plane() + spine_vector = self.joints_position[trunk_joint_numbers[3]] - self.joints_position[trunk_joint_numbers[2]] + + sagittal_plane_normal = np.cross(normal_trunk_plane, spine_vector) + sagittal_plane_normal /= np.linalg.norm(sagittal_plane_normal) + + right_arm_projection = right_upper_arm_vector - np.dot(right_upper_arm_vector, + sagittal_plane_normal) * sagittal_plane_normal + left_arm_projection = left_upper_arm_vector - np.dot(left_upper_arm_vector, + sagittal_plane_normal) * sagittal_plane_normal + + # Calculate flexion angles using the projection and the normalized spine vector + flex_right_upper_arm = np.degrees(np.arccos(np.dot(right_arm_projection, spine_vector) / + (np.linalg.norm(right_arm_projection) * np.linalg.norm( + spine_vector)))) + flex_left_upper_arm = np.degrees(np.arccos(np.dot(left_arm_projection, spine_vector) / + (np.linalg.norm(left_arm_projection) * np.linalg.norm( + spine_vector)))) + + return [flex_right_upper_arm, flex_left_upper_arm] + + def upper_arm_abduct(self): + m_body_number = rf.ergolab.body_part_number() + right_upper_arm_joint_numbers = m_body_number.right_arm() + left_upper_arm_joint_numbers = m_body_number.left_arm() + trunk_joint_numbers = m_body_number.trunk_whole_body() + + right_upper_arm_vector = self.joints_position[right_upper_arm_joint_numbers[1]] - self.joints_position[ + right_upper_arm_joint_numbers[0]] + left_upper_arm_vector = self.joints_position[left_upper_arm_joint_numbers[1]] - self.joints_position[ + left_upper_arm_joint_numbers[0]] + + normal_trunk_plane = self.trunk_plane() + + proj_right_upperarm_on_plane = right_upper_arm_vector - np.dot(right_upper_arm_vector, + normal_trunk_plane) * normal_trunk_plane + + proj_left_upperarm_on_plane = left_upper_arm_vector - np.dot(left_upper_arm_vector, + normal_trunk_plane) * normal_trunk_plane + + spine_vector = self.joints_position[trunk_joint_numbers[3]] - self.joints_position[trunk_joint_numbers[2]] + + right_side_degree = rf.ergolab.get_angle_between_degs(spine_vector, proj_right_upperarm_on_plane) + + left_side_degree = rf.ergolab.get_angle_between_degs(spine_vector, proj_left_upperarm_on_plane) + + if np.dot(np.cross(spine_vector, right_upper_arm_vector), normal_trunk_plane) < 0: + # if the arm go to the body: adduction + right_side_degree *= -1 + + if np.dot(np.cross(spine_vector, left_upper_arm_vector), normal_trunk_plane) > 0: + left_side_degree *= -1 + + return [right_side_degree, left_side_degree] + + def shoulder_rise(self): + m_body_number = rf.ergolab.body_part_number() + trunk_joint_numbers = m_body_number.trunk_upper_body() + right_shoulder_joint_numbers = m_body_number.right_shoulder() + left_shoulder_joint_numbers = m_body_number.left_shoulder() + spine_vector = self.joints_position[trunk_joint_numbers[0]] - self.joints_position[trunk_joint_numbers[2]] + right_shoulder_vector = self.joints_position[right_shoulder_joint_numbers[1]] - self.joints_position[ + right_shoulder_joint_numbers[0]] + left_shoulder_vector = self.joints_position[left_shoulder_joint_numbers[1]] - self.joints_position[left_shoulder_joint_numbers[0]] + + right_shoulder_rise_degree = 90 - rf.ergolab.get_angle_between_degs(spine_vector, right_shoulder_vector) + left_shoulder_rise_degree = 90 - rf.ergolab.get_angle_between_degs(spine_vector, left_shoulder_vector) + + return [right_shoulder_rise_degree, left_shoulder_rise_degree] + + def upper_arm_degrees(self): + flexion = self.upper_arm_flex() + side = self.upper_arm_abduct() + shoulder_rise = self.shoulder_rise() + + right_flexion = flexion[0] + left_flexion = flexion[1] + + right_side = side[0] + left_side = side[1] + + right_shoulder_rise = shoulder_rise[0] + left_shoulder_rise = shoulder_rise[1] + + return [right_flexion,left_flexion,right_side,left_side,right_shoulder_rise,left_shoulder_rise] \ No newline at end of file diff --git a/rofunc/utils/maniplab/__init__.py b/rofunc/utils/maniplab/__init__.py new file mode 100644 index 000000000..83300c42b --- /dev/null +++ b/rofunc/utils/maniplab/__init__.py @@ -0,0 +1,2 @@ +from .manipulability_calculator import * +from .read_skeleton_motion import * \ No newline at end of file diff --git a/rofunc/utils/maniplab/manipulability_calculator.py b/rofunc/utils/maniplab/manipulability_calculator.py new file mode 100644 index 000000000..9985d18f9 --- /dev/null +++ b/rofunc/utils/maniplab/manipulability_calculator.py @@ -0,0 +1,53 @@ +import numpy as np +import torch +from scipy.spatial.transform import Rotation as R + +def quaternion_to_rotation_matrix(quaternion): + return R.from_quat(quaternion).as_matrix() + +def forward_kinematics(joint_positions, joint_rotations, parent_indices): + num_joints = len(joint_positions) + global_positions = np.zeros_like(joint_positions) + global_rotations = [np.eye(3) for _ in range(num_joints)] + for i in range(num_joints): + if parent_indices[i] == -1: + global_positions[i] = joint_positions[i] + global_rotations[i] = quaternion_to_rotation_matrix(joint_rotations[i]) + else: + parent_index = parent_indices[i] + parent_rotation = global_rotations[parent_index] + parent_position = global_positions[parent_index] + rotation_matrix = quaternion_to_rotation_matrix(joint_rotations[i]) + global_rotations[i] = parent_rotation @ rotation_matrix + global_positions[i] = parent_position + parent_rotation @ joint_positions[i] + + # Adjust all joints so the feet are on the ground + left_foot_index = 14 # Replace with the actual index of LeftFoot + right_foot_index = 11 # Replace with the actual index of RightFoot + min_z = min(global_positions[left_foot_index, 2], global_positions[right_foot_index, 2]) + global_positions[:, 2] -= min_z + + return global_positions, global_rotations + +# def forward_kinematics(joint_positions, joint_rotations, parent_indices): +# num_joints = len(joint_positions) +# global_positions = np.zeros_like(joint_positions) +# global_rotations = [np.eye(3) for _ in range(num_joints)] +# for i in range(num_joints): +# if parent_indices[i] == -1: +# global_positions[i] = joint_positions[i] +# global_rotations[i] = quaternion_to_rotation_matrix(joint_rotations[i]) +# else: +# parent_index = parent_indices[i] +# parent_rotation = global_rotations[parent_index] +# parent_position = global_positions[parent_index] +# rotation_matrix = quaternion_to_rotation_matrix(joint_rotations[i]) +# global_rotations[i] = parent_rotation @ rotation_matrix +# global_positions[i] = parent_position + parent_rotation @ joint_positions[i] +# return global_positions, global_rotations + +def calculate_manipulability(jacobian): + M_v = jacobian @ jacobian.T + M_F = np.linalg.inv(M_v[3:, 3:]) + eigenvalues, eigenvectors = np.linalg.eigh(M_F) + return eigenvalues, eigenvectors \ No newline at end of file diff --git a/rofunc/utils/maniplab/read_skeleton_motion.py b/rofunc/utils/maniplab/read_skeleton_motion.py new file mode 100644 index 000000000..658548877 --- /dev/null +++ b/rofunc/utils/maniplab/read_skeleton_motion.py @@ -0,0 +1,5 @@ +import numpy as np + +def read_skeleton_motion(file_path): + skeleton = np.load(file_path, allow_pickle=True).item() + return skeleton['skeleton_tree']['node_names'], skeleton['rotation']['arr'], skeleton['skeleton_tree']['parent_indices']['arr'], skeleton['skeleton_tree']['local_translation']['arr'] \ No newline at end of file diff --git a/rofunc/utils/robolab/kinematics/__init__.py b/rofunc/utils/robolab/kinematics/__init__.py index 98b90b69a..4daaac452 100644 --- a/rofunc/utils/robolab/kinematics/__init__.py +++ b/rofunc/utils/robolab/kinematics/__init__.py @@ -1,2 +1,2 @@ from .fk import get_fk_from_chain, get_fk_from_model -from .robot_class import RobotModel +from .robot_class import RobotModel \ No newline at end of file diff --git a/rofunc/utils/visualab/__init__.py b/rofunc/utils/visualab/__init__.py index f6b845963..c09063465 100644 --- a/rofunc/utils/visualab/__init__.py +++ b/rofunc/utils/visualab/__init__.py @@ -7,6 +7,7 @@ from .utils import * from .image import * from .interact import * +from .ergo_manip_plot import * # Segment package from .segment import * diff --git a/rofunc/utils/visualab/ergo_manip_plot.py b/rofunc/utils/visualab/ergo_manip_plot.py new file mode 100644 index 000000000..7c7ed0c0b --- /dev/null +++ b/rofunc/utils/visualab/ergo_manip_plot.py @@ -0,0 +1,52 @@ +import numpy as np + +def plot_skeleton(ax, global_positions, parent_indices): + for i in range(len(global_positions)): + if parent_indices[i] != -1: + parent_pos = global_positions[parent_indices[i]] + joint_pos = global_positions[i] + ax.plot([parent_pos[0], joint_pos[0]], [parent_pos[1], joint_pos[1]], [parent_pos[2], joint_pos[2]], 'k-') + +def plot_ellipsoid(ax, eigenvalues, eigenvectors, center, color): + eigenvalues = eigenvalues[-3:] + eigenvectors = eigenvectors[-3:, -3:] + u = np.linspace(0, 2 * np.pi, 100) + v = np.linspace(0, np.pi, 100) + x = np.outer(np.cos(u), np.sin(v)) / 5 + y = np.outer(np.sin(u), np.sin(v)) / 5 + z = np.outer(np.ones_like(u), np.cos(v)) / 5 + for i in range(len(x)): + for j in range(len(x)): + [x[i, j], y[i, j], z[i, j]] = np.dot(eigenvectors, np.array([x[i, j], y[i, j], z[i, j]]) * np.sqrt(eigenvalues)) + x[i, j] += center[0] + y[i, j] += center[1] + z[i, j] += center[2] + ax.plot_surface(x, y, z, color=color, alpha=0.3) + +def ua_get_color(score): + if 1 <= score <= 2: + return 'green' + elif 3 <= score <= 4: + return 'orange' + elif 5 <= score <= 6: + return 'red' + else: + return 'purple' # Other scores in purple + +def la_get_color(score): + if 1 <= score < 2: + return 'green' + elif 2 <= score: + return 'orange' + else: + return 'purple' # Other scores in purple + +def trunk_get_color(score): + if 1 <= score <= 2: + return 'green' + elif 3 <= score <= 4: + return 'orange' + elif 5 <= score <= 6: + return 'red' + else: + return 'purple' # Other scores in purple \ No newline at end of file From a4ce58c95ad34160ab5932a17328380b4ca395c7 Mon Sep 17 00:00:00 2001 From: Junjia Liu <24936522+Skylark0924@users.noreply.github.com> Date: Fri, 8 Nov 2024 13:50:37 +0800 Subject: [PATCH 2/2] Update example_ergo_manip.py --- examples/visualab/example_ergo_manip.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/examples/visualab/example_ergo_manip.py b/examples/visualab/example_ergo_manip.py index de175a9a3..3c2bea628 100644 --- a/examples/visualab/example_ergo_manip.py +++ b/examples/visualab/example_ergo_manip.py @@ -1,3 +1,10 @@ +""" +Ergo manipulation +=========================== + +Examples +""" + import numpy as np import rofunc as rf import matplotlib.pyplot as plt @@ -71,4 +78,4 @@ def update(frame): ax = fig.add_subplot(111, projection='3d') ani = FuncAnimation(fig, update, frames=len(skeleton_joint), repeat=True) # ani.save('/home/ubuntu/Ergo-Manip/data/gif/demo_2_andrew.gif', writer='imagemagick', fps=3) - plt.show() \ No newline at end of file + plt.show()