Helper_class2.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Math stuff\n",
    "import numpy as np\n",
    "import math\n",
    "\n",
    "#3D Object stuff\n",
    "import pyvista as pv\n",
    "\n",
    "#Mujoco stuff\n",
    "#Environment stuff\n",
    "import gym\n",
    "import mujoco_py"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def check_in_target_sphere(sphere_center_point, EE_cordinates, sphere_radius = 0.05):\n",
    "    EE_cordinate_mesh = pv.PolyData(EE_cordinates)\n",
    "\n",
    "    # Generate a sphere around the center point\n",
    "    sphere = pv.Sphere(radius=sphere_radius)\n",
    "    sphere = sphere.translate(sphere_center_point)\n",
    "\n",
    "    intersection_set = EE_cordinate_mesh.select_enclosed_points(sphere)\n",
    "\n",
    "\n",
    "    # Print the result of the intersection test\n",
    "    if intersection_set['SelectedPoints'].sum() >= 1:\n",
    "        print(\"intersected\")\n",
    "        return 1\n",
    "    else:\n",
    "        return 0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "class AgentUtils:\n",
    "    def __init__(self, simulation_model, threeD_Object, initial_action, target_coordinate, \n",
    "                 target_radius = 0.08):\n",
    "        self.sim = simulation_model\n",
    "        self.last_action = initial_action\n",
    "\n",
    "        self.threeD_object = threeD_Object\n",
    "\n",
    "        self.TARGET_COORDINATE = target_coordinate\n",
    "        self.target_radius = target_radius\n",
    "\n",
    "\n",
    "    def EE_to_target_distance_euclidean(self):\n",
    "        x_t,y_t,z_t = self.sim.data.get_body_xpos(\"EE\").copy()\n",
    "        x,y,z = self.TARGET_COORDINATE \n",
    "\n",
    "        distance = np.sqrt((x-x_t)**2 + (y-y_t)**2 + (z-z_t)**2)\n",
    "        return distance\n",
    "\n",
    "    def EE_to_target_distance_absolute(self):\n",
    "        x_t,y_t,z_t = self.sim.data.get_body_xpos(\"EE\").copy()\n",
    "        x,y,z = self.TARGET_COORDINATE \n",
    "\n",
    "        distance = np.abs(x-x_t) + np.abs(y-y_t) + np.abs(z-z_t)\n",
    "        return distance\n",
    "\n",
    "    def get_reward(self):\n",
    "        distance = -self.EE_to_target_distance_euclidean()\n",
    "        return distance\n",
    "    \n",
    "    def get_reward_exp_reward(self, EE_pos, exponent_steepness=1):\n",
    "        distance = self.arm_robot_reward_function(EE_pos=EE_pos)\n",
    "        return (np.exp(distance) * exponent_steepness)\n",
    "    \n",
    "\n",
    "    def huber_loss_reward(self):\n",
    "        if (self.EE_to_target_distance_euclidean() < 0.1):\n",
    "            return 1/2 * self.get_reward()\n",
    "        else:\n",
    "            return 0.1*(-self.EE_to_target_distance_absolute() - 1/2*0.1)\n",
    "\n",
    "    def check_EE_is_in_the_target_area(self):\n",
    "        EE_coordinates = self.sim.data.get_body_xpos(\"EE\").copy()\n",
    "        #EE_coordinate_mesh = pv.PolyData(EE_coordinates)\n",
    "\n",
    "        # Generate a sphere around the center point\n",
    "        sphere = pv.Sphere(radius=self.target_radius, center=self.TARGET_COORDINATE)\n",
    "        index = sphere.find_closest_point(EE_coordinates)\n",
    "\n",
    "        point_coordinates = sphere.points[index]\n",
    "\n",
    "        #compute the distance\n",
    "        distance = np.sqrt(np.sum((EE_coordinates - point_coordinates)**2))\n",
    "        if distance < 0.03:\n",
    "            print(\"intersected\")\n",
    "            return 1\n",
    "        else:\n",
    "            return 0\n",
    "        #intersection_set = EE_coordinate_mesh.select_enclosed_points(sphere)\n",
    "\n",
    "        # Print the result of the intersection test\n",
    "        # if intersection_set['SelectedPoints'].sum() >= 1:\n",
    "        #     print(\"intersected\")\n",
    "        #     return 1\n",
    "        # else:\n",
    "        #     return 0\n",
    "    \n",
    "    #This modifies the angles to be circular\n",
    "    def get_joint_angles(self, action):\n",
    "        \n",
    "\n",
    "        old_joint_angles = self.last_action\n",
    "\n",
    "        sum_angles = old_joint_angles + action.copy()\n",
    "    \n",
    "        # Wrap the result within the range of -2π to 2π\n",
    "        sum_angles_wrapped = sum_angles % (2 * math.pi)\n",
    "        \n",
    "        # Adjust the result to be within the range of -π to π\n",
    "        new_joint_angles = []\n",
    "        for angle in sum_angles_wrapped:\n",
    "            if angle > math.pi:\n",
    "                angle -= 2 * math.pi\n",
    "            elif angle < -math.pi:\n",
    "                angle += 2 * math.pi\n",
    "            new_joint_angles.append(angle)\n",
    "        \n",
    "        self.last_action = new_joint_angles\n",
    "        \n",
    "        return new_joint_angles\n",
    "\n",
    "    def buttom_platform_touched(self):\n",
    "        links_pos = self.get_non_stationary_links_and_EE_coordinates()\n",
    "        \n",
    "        crashed_arr = [1 if link_pos[2] <= 0.01 else 0 for link_pos in links_pos]\n",
    "        if(np.sum(crashed_arr) > 0):\n",
    "            print(\"Crashed Platform\")\n",
    "            return 1\n",
    "        else:\n",
    "            return 0\n",
    "\n",
    "        \n",
    "    def get_non_stationary_links_and_EE_coordinates(self):\n",
    "        \n",
    "        link_indicies = [i for i in range(9,self.sim.model.nbody)] #From 9 to end is Link2 to EE\n",
    "        links_position = [self.sim.data.xipos[index] for index in link_indicies]  \n",
    "\n",
    "        return np.array(links_position)\n",
    "\n",
    "    def link_distances_in_next_state(self, next_state_actions_batch):\n",
    "        \n",
    "        #current_state_distance = np.array([])\n",
    "        next_state_distance = np.array([])\n",
    "\n",
    "        for action in next_state_actions_batch:\n",
    "\n",
    "            self.sim.data.qpos[:] = action\n",
    "\n",
    "            try:\n",
    "                self.sim.step()\n",
    "            except mujoco_py.MujocoException as e:\n",
    "                if \"Got MuJoCo Warning: Nan, Inf or huge value in QACC\" in str(e):\n",
    "                    print(\"Simulation is unstable during training. Taking corrective action...\")\n",
    "                    # Do something to correct the instability, e.g. reset the simulation or adjust the control parameters\n",
    "                    continue\n",
    "                else:\n",
    "                    # Handle other MujocoExceptions as needed\n",
    "                    print(f\"Unexpected MujocoException: {e}\")\n",
    "\n",
    "\n",
    "            next_state_closest_distances_to_obstacle = self.threeD_object.compute_distance_to_object(\n",
    "                        self.threeD_object.link_closest_point_to_object() )\n",
    "            \n",
    "            #current_state_distance = np.append(current_state_distance,state_distance)\n",
    "            next_state_distance = np.append(next_state_distance, next_state_closest_distances_to_obstacle)\n",
    "            \n",
    "        return next_state_distance\n",
    "\n",
    "   \n",
    "    def get_objective_target_coordinate(self):\n",
    "        return self.TARGET_COORDINATE"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "class ThreeD_Obstacle():\n",
    "    def __init__(self, simulation, sphere_or_ellipsoid, radius_or_axis, \n",
    "                 my_position, safety_l1, safety_l2, target_pos,\n",
    "                  translation_rate = [0,0,0]):\n",
    "        #simulation stuff\n",
    "        self.sim = simulation\n",
    "       \n",
    "        #Target stuff\n",
    "        self.target_pos = target_pos\n",
    "        \n",
    "        #object stuff\n",
    "        #Given as radius or half a width\n",
    "        self.threeD_obstable = None\n",
    "        self.l1_layer_mesh = None\n",
    "        self.l2_layer_mesh = None\n",
    "        self.my_center_pos = my_position #aka center\n",
    "        self.initial_center_pos = my_position\n",
    "        self.translation_rate = translation_rate\n",
    "        self.translate_forward = True\n",
    "        self.object_hit_count = 0 ##To keep track how many times the object was hit by the robot\n",
    "\n",
    "\n",
    "        if(sphere_or_ellipsoid == \"Sphere\" or sphere_or_ellipsoid == \"sphere\" ):\n",
    "            self.threeD_obstable = pv.Sphere(radius=radius_or_axis, center=self.my_center_pos)\n",
    "            #self.threeD_obstable = self.threeD_obstable.translate(self.my_center_pos)\n",
    "\n",
    "            self.l1_layer_mesh = pv.Sphere(radius=safety_l1, center= self.my_center_pos)\n",
    "            self.l2_layer_mesh = pv.Sphere(radius=safety_l2, center= self.my_center_pos)\n",
    "            # self.l1_layer_mesh = self.l1_layer_mesh.translate(self.my_center_pos)\n",
    "            # self.l2_layer_mesh = self.l2_layer_mesh.translate(self.my_center_pos)\n",
    "        else:\n",
    "            self.threeD_obstable = pv.ParametricEllipsoid(radius_or_axis[0],\n",
    "                                                          radius_or_axis[1],radius_or_axis[2])\n",
    "            self.threeD_obstable = self.threeD_obstable.translate(self.my_center_pos)\n",
    "\n",
    "            self.l1_layer_mesh = pv.ParametricEllipsoid(safety_l1[0],\n",
    "                                                          safety_l1[1],safety_l1[2])\n",
    "            self.l2_layer_mesh = pv.ParametricEllipsoid(safety_l2[0],\n",
    "                                                          safety_l2[1],safety_l2[2])\n",
    "\n",
    "            self.l1_layer_mesh = self.l1_layer.translate(self.my_center_pos)\n",
    "            self.l1_layer_mesh = self.l2_layer.translate(self.my_center_pos)\n",
    "\n",
    "  \n",
    "        if not isinstance(self.my_center_pos , np.ndarray):\n",
    "            self.my_center_pos  = np.array(self.my_center_pos )\n",
    "        #Ensuring that the translation rates per x,y,z coordinates array is a numpy.array\n",
    "        if not isinstance(translation_rate, np.ndarray):\n",
    "            self.translation_rate = np.array(self.translation_rate)\n",
    "\n",
    "    def get_center_pos(self):\n",
    "        return self.my_center_pos\n",
    "\n",
    "    def get_non_stationary_links_and_EE_coordinates(self):\n",
    "        \n",
    "        link_indicies = [i for i in range(9,self.sim.model.nbody)] #From 9 to end is Link2 to EE\n",
    "        links_position = [self.sim.data.xipos[index] for index in link_indicies]  \n",
    "\n",
    "        return np.array(links_position)\n",
    "    \n",
    "\n",
    "    #this function will serve for the object, for the inner layer and outer layer checks\n",
    "    def intruded(self, l1_12_object):\n",
    "\n",
    "        link_and_EE_coordinates = self.get_non_stationary_links_and_EE_coordinates()\n",
    "        meshes = [pv.PolyData(pos) for pos in link_and_EE_coordinates]\n",
    "\n",
    "        intersection_list = [mesh.select_enclosed_points(l1_12_object)['SelectedPoints'] for mesh in meshes]\n",
    "        return np.concatenate(intersection_list)\n",
    "    \n",
    "    def object_intruded(self):\n",
    "        intersection_list = self.intruded(self.threeD_obstable)\n",
    "        return intersection_list\n",
    "\n",
    "    def l1_intruded(self):\n",
    "        intersection_list = self.intruded(self.l1_layer_mesh)\n",
    "        return intersection_list\n",
    "    \n",
    "    def l2_intruded(self):\n",
    "        intersection_list = self.intruded(self.l2_layer_mesh)\n",
    "        return intersection_list\n",
    "    \n",
    "    def find_closes_point(self, l1_12_object):\n",
    "        link_and_EE_coordinates = self.get_non_stationary_links_and_EE_coordinates()\n",
    "        # print(link_and_EE_coordinates)\n",
    "        # print(self.threeD_obstable.find_closest_point(link_and_EE_coordinates[0]))\n",
    "        closest_points = [l1_12_object.find_closest_point(link_and_EE_pos) for link_and_EE_pos in link_and_EE_coordinates]\n",
    "        return np.array(closest_points)\n",
    "\n",
    "    def link_closest_point_to_object(self):\n",
    "        closest_points = self.find_closes_point(self.threeD_obstable)\n",
    "        closest_points_pos = self.threeD_obstable.points[closest_points]\n",
    "        \n",
    "        return closest_points_pos\n",
    "\n",
    "    def link_closest_point_to_l1(self):\n",
    "        closest_points = self.find_closes_point(self.l1_layer_mesh)\n",
    "        closest_points_pos = self.l1_layer_mesh.points[closest_points]\n",
    "        \n",
    "        return closest_points_pos\n",
    "    \n",
    "    def link_closest_point_to_l2(self):\n",
    "        closest_points = self.find_closes_point(self.l2_layer_mesh)\n",
    "        closest_points_pos = self.l2_layer_mesh.points[closest_points]\n",
    "        return closest_points_pos\n",
    "    \n",
    "    def compute_distance_to(self, closest_positions_on_l1_l2_object):\n",
    "        link_and_EE_coordinates = self.get_non_stationary_links_and_EE_coordinates()\n",
    "        \n",
    "        distances = [np.sqrt(np.sum((link_and_EE_pos - closest_pos)**2)) \n",
    "                     for link_and_EE_pos, closest_pos in zip(link_and_EE_coordinates, closest_positions_on_l1_l2_object)]\n",
    "        \n",
    "\n",
    "\n",
    "        return np.array(distances)  \n",
    "\n",
    "    def compute_distance_to_object(self, closest_positions_on_l1_l2_object):\n",
    "        return self.compute_distance_to(closest_positions_on_l1_l2_object)\n",
    "    def compute_distance_to_l1(self, closest_positions_on_l1_l2_object):\n",
    "        distances = self.compute_distance_to(closest_positions_on_l1_l2_object)\n",
    "\n",
    "        link_and_EE_coordinates = self.get_non_stationary_links_and_EE_coordinates()\n",
    "        meshes = [pv.PolyData(pos) for pos in link_and_EE_coordinates]\n",
    "        intersection_list = [mesh.select_enclosed_points(self.l1_layer_mesh)['SelectedPoints'][0] for mesh in meshes]\n",
    "\n",
    "        distances = [0 if inter == 1 else dist for dist, inter in zip(distances, intersection_list)]\n",
    "        return distances\n",
    "    \n",
    "    def compute_distance_to_l2(self, closest_positions_on_l1_l2_object):\n",
    "        return self.compute_distance_to(closest_positions_on_l1_l2_object)\n",
    "    \n",
    "    def compute_distance_to_l1_when_in_l2_by_closest_points(self, closest_positions_on_l1_l2_object):\n",
    "        closest_pos_to_l1 = self.link_closest_point_to_l1()\n",
    "        closest_pos_to_l2 = self.link_closest_point_to_l2()\n",
    "        distances_to_l1 = self.compute_distance_to_l1(closest_positions_on_l1_l2_object)\n",
    "\n",
    "        distances_norm = [ np.sqrt(np.sum((dl1-dl2)**2)) for dl1, dl2 in zip(closest_pos_to_l1, closest_pos_to_l2)]\n",
    "        distances_to_l1 = distances_to_l1/distances_norm\n",
    "        return distances_to_l1\n",
    "\n",
    "    def compute_link_distance_to_object_when_in_l1_by_closest_points_for_cost(self):\n",
    "        l1_intruded_check_array = self.l1_intruded()\n",
    "        object_intruded = self.object_intruded()\n",
    "\n",
    "        if(any(object_intruded)):\n",
    "            return 1.0\n",
    "        elif(any(l1_intruded_check_array)):\n",
    "\n",
    "            # closest_pos_to_ob = self.link_closest_point_to_object()\n",
    "            # closest_pos_to_l1 = self.link_closest_point_to_l1()\n",
    "            \n",
    "            # distances_to_ob = self.compute_distance_to_object(closest_pos_to_ob)\n",
    "\n",
    "            # distances_norm = [ np.sqrt(np.sum((dob-dl1)**2)) for dob, dl1 in zip(closest_pos_to_ob, closest_pos_to_l1)]\n",
    "            # distances_to_ob = distances_to_ob/distances_norm\n",
    "            #distances_to_ob = (1 - np.min(distances_to_ob)) * 100 + 10\n",
    "            return 1.0\n",
    "        else:\n",
    "            return 0\n",
    "\n",
    "    def compute_link_distance_to_object_when_in_l1_by_closest_points_for_repulsion(self):\n",
    "        l1_intruded_check_array = self.l1_intruded()\n",
    "        object_intruded = self.object_intruded()\n",
    "\n",
    "        if(any(object_intruded)):\n",
    "            return 0 + 1e-4\n",
    "        elif(any(l1_intruded_check_array)):\n",
    "\n",
    "            closest_pos_to_ob = self.link_closest_point_to_object()\n",
    "            closest_pos_to_l1 = self.link_closest_point_to_l1()\n",
    "            \n",
    "            distances_to_ob = self.compute_distance_to_object(closest_pos_to_ob)\n",
    "\n",
    "            distances_norm = [ np.sqrt(np.sum((dob-dl1)**2)) for dob, dl1 in zip(closest_pos_to_ob, closest_pos_to_l1)]\n",
    "            distances_to_ob = distances_to_ob/distances_norm\n",
    "            return np.min(distances_to_ob)\n",
    "        else:\n",
    "            return 1\n",
    "\n",
    "    ########### Intrusions by target\n",
    "    def intruded_by_target(self, l1_12_object):\n",
    "       \n",
    "        target_mesh = pv.PolyData(self.target_pos)\n",
    "        intersection = target_mesh.select_enclosed_points(l1_12_object)['SelectedPoints']\n",
    "        return np.sum(intersection)\n",
    "\n",
    "    def goal_intruded_object(self):\n",
    "        return self.intruded_by_target(self.threeD_obstable)\n",
    "\n",
    "    def goal_intruded_l1(self):\n",
    "        return self.intruded_by_target(self.l1_layer_mesh)   \n",
    "    \n",
    "    def find_closet_point_to_goal(self, l1_12_object):\n",
    "        closest_point = l1_12_object.find_closest_point(self.target_pos)\n",
    "        closest_coordinate = l1_12_object.points[closest_point]\n",
    "        return np.array(closest_coordinate)\n",
    "    \n",
    "    def compute_object_distance_to_goal_norm(self):\n",
    "        if(self.goal_intruded_object()):\n",
    "            return 0\n",
    "        elif(self.goal_intruded_l1()):\n",
    "            coordinate_to_object = self.find_closet_point_to_goal(self.threeD_obstable)\n",
    "            coordinate_to_l1 = self.find_closet_point_to_goal(self.l1_layer_mesh)\n",
    "\n",
    "            distance_goal_to_object = np.sqrt(np.sum((self.target_pos - coordinate_to_object)**2))\n",
    "            norm = np.sqrt(np.sum((coordinate_to_object - coordinate_to_l1)**2))\n",
    "            return distance_goal_to_object/norm\n",
    "        else:\n",
    "            return 1.0\n",
    "\n",
    "\n",
    "    def obstacle_proximity_to_target_penalty(self):\n",
    "        object_intruded = self.goal_intruded_object()\n",
    "        l1_intruded = self.goal_intruded_l1()\n",
    "\n",
    "        if(object_intruded == 1):\n",
    "            return 0.0 + 1e-4\n",
    "        elif(l1_intruded):\n",
    "            return self.compute_object_distance_to_goal_norm()\n",
    "        else:\n",
    "            return 1\n",
    "\n",
    "\n",
    "    def safety_penalty_by_object_proximity_to_goal(self):\n",
    "        l1_intruded_check_array = self.l1_intruded()\n",
    "        object_intruded_check_array = self.object_intruded()\n",
    "\n",
    "        if(any(object_intruded_check_array) or any(l1_intruded_check_array)):\n",
    "            #closest_pos_to_ob = self.link_closest_point_to_object()\n",
    "            penalty = self.obstacle_proximity_to_target_penalty()\n",
    "            \n",
    "            return penalty\n",
    "        else:\n",
    "            return 1.0\n",
    "\n",
    "\n",
    "    def linearly_translate_object_back_and_forward(self):\n",
    "        #Translation does not go overly bellow 0 in the z-axis\n",
    "        if self.translate_forward and (self.my_center_pos[0] >= 1.6 or self.my_center_pos[1] >= 1.6 or self.my_center_pos[2] >= 1.6):\n",
    "            self.translate_forward = False\n",
    "            self.translation_rate = -self.translation_rate\n",
    "        elif not self.translate_forward and (self.my_center_pos[0] <= -1.6 or self.my_center_pos[1] <= -1.6 or self.my_center_pos[2] < 0.0):\n",
    "            self.translate_forward = True\n",
    "            self.translation_rate = -self.translation_rate\n",
    "\n",
    "        self.threeD_obstable = self.threeD_obstable.translate(self.translation_rate)\n",
    "        self.l1_layer_mesh = self.l1_layer_mesh.translate(self.translation_rate)\n",
    "        self.l2_layer_mesh = self.l2_layer_mesh.translate(self.translation_rate)\n",
    "        \n",
    "        self.my_center_pos = self.threeD_obstable.center\n",
    "    \n",
    "    def pseudo_random_linear_translation(self):\n",
    "        #Translation does not go overly bellow 0 in the z-axis\n",
    "\n",
    "        x = np.random.randint([3],[10])[0]/1000\n",
    "        \n",
    "        if self.translate_forward:\n",
    "            self.translation_rate[0] = x\n",
    "        else:\n",
    "            self.translation_rate[0] = -x\n",
    "            \n",
    "        if self.translate_forward and (self.my_center_pos[0] >= 1.6 or self.my_center_pos[1] >= 1.6 or self.my_center_pos[2] >= 1.6):\n",
    "            self.translate_forward = False\n",
    "            self.translation_rate = -self.translation_rate\n",
    "        elif not self.translate_forward and (self.my_center_pos[0] <= -1.6 or self.my_center_pos[1] <= -1.6 or self.my_center_pos[2] < 0.0):\n",
    "            self.translate_forward = True\n",
    "            self.translation_rate = -self.translation_rate\n",
    "\n",
    "\n",
    "        \n",
    "        self.threeD_obstable = self.threeD_obstable.translate(self.translation_rate)\n",
    "        self.l1_layer_mesh = self.l1_layer_mesh.translate(self.translation_rate)\n",
    "        self.l2_layer_mesh = self.l2_layer_mesh.translate(self.translation_rate)\n",
    "        \n",
    "        self.my_center_pos = self.threeD_obstable.center\n",
    "\n",
    "    def reset(self):\n",
    "        self.translate_forward = True\n",
    "        self.my_center_pos = self.initial_center_pos\n",
    "\n",
    "\n",
    "    \n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Features:\n",
    "    def __init__(self, simulation_model, threeD_obstacle, agent_util):\n",
    "        self.sim = simulation_model\n",
    "        self.link_set = ['base_link','link1','link2','link3','link4','link5','link6','EE']\n",
    "\n",
    "        self.obstacle = threeD_obstacle\n",
    "\n",
    "        self.agent_util = agent_util\n",
    "\n",
    "    def get_EE_coordinates(self):\n",
    "        EE_position = self.sim.data.get_body_xpos(\"EE\").copy()\n",
    "        return EE_position\n",
    "    \n",
    "    def get_non_stationary_links_and_EE_coordinates(self):\n",
    "        \n",
    "        link_indicies = [i for i in range(9,self.sim.model.nbody)] #From 9 to end is Link2 to EE\n",
    "        links_position = [self.sim.data.xipos[index] for index in link_indicies]  \n",
    "\n",
    "        return np.array(links_position)\n",
    "      \n",
    "    def robot_links_distance_to_obstacle_for_feature(self):  \n",
    "        closest_links_pos_to_object =  self.obstacle.link_closest_point_to_object()\n",
    "        links_and_EE_distances_to_obj = self.obstacle.compute_distance_to_object(\n",
    "            closest_links_pos_to_object)\n",
    "        \n",
    "        return links_and_EE_distances_to_obj\n",
    "    \n",
    "    def robot_links_intruded_l1_mesh_for_feature(self):  \n",
    "        closest_links_pos_to_l1 =  self.obstacle.link_closest_point_to_l1()\n",
    "        links_and_EE_distances_to_l1 = self.obstacle.compute_distance_to_l1(\n",
    "           closest_links_pos_to_l1)\n",
    "        \n",
    "        #intrusion = self.obstacle.l1_intruded()\n",
    "        \n",
    "        return links_and_EE_distances_to_l1\n",
    "    \n",
    "    def get_obstacle_coordinate_for_feature(self):\n",
    "        return self.obstacle.get_center_pos()\n",
    "\n",
    "    #robot_link_set = ['base_link','link1','link2','link3','link4','link5','link6']\n",
    "    def compute_robot_center_of_mass_for_feature(self):\n",
    "        links_index = [self.sim.model.body_name2id(link) for link in self.link_set]\n",
    "        mass_of_links = self.sim.model.body_mass[links_index]\n",
    "\n",
    "        total_mass = mass_of_links.sum()\n",
    "\n",
    "        x_mul_mass = (self.sim.data.xipos[links_index][:,0] * mass_of_links).sum()\n",
    "        y_mul_mass = (self.sim.data.xipos[links_index][:,1] * mass_of_links).sum()\n",
    "        z_mul_mass = (self.sim.data.xipos[links_index][:,2] * mass_of_links).sum()\n",
    "\n",
    "        center_of_mass = np.array([x_mul_mass,y_mul_mass,z_mul_mass])/total_mass\n",
    "        return center_of_mass\n",
    "\n",
    "    def get_joints_angular_velocities_for_feature(self):\n",
    "        return self.sim.data.qvel\n",
    "    \n",
    "    def get_joints_sine_and_cosine_angle_for_feature(self):\n",
    "        sine_angles = np.sin(self.sim.data.qpos) #cosine and sin from each joints angle in radians\n",
    "        cosine_angles = np.cos(self.sim.data.qpos)\n",
    "\n",
    "        return sine_angles, cosine_angles\n",
    "    \n",
    "    def get_joints_randian_angle_for_feature(self):\n",
    "        return self.sim.data.qpos.copy()\n",
    "    \n",
    "    def get_all_features(self):\n",
    "        #sine_angles, cosine_angles = self.get_joints_sine_and_cosine_angle_for_feature()\n",
    "        # print(\"angular vel\", self.get_joints_angular_velocities_for_feature())\n",
    "        # print()\n",
    "        # print(\"distances to obstacle\", self.robot_links_distance_to_obstacle_for_feature())\n",
    "        # print()\n",
    "        # print(\"EE coordinates\", self.get_EE_coordinates())\n",
    "        # print()\n",
    "        # print(\"Joint in radians\", self.get_joints_randian_angle_for_feature())\n",
    "        \n",
    "        all_features = np.concatenate((#self.get_EE_coordinates(),\n",
    "                                       self.compute_robot_center_of_mass_for_feature().copy(), \n",
    "                                       self.get_joints_angular_velocities_for_feature().copy(),\n",
    "                                       #sine_angles,\n",
    "                                       #cosine_angles,\n",
    "                                       self.get_joints_randian_angle_for_feature().copy(),\n",
    "                                       self.get_obstacle_coordinate_for_feature().copy(),\n",
    "                                       self.robot_links_distance_to_obstacle_for_feature().copy(),\n",
    "                                       self.get_non_stationary_links_and_EE_coordinates().copy().flatten(),\n",
    "                                       self.robot_links_intruded_l1_mesh_for_feature().copy(),\n",
    "                                       ))\n",
    "        return all_features\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "rlenv1",
   "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.8.16"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}