tf_nao.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import tensorflow as tf\n",
    "import tensorflow_hub as hub\n",
    "from tensorflow_docs.vis import embed\n",
    "import numpy as np\n",
    "import cv2\n",
    "\n",
    "# Import matplotlib libraries\n",
    "from matplotlib import pyplot as plt\n",
    "from matplotlib.collections import LineCollection\n",
    "import matplotlib.patches as patches\n",
    "\n",
    "# Some modules to display an animation using imageio.\n",
    "import imageio\n",
    "from IPython.display import HTML, display\n",
    "\n",
    "import cv2 \n",
    "import json\n",
    "import time\n",
    "import math\n",
    "import sympy as sp"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Dictionary that maps from joint names to keypoint indices.\n",
    "KEYPOINT_DICT = {\n",
    "    'nose': 0,\n",
    "    'left_eye': 1,\n",
    "    'right_eye': 2,\n",
    "    'left_ear': 3,\n",
    "    'right_ear': 4,\n",
    "    'left_shoulder': 5,\n",
    "    'right_shoulder': 6,\n",
    "    'left_elbow': 7,\n",
    "    'right_elbow': 8,\n",
    "    'left_wrist': 9,\n",
    "    'right_wrist': 10,\n",
    "    'left_hip': 11,\n",
    "    'right_hip': 12,\n",
    "    'left_knee': 13,\n",
    "    'right_knee': 14,\n",
    "    'left_ankle': 15,\n",
    "    'right_ankle': 16\n",
    "}\n",
    "\n",
    "# Maps bones to a matplotlib color name.\n",
    "KEYPOINT_EDGE_INDS_TO_COLOR = {\n",
    "    (0, 1): 'm',\n",
    "    (0, 2): 'c',\n",
    "    (1, 3): 'm',\n",
    "    (2, 4): 'c',\n",
    "    (0, 5): 'm',\n",
    "    (0, 6): 'c',\n",
    "    (5, 7): 'm',\n",
    "    (7, 9): 'm',\n",
    "    (6, 8): 'c',\n",
    "    (8, 10): 'c',\n",
    "    (5, 6): 'y',\n",
    "    (5, 11): 'm',\n",
    "    (6, 12): 'c',\n",
    "    (11, 12): 'y',\n",
    "    (11, 13): 'm',\n",
    "    (13, 15): 'm',\n",
    "    (12, 14): 'c',\n",
    "    (14, 16): 'c'\n",
    "}\n",
    "\n",
    "def _keypoints_and_edges_for_display(keypoints_with_scores,\n",
    "                                     height,\n",
    "                                     width,\n",
    "                                     keypoint_threshold=0.11):\n",
    "  \"\"\"Returns high confidence keypoints and edges for visualization.\n",
    "\n",
    "  Args:\n",
    "    keypoints_with_scores: A numpy array with shape [1, 1, 17, 3] representing\n",
    "      the keypoint coordinates and scores returned from the MoveNet model.\n",
    "    height: height of the image in pixels.\n",
    "    width: width of the image in pixels.\n",
    "    keypoint_threshold: minimum confidence score for a keypoint to be\n",
    "      visualized.\n",
    "\n",
    "  Returns:\n",
    "    A (keypoints_xy, edges_xy, edge_colors) containing:\n",
    "      * the coordinates of all keypoints of all detected entities;\n",
    "      * the coordinates of all skeleton edges of all detected entities;\n",
    "      * the colors in which the edges should be plotted.\n",
    "  \"\"\"\n",
    "  keypoints_all = []\n",
    "  keypoint_edges_all = []\n",
    "  edge_colors = []\n",
    "  num_instances, _, _, _ = keypoints_with_scores.shape\n",
    "  for idx in range(num_instances):\n",
    "    kpts_x = keypoints_with_scores[0, idx, :, 1]\n",
    "    kpts_y = keypoints_with_scores[0, idx, :, 0]\n",
    "    kpts_scores = keypoints_with_scores[0, idx, :, 2]\n",
    "    kpts_absolute_xy = np.stack(\n",
    "        [width * np.array(kpts_x), height * np.array(kpts_y)], axis=-1)\n",
    "    kpts_above_thresh_absolute = kpts_absolute_xy[\n",
    "        kpts_scores > keypoint_threshold, :]\n",
    "    keypoints_all.append(kpts_above_thresh_absolute)\n",
    "\n",
    "    for edge_pair, color in KEYPOINT_EDGE_INDS_TO_COLOR.items():\n",
    "      if (kpts_scores[edge_pair[0]] > keypoint_threshold and\n",
    "          kpts_scores[edge_pair[1]] > keypoint_threshold):\n",
    "        x_start = kpts_absolute_xy[edge_pair[0], 0]\n",
    "        y_start = kpts_absolute_xy[edge_pair[0], 1]\n",
    "        x_end = kpts_absolute_xy[edge_pair[1], 0]\n",
    "        y_end = kpts_absolute_xy[edge_pair[1], 1]\n",
    "        line_seg = np.array([[x_start, y_start], [x_end, y_end]])\n",
    "        keypoint_edges_all.append(line_seg)\n",
    "        edge_colors.append(color)\n",
    "  if keypoints_all:\n",
    "    keypoints_xy = np.concatenate(keypoints_all, axis=0)\n",
    "  else:\n",
    "    keypoints_xy = np.zeros((0, 17, 2))\n",
    "\n",
    "  if keypoint_edges_all:\n",
    "    edges_xy = np.stack(keypoint_edges_all, axis=0)\n",
    "  else:\n",
    "    edges_xy = np.zeros((0, 2, 2))\n",
    "  return keypoints_xy, edges_xy, edge_colors\n",
    "\n",
    "\n",
    "def draw_prediction_on_image(\n",
    "    image, keypoints_with_scores, crop_region=None, close_figure=False,\n",
    "    output_image_height=None):\n",
    "  \"\"\"Draws the keypoint predictions on image.\n",
    "\n",
    "  Args:\n",
    "    image: A numpy array with shape [height, width, channel] representing the\n",
    "      pixel values of the input image.\n",
    "    keypoints_with_scores: A numpy array with shape [1, 1, 17, 3] representing\n",
    "      the keypoint coordinates and scores returned from the MoveNet model.\n",
    "    crop_region: A dictionary that defines the coordinates of the bounding box\n",
    "      of the crop region in normalized coordinates (see the init_crop_region\n",
    "      function below for more detail). If provided, this function will also\n",
    "      draw the bounding box on the image.\n",
    "    output_image_height: An integer indicating the height of the output image.\n",
    "      Note that the image aspect ratio will be the same as the input image.\n",
    "\n",
    "  Returns:\n",
    "    A numpy array with shape [out_height, out_width, channel] representing the\n",
    "    image overlaid with keypoint predictions.\n",
    "  \"\"\"\n",
    "  height, width, channel = image.shape\n",
    "  aspect_ratio = float(width) / height\n",
    "  fig, ax = plt.subplots(figsize=(12 * aspect_ratio, 12))\n",
    "  # To remove the huge white borders\n",
    "  fig.tight_layout(pad=0)\n",
    "  ax.margins(0)\n",
    "  ax.set_yticklabels([])\n",
    "  ax.set_xticklabels([])\n",
    "  plt.axis('off')\n",
    "\n",
    "  im = ax.imshow(image)\n",
    "  line_segments = LineCollection([], linewidths=(4), linestyle='solid')\n",
    "  ax.add_collection(line_segments)\n",
    "  # Turn off tick labels\n",
    "  scat = ax.scatter([], [], s=60, color='#FF1493', zorder=3)\n",
    "\n",
    "  (keypoint_locs, keypoint_edges,\n",
    "   edge_colors) = _keypoints_and_edges_for_display(\n",
    "       keypoints_with_scores, height, width)\n",
    "\n",
    "  line_segments.set_segments(keypoint_edges)\n",
    "  line_segments.set_color(edge_colors)\n",
    "  if keypoint_edges.shape[0]:\n",
    "    line_segments.set_segments(keypoint_edges)\n",
    "    line_segments.set_color(edge_colors)\n",
    "  if keypoint_locs.shape[0]:\n",
    "    scat.set_offsets(keypoint_locs)\n",
    "\n",
    "  if crop_region is not None:\n",
    "    xmin = max(crop_region['x_min'] * width, 0.0)\n",
    "    ymin = max(crop_region['y_min'] * height, 0.0)\n",
    "    rec_width = min(crop_region['x_max'], 0.99) * width - xmin\n",
    "    rec_height = min(crop_region['y_max'], 0.99) * height - ymin\n",
    "    rect = patches.Rectangle(\n",
    "        (xmin,ymin),rec_width,rec_height,\n",
    "        linewidth=1,edgecolor='b',facecolor='none')\n",
    "    ax.add_patch(rect)\n",
    "\n",
    "  fig.canvas.draw()\n",
    "  image_from_plot = np.frombuffer(fig.canvas.tostring_rgb(), dtype=np.uint8)\n",
    "  image_from_plot = image_from_plot.reshape(\n",
    "      fig.canvas.get_width_height()[::-1] + (3,))\n",
    "  plt.close(fig)\n",
    "  if output_image_height is not None:\n",
    "    output_image_width = int(output_image_height / height * width)\n",
    "    image_from_plot = cv2.resize(\n",
    "        image_from_plot, dsize=(output_image_width, output_image_height),\n",
    "         interpolation=cv2.INTER_CUBIC)\n",
    "  return image_from_plot\n",
    "\n",
    "def to_gif(images, duration):\n",
    "  \"\"\"Converts image sequence (4D numpy array) to gif.\"\"\"\n",
    "  imageio.mimsave('./animation.gif', images, duration=duration)\n",
    "  return embed.embed_file('./animation.gif')\n",
    "\n",
    "def progress(value, max=100):\n",
    "  return HTML(\"\"\"\n",
    "      <progress\n",
    "          value='{value}'\n",
    "          max='{max}',\n",
    "          style='width: 100%'\n",
    "      >\n",
    "          {value}\n",
    "      </progress>\n",
    "  \"\"\".format(value=value, max=max))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "model_name = \"movenet_lightning\"\n",
    "\n",
    "if \"movenet_lightning\" in model_name:\n",
    "  module = hub.load(\"https://tfhub.dev/google/movenet/singlepose/lightning/2\")\n",
    "  input_size = 192\n",
    "elif \"movenet_thunder\" in model_name:\n",
    "  module = hub.load(\"https://tfhub.dev/google/movenet/singlepose/thunder/2\")\n",
    "  input_size = 256\n",
    "else:\n",
    "  raise ValueError(\"Unsupported model name: %s\" % model_name)\n",
    "\n",
    "def movenet(input_image):\n",
    "  \"\"\"Runs detection on an input image.\n",
    "\n",
    "  Args:\n",
    "    input_image: A [1, height, width, 3] tensor represents the input image\n",
    "      pixels. Note that the height/width should already be resized and match the\n",
    "      expected input resolution of the model before passing into this function.\n",
    "\n",
    "  Returns:\n",
    "    A [1, 1, 17, 3] float numpy array representing the predicted keypoint\n",
    "    coordinates and scores.\n",
    "  \"\"\"\n",
    "  model = module.signatures['serving_default']\n",
    "\n",
    "  # SavedModel format expects tensor type of int32.\n",
    "  input_image = tf.cast(input_image, dtype=tf.int32)\n",
    "  # Run model inference.\n",
    "  outputs = model(input_image)\n",
    "  # Output is a [1, 1, 17, 3] tensor.\n",
    "  keypoints_with_scores = outputs['output_0'].numpy()\n",
    "  return keypoints_with_scores"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "#angle calculations\n",
    "#find angle\n",
    "# Python3 program for the above approach\n",
    "from math import atan\n",
    "# from angle_control_func import move_joint\n",
    "# Function to find the\n",
    "# angle between two lines\n",
    "def findAngle(point1, point2, point3):\n",
    "    M1 = (point1[1]-point2[1])/(point1[0]-point2[0])\n",
    "    M2 = (point3[1]-point2[1])/(point3[0]-point2[0])\n",
    "    PI = 3.14159265\n",
    "\t\n",
    "\t# Store the tan value of the angle\n",
    "    angle = abs((M2 - M1) / (1 + M1 * M2))\n",
    "\n",
    "\t# Calculate tan inverse of the angle\n",
    "    ret = atan(angle)\n",
    "\n",
    "\t# Print the result\n",
    "    return (round(ret, 2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "a = -1\n",
    "b = -1\n",
    "\n",
    "def findElbow3D(shoulder, elbow, wrist):\n",
    "    wtoe = sp.sqrt(sp.Pow(wrist[0] - elbow[0], 2) + sp.Pow(wrist[1] - elbow[1], 2))\n",
    "    etos = sp.sqrt(sp.Pow(shoulder[0] - elbow[0], 2) + sp.Pow(shoulder[1] - elbow[1], 2))\n",
    "    a = max(a , etos)\n",
    "    b = max(b , wtoe)\n",
    "\n",
    "    shoulder[2] = 0\n",
    "    elbow[2] = sp.sqrt(sp.Pow(a, 2) - sp.Pow(shoulder[0] - elbow[0], 2) - sp.Pow(shoulder[1] - elbow[1], 2))\n",
    "    wrist[2] = elbow[2] + sp.sqrt(sp.Pow(b, 2) - sp.Pow(wrist[0] - elbow[0], 2) - sp.Pow(wrist[1] - elbow[1], 2))\n",
    "    \n",
    "    d = sp.sqrt(sp.Pow(wrist[0] - shoulder[0], 2) + sp.Pow(shoulder[1] - wrist[1], 2) + sp.Pow(shoulder[2] - wrist[2], 2))\n",
    "    x, y = sp.symbols('x y')\n",
    "    equation1 = sp.Eq(a * x + b * y, d)\n",
    "    equation2 = sp.Eq(sp.Pow(a, 2) * (x**2) - sp.Pow(b, 2) * (y**2), sp.Pow(a, 2) - sp.Pow(b, 2))\n",
    "    \n",
    "    solution = sp.solve((equation1, equation2), (x, y))\n",
    "    alpha = math.asin(solution[0][0])\n",
    "    beta = math.asin(solution[0][1])\n",
    "\n",
    "    return round(alpha,2) , round(beta,2)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "tags": [
     "need to modify"
    ]
   },
   "outputs": [],
   "source": [
    "# def calculate_head_yaw(left_eye, right_eye, left_ear, right_ear):\n",
    "#     # Calculate midpoints\n",
    "#     midpoint_eyes = ((left_eye[0] + right_eye[0]) / 2, (left_eye[1] + right_eye[1]) / 2)\n",
    "#     midpoint_ears = ((left_ear[0] + right_ear[0]) / 2, (left_ear[1] + right_ear[1]) / 2)\n",
    "\n",
    "#     # Calculate angle to horizontal\n",
    "#     delta_x = midpoint_ears[0] - midpoint_eyes[0]\n",
    "#     delta_y = midpoint_ears[1] - midpoint_eyes[1]\n",
    "\n",
    "#     # Calculate angle in radians\n",
    "#     angle = math.atan2(delta_y, delta_x)\n",
    "\n",
    "#     return angle\n",
    "\n",
    "\n",
    "def calculate_head_yaw(left_eye, right_eye, left_ear, right_ear, left_shoulder, right_shoulder):\n",
    "    # Calculate midpoints\n",
    "    midpoint_eyes = ((left_eye[0] + right_eye[0]) / 2, (left_eye[1] + right_eye[1]) / 2)\n",
    "    midpoint_ears = ((left_ear[0] + right_ear[0]) / 2, (left_ear[1] + right_ear[1]) / 2)\n",
    "    midpoint_shoulders = ((left_shoulder[0] + right_shoulder[0]) / 2, (left_shoulder[1] + right_shoulder[1]) / 2)\n",
    "\n",
    "    # Adjust the midpoints based on shoulder orientation\n",
    "    adjusted_midpoint_eyes = (midpoint_eyes[0] - midpoint_shoulders[0], midpoint_eyes[1] - midpoint_shoulders[1])\n",
    "    adjusted_midpoint_ears = (midpoint_ears[0] - midpoint_shoulders[0], midpoint_ears[1] - midpoint_shoulders[1])\n",
    "\n",
    "    # Calculate angle to horizontal\n",
    "    delta_x = adjusted_midpoint_ears[0] - adjusted_midpoint_eyes[0]\n",
    "    delta_y = adjusted_midpoint_ears[1] - adjusted_midpoint_eyes[1]\n",
    "\n",
    "    # Calculate angle in radians\n",
    "    angle = math.atan2(delta_y, delta_x)\n",
    "\n",
    "    return angle\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "def findKnee(Hip, foot):\n",
    "    l = math.sqrt((Hip[0]-foot[0])**2 + (Hip[1]-foot[1])**2)\n",
    "    b = 0.10882269\n",
    "    c = 0.10681073\n",
    "\n",
    "    cos_ang = ((c**2)+(b**2)-(l**2))/(2*b*c)\n",
    "    print(cos_ang)\n",
    "    knee_angle = math.acos(abs(cos_ang))\n",
    "    return knee_angle"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "# def calculate_angle(p1, p2, p3):\n",
    "#     \"\"\"\n",
    "#     Calculate the angle formed by three points: p1, p2, and p3.\n",
    "#     p2 is the vertex point where the angle is formed.\n",
    "#     \"\"\"\n",
    "#     # Create vectors\n",
    "#     vector1 = (p1[0] - p2[0], p1[1] - p2[1])\n",
    "#     vector2 = (p3[0] - p2[0], p3[1] - p2[1])\n",
    "\n",
    "#     # Calculate dot product\n",
    "#     dot_product = vector1[0]*vector2[0] + vector1[1]*vector2[1]\n",
    "\n",
    "#     # Calculate magnitudes\n",
    "#     magnitude1 = math.sqrt(vector1[0]**2 + vector1[1]**2)\n",
    "#     magnitude2 = math.sqrt(vector2[0]**2 + vector2[1]**2)\n",
    "\n",
    "#     # Calculate the angle in radians\n",
    "#     angle = math.acos(dot_product / (magnitude1 * magnitude2))\n",
    "\n",
    "#     return angle\n",
    "\n",
    "# def knee_pitch(hip,knee,ankle):\n",
    "#     \"\"\"\n",
    "#     Calculate the knee pitch angles for both legs.\n",
    "#     Coordinates are given as (x, y) tuples.\n",
    "#     \"\"\"\n",
    "#     knee_pitch = calculate_angle(hip, knee, ankle)\n",
    "\n",
    "#     return knee_pitch\n",
    "\n",
    "# def knee_pitch(left_hip, left_knee, left_ankle):\n",
    "#     # Helper function to calculate the Euclidean distance between two points\n",
    "#     def distance(p1, p2):\n",
    "#         return math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)\n",
    "    \n",
    "#     # Calculate the lengths of the sides of the triangle\n",
    "#     a = distance(left_hip, left_knee)  # hip to knee\n",
    "#     b = distance(left_knee, left_ankle)  # knee to ankle\n",
    "#     c = distance(left_hip, left_ankle)  # hip to ankle\n",
    "\n",
    "#     # Calculate the knee pitch using the law of cosines\n",
    "#     # Ensure the denominator is not zero to avoid division by zero\n",
    "#     if a * b != 0:\n",
    "#         cosine_angle = (a**2 + b**2 - c**2) / (2 * a * b)\n",
    "#         # Ensure the cosine value is in the valid range [-1, 1] to avoid math domain error\n",
    "#         cosine_angle = max(min(cosine_angle, 1), -1)\n",
    "#         knee_pitch = math.acos(cosine_angle)\n",
    "#     else:\n",
    "#         knee_pitch = 0  # In case the points are overlapping\n",
    "\n",
    "#     return knee_pitch\n",
    "\n",
    "# def calculate_angles(left_hip, right_hip, left_knee, right_knee, left_ankle, right_ankle):\n",
    "#     # Helper function to calculate angle between three points\n",
    "#     def angle_between_points(p1, p2, p3):\n",
    "#         a = math.sqrt((p2[0] - p3[0])**2 + (p2[1] - p3[1])**2)\n",
    "#         b = math.sqrt((p1[0] - p3[0])**2 + (p1[1] - p3[1])**2)\n",
    "#         c = math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)\n",
    "#         return math.acos((a**2 + c**2 - b**2) / (2 * a * c))\n",
    "\n",
    "#     # Average the coordinates for left and right joints to get a single value for each joint\n",
    "#     hip = [(left_hip[0] + right_hip[0]) / 2, (left_hip[1] + right_hip[1]) / 2]\n",
    "#     knee = [(left_knee[0] + right_knee[0]) / 2, (left_knee[1] + right_knee[1]) / 2]\n",
    "#     ankle = [(left_ankle[0] + right_ankle[0]) / 2, (left_ankle[1] + right_ankle[1]) / 2]\n",
    "\n",
    "#     # Calculate the angles\n",
    "#     knee_pitch = angle_between_points(hip, knee, ankle)\n",
    "#     hip_pitch = angle_between_points([hip[0], hip[1] - 1], hip, knee)\n",
    "#     ankle_pitch = angle_between_points(knee, ankle, [ankle[0] + 1, ankle[1]])\n",
    "\n",
    "#     return knee_pitch, hip_pitch, ankle_pitch\n",
    "\n",
    "def calculate_angle(p1, p2, p3):\n",
    "    \"\"\"\n",
    "    Helper function to calculate the angle formed by three points.\n",
    "    :param p1, p2, p3: tuples representing the points (x, y).\n",
    "    :return: angle in radians.\n",
    "    \"\"\"\n",
    "    a = (p2[0] - p1[0], p2[1] - p1[1])  # vector from p1 to p2\n",
    "    b = (p3[0] - p2[0], p3[1] - p2[1])  # vector from p2 to p3\n",
    "\n",
    "    dot_product = a[0] * b[0] + a[1] * b[1]\n",
    "    magnitude_a = math.sqrt(a[0]**2 + a[1]**2)\n",
    "    magnitude_b = math.sqrt(b[0]**2 + b[1]**2)\n",
    "\n",
    "    # Angle in radians\n",
    "    angle = math.acos(dot_product / (magnitude_a * magnitude_b))\n",
    "\n",
    "    # Adjusting angle based on your requirements\n",
    "    if a[1] < 0:  # Leg/hip goes backward\n",
    "        angle = -angle\n",
    "\n",
    "    return angle\n",
    "\n",
    "def knee_hip_pitch(left_shoulder, right_shoulder, left_hip, right_hip, left_knee, right_knee, left_ankle, right_ankle):\n",
    "    \"\"\"\n",
    "    Calculate the knee pitch and hip pitch angles.\n",
    "    :param left_shoulder, right_shoulder, left_hip, right_hip, left_knee, right_knee, left_ankle, right_ankle: coordinates (x, y) of each joint.\n",
    "    :return: knee_pitch, hip_pitch in radians.\n",
    "    \"\"\"\n",
    "\n",
    "    # Average hip and knee coordinates for symmetry\n",
    "    hip = ((left_hip[0] + right_hip[0]) / 2, (left_hip[1] + right_hip[1]) / 2)\n",
    "    knee = ((left_knee[0] + right_knee[0]) / 2, (left_knee[1] + right_knee[1]) / 2)\n",
    "    ankle = ((left_ankle[0] + right_ankle[0]) / 2, (left_ankle[1] + right_ankle[1]) / 2)\n",
    "\n",
    "    # Assuming shoulders are aligned horizontally\n",
    "    shoulder = ((left_shoulder[0] + right_shoulder[0]) / 2, (left_shoulder[1] + right_shoulder[1]) / 2)\n",
    "\n",
    "    # Calculate knee pitch\n",
    "    knee_pitch = calculate_angle(hip, knee, ankle)\n",
    "\n",
    "    # Calculate hip pitch\n",
    "    hip_pitch = calculate_angle(shoulder, hip, knee)\n",
    "\n",
    "    return knee_pitch, hip_pitch"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "def shoulder_yaw_angle(shoulder, elbow, wrist):\n",
    "    # Consider only the x-coordinates for a front view\n",
    "    shoulder_x, _,a = shoulder\n",
    "    elbow_x, _,b = elbow\n",
    "    wrist_x, _,c = wrist\n",
    "\n",
    "    # Calculate the vectors representing the arm segments\n",
    "    shoulder_to_elbow_vector = elbow_x - shoulder_x\n",
    "    elbow_to_wrist_vector = wrist_x - elbow_x\n",
    "\n",
    "    # Calculate the magnitudes of the vectors\n",
    "    magnitude_shoulder_to_elbow = abs(shoulder_to_elbow_vector)\n",
    "    magnitude_elbow_to_wrist = abs(elbow_to_wrist_vector)\n",
    "\n",
    "    # Calculate the cosine of the angle between the vectors\n",
    "    cos_angle = 0.0  # Default to 0 to handle division by zero\n",
    "\n",
    "    if magnitude_shoulder_to_elbow != 0 and magnitude_elbow_to_wrist != 0:\n",
    "        cos_angle = shoulder_to_elbow_vector / (magnitude_shoulder_to_elbow * magnitude_elbow_to_wrist)\n",
    "\n",
    "    # Calculate the shoulder yaw angle in radians\n",
    "    shoulder_yaw_angle_rad = math.acos(abs(cos_angle))\n",
    "\n",
    "    return shoulder_yaw_angle_rad"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "def arm_pitch(wrist, elbow, shoulder, hip):\n",
    "    pitch_angle = math.atan2(wrist[1] - shoulder[1], wrist[0] - shoulder[0]) - math.atan2(hip[1] - shoulder[1], hip[0] - shoulder[0])\n",
    "    return pitch_angle"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "ename": "",
     "evalue": "",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31mThe Kernel crashed while executing code in the the current cell or a previous cell. Please review the code in the cell(s) to identify a possible cause of the failure. Click <a href='https://aka.ms/vscodeJupyterKernelCrash'>here</a> for more info. View Jupyter <a href='command:jupyter.viewOutput'>log</a> for further details."
     ]
    }
   ],
   "source": [
    "vid = cv2.VideoCapture(0) \n",
    "  \n",
    "while(True):\n",
    "    ret, frame = vid.read() \n",
    "\n",
    "    image_data = frame\n",
    "    \n",
    "    image = tf.convert_to_tensor(image_data, dtype=tf.uint8)\n",
    "    input_image = tf.expand_dims(image, axis=0)\n",
    "    input_image = tf.image.resize_with_pad(input_image, input_size, input_size)\n",
    "\n",
    "    keypoints_with_scores = movenet(input_image)\n",
    "\n",
    "    Lshoulder = keypoints_with_scores[0][0][5]\n",
    "    Lelbow = keypoints_with_scores[0][0][7]\n",
    "    Lwrist = keypoints_with_scores[0][0][9]\n",
    "    Lhip = keypoints_with_scores[0][0][11]\n",
    "    LAnkle = keypoints_with_scores[0][0][15]\n",
    "    LeftEar = keypoints_with_scores[0][0][3]\n",
    "    LKnee = keypoints_with_scores[0][0][13]\n",
    "    LeftEye = keypoints_with_scores[0][0][1]\n",
    "\n",
    "    RightEar = keypoints_with_scores[0][0][4]\n",
    "    Rshoulder = keypoints_with_scores[0][0][6]\n",
    "    Relbow = keypoints_with_scores[0][0][10]\n",
    "    Rwrist = keypoints_with_scores[0][0][9]\n",
    "    Rhip = keypoints_with_scores[0][0][12]\n",
    "    RAnkle = keypoints_with_scores[0][0][16]\n",
    "    RKnee = keypoints_with_scores[0][0][14]\n",
    "    RightEye = keypoints_with_scores[0][0][2]\n",
    "\n",
    "    head_angle = calculate_head_yaw(LeftEye , RightEye, LeftEar , RightEar, Lshoulder, Rshoulder)\n",
    "    Lshoulder_angle = findAngle(Lelbow, Lshoulder, Lhip)\n",
    "    LElbow_angle= findAngle(Lshoulder , Lelbow , Lwrist)\n",
    "    Rshoulder_angle = findAngle(Relbow, Rshoulder, Rhip)\n",
    "    RElbow_angle = findAngle(Rshoulder , Relbow , Rwrist)\n",
    "\n",
    "    KneePitch , HipPitch = knee_hip_pitch(Lshoulder, Rshoulder, Lhip, Rhip, LKnee, RKnee, LAnkle, RAnkle)\n",
    "\n",
    "    Lshoulder_pitch = arm_pitch(Lwrist , Lelbow ,Lshoulder, Lhip )\n",
    "    Rshoulder_pitch = arm_pitch(Rwrist , Relbow ,Rshoulder, Rhip)\n",
    "    # LShoulder_pitch_angle = shoulder_yaw_angle(Lshoulder , Lelbow , Lwrist)\n",
    "    # RShoulder_pitch_angle = shoulder_yaw_angle(Rshoulder , Relbow , Rwrist)\n",
    "\n",
    "    data = {\n",
    "            \"LShoulderRoll\": Lshoulder_angle , \n",
    "            \"RShoulderRoll\": -Rshoulder_angle , \n",
    "            \"LElbowRoll\": -LElbow_angle , \n",
    "            \"RElbowRoll\": -RElbow_angle, \n",
    "            \"HeadYaw\": -head_angle , \n",
    "            # \"LShoulderPitch\": -Lshoulder_pitch+1.57,\n",
    "            # \"RShoulderPitch\": -Rshoulder_pitch+1.57,\n",
    "            # \"RKneePitch\": -KneePitch , \n",
    "            # \"LKneePitch\": -KneePitch , \n",
    "            # \"LHipPitch\": -HipPitch , \n",
    "            # \"RHipPitch\": -HipPitch , \n",
    "            # \"right_ankle\": AnklePitch , \n",
    "            # \"left_ankle\": AnklePitch,\n",
    "            # \"left_shoulder_pitch\":LShoulder_pitch_angle,\n",
    "            # \"right_shoulder_pitch\":RShoulder_pitch_angle\n",
    "            }\n",
    "\n",
    "    with open('angles.json', 'w') as json_file:   \n",
    "        json.dump(data, json_file)\n",
    "    time.sleep(1)\n",
    "\n",
    "    display_image = tf.expand_dims(image, axis=0)\n",
    "    display_image = tf.cast(tf.image.resize_with_pad(\n",
    "        display_image, 1280, 1280), dtype=tf.int32)\n",
    "    output_overlay = draw_prediction_on_image(\n",
    "        np.squeeze(display_image.numpy(), axis=0), keypoints_with_scores)\n",
    "    cv2.imshow('output_overlay' ,output_overlay)\n",
    "    if cv2.waitKey(1) & 0xFF == ord('q'):\n",
    "        break\n",
    "  \n",
    "vid.release() \n",
    "cv2.destroyAllWindows()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.5"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}