3_Reachy_the_greengrocer.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Tutorial n°3 - SDK & Pollen-Vision : Reachy the greengrocer"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this tutorial, we will learn how to do tasks with Reachy using :\n",
    "- the SDK client to make it move,\n",
    "- the module pollen-vision to use object detection in its environment. \n",
    "\n",
    "Here, we are going to ask Reachy to sort fruits on a table and to drop them in user-defined places, according to which fruit it is. \n",
    "\n",
    "What you will learn : \n",
    "- How to do object detection\n",
    "- How to switch from the image frame to the robot frame\n",
    "- How to make Reachy move according to what it sees"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Prerequisites\n",
    "\n",
    "To use the SDK client and pollen-vision, you first need to install them. If it is not the case, checkout the section below 👇\n",
    "<details>\n",
    "\n",
    "<summary>Install the Python library reachy2-sdk</summary>\n",
    "\n",
    "In general, you'd better **work in a virtual environment**. You have 2 different ways to install the sdk : \n",
    "- by running the following command:\n",
    "\n",
    "<code>\n",
    "pip install reachy2-sdk -e .\n",
    "</code>\n",
    "\n",
    "- from source by following the instructions on the [GitHub repository](https://github.com/pollen-robotics/reachy2-sdk)\n",
    "\n",
    "</details>\n",
    "\n",
    "<details>\n",
    "\n",
    "<summary>Install the Python library pollen-vision</summary>\n",
    "\n",
    "In general, you'd better **work in a virtual environment**. You have 2 different ways to install the sdk : \n",
    "- by running the following command:\n",
    "\n",
    "<code>\n",
    "pip install pollen-vision\n",
    "</code>\n",
    "\n",
    "- from source by following the instructions on the [GitHub repository](https://github.com/pollen-robotics/pollen-vision)\n",
    "\n",
    "</details>\n",
    "\n",
    "If you have never used the **SDK client** before, don't forget to do the [Getting Started notebooks](https://github.com/pollen-robotics/reachy2-sdk/tree/develop/src/examples), that will help you understand all the basics you need to know about it ! And if you want to go further, you can also check the [SDK documentation](https://pollen-robotics.github.io/reachy2-sdk/reachy2_sdk.html). \n",
    "\n",
    "You can also familiarize yourself with **pollen-vision** by making the [Vision Models notebooks](https://github.com/pollen-robotics/pollen-vision/tree/develop/examples/vision_models_examples). \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Set up\n",
    "\n",
    "### 2.1. Material\n",
    "\n",
    "For this tutorial, you have to set up your environment. \n",
    "So you need :\n",
    "- Reachy (obviously)\n",
    "- a table, no higher than the robot's elbows\n",
    "- a white plate\n",
    "- a white bowl\n",
    "- apples\n",
    "- oranges\n",
    "\n",
    "\n",
    "### 2.2. Scene\n",
    "\n",
    "> Your Reachy must be at a sufficient **height** so that its outstretched arms do not touch the mobile base. \n",
    "\n",
    "> As usual, Reachy needs to be in a safe environment with enough place to move around, no one in reachable space and no obstacles. And always keep the emergency stop button nearby!\n",
    "\n",
    "To set up your scene : \n",
    "\n",
    "1.    Switch Reachy on and place it facing the table but 20 cm back (*it will need to bent its arms without obstacle, then it will be moved forward to the edge of the table*). \n",
    "2.    Make sure the torso camera is connected to the bedrock, as that's the one we'll be using here (*it's the one pluged on the outermost USB port*).\n",
    "3.    On the table, place the white plate on the left, then the oranges, the apples and the white bowl, such as the image below. Don't put them too far, Reachy doesn't have expandable arms.\n",
    "4.    You will need good lighting so that objects can be detected.\n",
    "\n",
    "<p align=\"center\"> \n",
    "    <img src=\"images/set_up_tuto_profil.jpg\" alt=\"Image 1\" style=\"width: 22.5%; margin-right: 10px;\"/>\n",
    "    <img src=\"images/set_up_tuto_face.jpg\" alt=\"Image 2\" style=\"width: 40%;\"/>\n",
    "</p>\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When you're all set up, you can move on to the next step."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3. Preview\n",
    "\n",
    "In this tutorial, we'll gradually build the program that will enable Reachy to sort fruit using the SDK Client and Pollen-Vision. This is what it your Reachy will be able to do at the end ! \n",
    "\n",
    "<p align = \"center\"> \n",
    "    <img src=\"images/gif_oranges.gif\" alt=\"Gif 1\" style=\"width: 27.5%; margin-right: 10px;\"/>\n",
    "    <img src=\"images/gif_apple_human.gif\" alt=\"Gif 2\" style=\"width: 40%; \"/>\n",
    "</p>\n",
    "\n",
    "Exciting, isn't it ? So let's get to work !"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4. Let's build it ! \n",
    "\n",
    "### 4.1. Instanciation of the SDK"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First, we will connect to the robot. To do so, we need to import the SDK client package and to instanciate a ReachySDK. That module is the one allowing us to control Reachy.\n",
    "\n",
    "Two requirements :\n",
    "- Your computer needs to be on the same network as the robot.\n",
    "- You need to know your Reachy's IP : to do so, you have two options : \n",
    "    - you can check it on the dashboard (*.local_IP_address_of_your_robot:8000* in a browser), section Network. \n",
    "    - you can have a look at the small screen on Reachy's back, that will show you one at a time its Ethernet and its Wifi IP. \n",
    "\n",
    "Now, let's connect to it. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#import the package\n",
    "from reachy2_sdk import ReachySDK \n",
    "\n",
    "#connect to the robot\n",
    "reachy = ReachySDK('localhost') # replace 'localhost' with the actual IP address of your Reachy\n",
    "print(\"Reachy is connected :\", reachy.is_connected())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you are getting the message \"Could not connect to Reachy\", make sure that :\n",
    "-  Reachy is turned on\n",
    "- the reachy2_core.service is running \n",
    "- the IP address is correct. \n",
    "\n",
    "*More info on the debug section of the [sdk documentation](https://pollen-robotics.github.io/reachy2-docs/help/help/recovering/).<br>*"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4.2. Instanciation of Pollen-Vision \n",
    "\n",
    "Then, we will use Pollen-Vision repository to be able to detect objects from the camera view thanks to AI and extract informations about them, for Reachy to adapt its actions to its environment.\n",
    "\n",
    "More specifically, we'll be using the *Perception* module, which combines object detection using a [YOLO model](https://docs.ultralytics.com/models/yolo-world/), image segmentation and conversion of the extracted informations from pixels into real-world coordinates. \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 4.2.1. How does it work\n",
    "\n",
    "We set the name of the objects we're interested in, and the model detects those objects with a prediction confidence score (from 0, the lowest, to 1, the highest). And from those detections, we get different informations about the objects such as the coordinates of the bounding box, the centroid, and the depth. As this is done on Reachy's camera view, we can track the objects dynamically and minimize the impact of artifacts by retaining only the objects that are always detected from one frame to the next. \n",
    "\n",
    "Then, we can switch from pixels frame to Reachy's frame and get the informations of the objects in the real world frame, using parameters such as the camera intrinsic matrix (that allows to transform pixel coordinates into 3D coordinates from the camera frame) and the transform of the camera in Reachy'sframe (*T_reachy_cam*, which is known and allows to switch from the camera to Reachy's frame of reference). \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 4.2.2. How to instanciate it\n",
    "\n",
    "To define it, we need to get all the informations we talked about before : \n",
    "\n",
    "- the intrinsic matrix of the camera, that can be found thanks to the PollenSDKCameraWrapper, a class of pollen-vision that allows to get informations about Reachy's cameras. \n",
    "- the T_reachy_cam\n",
    "- the prediction score threshold for the objects to be considered\n",
    "- the frequency at which we want to perform object detection on the camera frames"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "##### **PollenSDKCameraWrapper**\n",
    "\n",
    "We are going to use this module to get informations about Reachy's camera, such as : \n",
    "- the images captured by the camera, with *get_data()* \n",
    "- parameters like the camera intrinsic matrix (named K by convention) with *get_K()* for the RGB camera, and *get_depth_K()* for the depth camera. \n",
    "\n",
    "We need to instanciate it, with the SDK client object we instanciated earlier. Then, we are going to print the image captured by the torso camera when you execute the cell. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from pollen_vision.camera_wrappers.pollen_sdk_camera.pollen_sdk_camera_wrapper import PollenSDKCameraWrapper\n",
    "\n",
    "# instanciation of the camera wrapper\n",
    "r_cam = PollenSDKCameraWrapper(reachy)\n",
    "\n",
    "# get the image as a np.array, and the timestamp of the image\n",
    "data, _, timestamp = r_cam.get_data()\n",
    "\n",
    "# displays the image from the RGB camera (depth camera is available by changing 'left' to 'depth')\n",
    "from PIL import Image\n",
    "img = data['left']\n",
    "Image.fromarray(img) "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "##### **T_reachy_cam**\n",
    "\n",
    "Now, we need to define the T_reachy_cam. The camera frame is oriented differently from the robot frame, with the x axis to the left, the y axis down and the z axis back (as a reminder, the robot frame of reference has the x axis forward, the y axis to the left and the z axis up). Moreover, the torso camera is tilted downwards. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from reachy2_sdk.utils.utils import invert_affine_transformation_matrix\n",
    "\n",
    "T_cam_reachy = reachy.cameras.depth.get_extrinsics()\n",
    "T_reachy_cam = invert_affine_transformation_matrix(T_cam_reachy)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "##### **Labels** \n",
    "\n",
    "To know what to use as a frequency and prediction score threshold, you first need to set the labels you're gonna use to say what you want to detect. Indeed, to use IA model for object detection, it's important to find the most relevant label for the model to find the objects, and it's not always the most intuitive words. \n",
    "\n",
    "To test those, you can use directly two submodules used in Perception to see if your objects are correctly detected on your image :\n",
    "-  the YOLO model (*the one that detects objects*) \n",
    "-  the Annotator (*the one that allows the visualization*).\n",
    "\n",
    "Be creative, try even non-instinctive words, with more or less precise adjectives, and test everything you can ! \n",
    "\n",
    "In this tutorial, we decide to define two objects in which to place the fruit: a right-hand side and a left-hand side. And we'll define the type of fruit to be placed on the right and the one on the left. Here, we have a plate on the left and a bowl on the right, and the fruits to be placed are oranges and apples.\n",
    "\n",
    "We are going to try classic labels for the detection, in the candidate_labels, but you'll see that we need to adjust those because the detection won't be good enough. \n",
    "\n",
    "*Don't worry if the instanciation of the YoloWorldWrapper is taking a while, it can take about 1-1.5min to create it the first time. And you can have a warning message, with no impact on the rest of the tutorial*."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from pollen_vision.vision_models.object_detection import YoloWorldWrapper\n",
    "from pollen_vision.utils import get_bboxes\n",
    "from pollen_vision.utils import Annotator\n",
    "\n",
    "#those first labels won't be good enough\n",
    "labels = [\"orange\", \"apple\", \"plate\", \"bowl\"]\n",
    "\n",
    "yolo = YoloWorldWrapper() #allows to use the YOLO model for object detection\n",
    "annotator = Annotator() #allows to get annotated image with the detected objects\n",
    "yolo_predictions = yolo.infer(im=img, candidate_labels=labels, detection_threshold=0.15) \n",
    "bboxes = get_bboxes(yolo_predictions) #returns the bounding boxes of the detected objects\n",
    "img_annotated = annotator.annotate(im=img, detection_predictions=yolo_predictions) \n",
    "Image.fromarray(img_annotated)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, you could see that the oranges aren't detected as \"orange\" and may be detected as apples. We are going to try new labels to be more effective. Also, you can add adjectives to be sure not to have any false positive. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "labels = [\"orange ball\", \"apple\", \"white plate\", \"white bowl\"]\n",
    "yolo_predictions = yolo.infer(im=img, candidate_labels=labels, detection_threshold=0.15)\n",
    "bboxes = get_bboxes(yolo_predictions)\n",
    "img_annotated = annotator.annotate(im=img, detection_predictions=yolo_predictions)\n",
    "Image.fromarray(img_annotated)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, we have good labels to detect our objects. \n",
    "\n",
    "\n",
    "##### **Detection threshold & Frequency**\n",
    "\n",
    "Now that we have our labels, we can see that we also have the prediction scores on the image : that will help you set the detection threshold when you instanciate your Perception object. Here, we decide to set it to 0.2, because the oranges are detected with a low score. \n",
    "\n",
    "The frequency depends on how well your objects are detected (if they're not well detected, a higher frequency will increase the number of frames over which the model attempts detection). Here, we are going to set it to 40 because the oranges aren't detected often. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 4.2.3. Instanciation\n",
    "\n",
    "Now we have all the elements to instanciate the Perception object : the CameraWrapper, the T_reachy_cam, the frequency of detection and the detection threshold."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from pollen_vision.perception import Perception\n",
    "\n",
    "perception = Perception(r_cam, T_reachy_cam, freq=40, yolo_thres=0.2)\n",
    "\n",
    "print(\"The Perception object is created.\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And now, we can start the objects detection and the tracking: we start by setting the tracked objects, then we launch it. \n",
    "\n",
    "You can display the image from the camera continuously, together with the objects detected, their bounding boxes and their masks, the prediction scores and their centroids, thanks to the \"visualize=True\"."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "left_obj_target, right_obj_target = \"white plate\", \"white bowl\"\n",
    "obj_to_left, obj_to_right = \"orange ball\", \"apple\"\n",
    "\n",
    "perception.set_tracked_objects([obj_to_left, obj_to_right, left_obj_target, right_obj_target])\n",
    "perception.start(visualize=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You have now a window with the camera view and the detected objects. The Perception part is now on.\n",
    "\n",
    "> *If you didn't get the new window, the notebook crashed unfortunately. Please restart your kernel and execute again the cells above.*"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4.3. Set Reachy ready"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We want Reachy to be in front of the table where there are the fruits, in a position that allows it to access the table without obstructing its camera's view. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Reachy has its motors off by default. So first, we need to turn them on. \n",
    "\n",
    "> Put Reachy in a safe environment, with no one in reachable space and no obstacles, and with someone who has the emergency stop button nearby. \n",
    "\n",
    "If you have followed the set-up correctly, Reachy is normally positioned facing the table, 20 cm backwards. So now, we turn it on and put it on its default pose (head and arms straight)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.turn_on()\n",
    "reachy.goto_posture('default')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, we can define the waiting pose we want it to take before starting grasping the fruits. \n",
    "\n",
    "For that, we will define a function to set the joint values of every part of the robot. \n",
    "\n",
    "Here, we want the arms to be parallel to the ground, with the elbows back and slightly apart, grippers open. We want the head to look at its working area, pointing towards the central point 30cm in front of its end-effectors. We let the possibility to set the duration of the moves directly in the parameters of the function (by default, it's going to take 2 seconds). \n",
    "\n",
    "> You can set your own pose according to your environment, but be aware that there is no limitation to avoid collision between parts of the robot (right arm against left arm, arm against forearm, arm against torso, etc.). So it's your responsability to try the pose on a virtual set-up first or using the forward_kinematics() when you place the body parts where you will want them to be (the robot needs to be compliant for that : you'll have to turn it off).\n",
    "\n",
    "For this tutorial, we will always define first the function, then execute it :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import time \n",
    "\n",
    "def get_to_waiting_pose(reachy: ReachySDK, duration: float = 2) -> None :\n",
    "    r_arm_move = reachy.r_arm.goto([30,10,-15,-115,0,0,-15], duration)\n",
    "    l_arm_move = reachy.l_arm.goto([30,-10,15,-115,0,0,15], duration)\n",
    "\n",
    "    reachy.r_arm.gripper.open()\n",
    "    reachy.l_arm.gripper.open()\n",
    "\n",
    "    waiting_position = reachy.r_arm.forward_kinematics([30,10,-15,-115,0,0,-15])[:3,3]\n",
    "    head_move = reachy.head.look_at(waiting_position[0]+0.3, 0, waiting_position[2], duration, wait = True)\n",
    "\n",
    "    print(\"Reachy in waiting pose\")\n",
    "\n",
    "print(\"Function get_to_waiting_pose defined.\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Make the robot move now : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "get_to_waiting_pose(reachy, duration = 3) #each move will take 3 seconds to be done, all in the same time. \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we are going to move Reachy forward to the edge of the table, by controlling the mobile base. We can either :\n",
    "-  **use the *goto* method**: we ask it to move 0.2m forward from its reference frame. \n",
    "\n",
    "Be aware that this frame is set at the start-up of the robot (so when you press the start button on the mobile base station), and not when you connect to the robot. Be careful, the reference frame doesn't update, unless you call yourself the function *reachy.mobile.reset_odometry()*. That means that if you want to move again 0.2m forward after the first move, you will have to set the command with x = 0.2 + 0.2 = 0.4m. So, to be sure the reference frame is well set, we can call the reset_odometry before sending the command. \n",
    "\n",
    "- **use the *translate_by* method** : we ask it to move 0.2m forward from its actual position. \n",
    "\n",
    "This second method seems safer in our case, so you can make the robot move now : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.mobile_base.translate_by(x= 0.2, y=0.0, wait=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If Reachy is not close enough to the table, you can adjust yourself by calling again the *translate_by* method, or turn it off (<code>reachy.mobile_base.turn_off()</code>) and move the robot yourself to the right place. \n",
    "\n",
    "Now, it's set for its grasping task. Let's see how we can deal with it !"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4.4. Construct our program\n",
    "\n",
    "We need to construct the program that will make Reachy sort the fruits. \n",
    "\n",
    "For that, we need it to:  \n",
    "1.    Detect the specific objects\n",
    "3.    Determine the closest object to the corresponding effector (left one for the oranges, right one for the apples)\n",
    "4.    Make the corresponding arm move to the closest fruit et grasp it\n",
    "5.    Make it go above the target object (the plate for the oranges, the bowl for the apples) and drop it\n",
    "6.    Go back to its waiting pose. \n",
    "\n",
    "So, let's do that step by step !"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 4.4.1. Detect the specific objects\n",
    "\n",
    "We need to use the Perception module to detect the oranges, the apples, the plate and the bowl, and to extract pertinent informations.\n",
    "\n",
    "if you have followed the set-up correctly, you should have a white plate and oranges on Reachy's left side and a white bowl and apples on its right side, like in the photos below. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p align=\"center\"> \n",
    "    <img src=\"images/Reachy_fruit_face.jpg\" alt=\"Image face\" style=\"width: 20%; ; margin-right: 10px;\"/>\n",
    "    <img src=\"images/Reachy_fruits_above.jpg\" alt=\"Image above\" style=\"width: 20%;\"/>\n",
    "</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Check on the camera view displayer that all your objects are detected and not hidden by the arms of the robot. If so, move them a bit. Fruits need to be standing up to be well detected. \n",
    "\n",
    "\n",
    "Now, you can use the function *get_objects_infos()* of Perception, to get all the informations of the extracted objects who have a prediction score above the detection threshold you set."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "detection_threshold = 0.2\n",
    "detected_objects = perception.get_objects_infos(detection_threshold)\n",
    "print(detected_objects)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see, it returns a list of unsorted dictionnaries representing each object detected among all the researched objects, with different informations, as the label, the score prediction, the bounding box, the mask... "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So we need to differenciate each object, depending on its type. We will define a function to do that. \n",
    "\n",
    "We give as parameters the Perception instanciation, the type of fruits to be placed to the left side, to the right side, the left target object, the right target object and the detection threshold. And it will return the list of dictionnaries of oranges, of apples, and the dictionnaries of the white plate and the white bowl. \n",
    "\n",
    "To avoid errors, we make sure to detect the two target objects to continue : if there are not detected, we reduce the detection threshold until a minimum threshold. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from typing import Tuple, Union, List, Dict, Optional\n",
    "\n",
    "def get_selected_objects(\n",
    "    perception: Perception, \n",
    "    obj_to_left: str, \n",
    "    obj_to_right: str, \n",
    "    left_obj_target: str, \n",
    "    right_obj_target: str, \n",
    "    detection_threshold: float\n",
    ") -> Tuple[List[dict], List[dict], dict, dict]:\n",
    "    \n",
    "    detected_objects = perception.get_objects_infos(detection_threshold)\n",
    "\n",
    "    # extracting the fruits to be drop to the left and to the right side\n",
    "    objects_to_left = [obj for obj in detected_objects if obj['name'] == obj_to_left]\n",
    "    objects_to_right = [obj for obj in detected_objects if obj['name'] == obj_to_right]\n",
    "\n",
    "    # Reduction of the detection threshold while the containers aren't detected until a minimum threshold is reached\n",
    "    min_detection_threshold = 0.1\n",
    "    while True : \n",
    "        left_target = [obj for obj in detected_objects if obj['name'] == left_obj_target]\n",
    "        right_target = [obj for obj in detected_objects if obj['name'] == right_obj_target]\n",
    "\n",
    "        if left_target != [] and right_target != [] :\n",
    "            break\n",
    "        \n",
    "        if detection_threshold > min_detection_threshold : \n",
    "            detection_threshold -= 0.01\n",
    "            print(f\"Targets not found, lowering threshold to {detection_threshold}\")\n",
    "        else : \n",
    "            print(\"Targets not found, new try with the minimal threshold\")\n",
    "        detected_objects = perception.get_objects_infos(detection_threshold)\n",
    "        time.sleep(1.5)\n",
    "        \n",
    "\n",
    "    print(f'{len(detected_objects)} objects detected including {len(objects_to_left)} {obj_to_left}s, {len(objects_to_right)} {obj_to_right}s, {len(left_target)} {left_obj_target} and {len(right_target)} {right_obj_target}.')\n",
    "    \n",
    "    return objects_to_left, objects_to_right, left_target[0], right_target[0]\n",
    "\n",
    "print(\"Function get_selected_objects defined.\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, you can execute this function to have the list of dictionnaries of each object type. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "oranges, apples, plate, bowl = get_selected_objects(perception, obj_to_left, obj_to_right, left_obj_target, right_obj_target, detection_threshold)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 4.4.2. Get the closest fruit to grasp\n",
    "\n",
    "Now we want to determine the fruit that requires the less effort from Reachy to grasp, meaning the fruit that is the closest to his corresponding effector (oranges will only be grasped by the left arm, and apples by the right arm). So we will define a function for that. \n",
    "\n",
    "We can first define a function to calculate distance between centroids of two objects defined by a dictionnary.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy.typing as npt\n",
    "import numpy as np\n",
    "\n",
    "def distance_between_objects(obj1: npt.NDArray[np.float64], obj2: npt.NDArray[np.float64]) -> float : \n",
    "    return np.linalg.norm(obj1[:3,3]-obj2[:3,3])\n",
    "\n",
    "dist = distance_between_objects(oranges[0]['pose'], plate['pose']) #returns the distance in meters between the first orange and the plate\n",
    "print(f'Distance between the first orange and the plate: {dist:.2f}m.')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then, we need to define functions to add some conditions for the object to be graspable : \n",
    "\n",
    "- it has not to be already in one of the target objects  : that's the goal of the *object_in_target()* function (it uses a threshold of distance between the centroids)\n",
    "\n",
    "- it has to be different from the last grasped object : that's the goal of the *is_a_new_object()* function, it avoids trying to grasp the same object a second time because of image capture latency and uses also a threshold of distance between centroids. \n",
    "\n",
    "- it has to be far enough from the robot to avoid collision with its torso : that's the goal of the *is_too_close* function."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def object_in_target(obj: Dict, target: Dict, threshold: float) -> bool:\n",
    "    return distance_between_objects(obj['pose'], target['pose']) < threshold\n",
    "\n",
    "\n",
    "def is_a_new_object(actual_obj: Dict, former_obj: Dict, threshold: float) -> bool:\n",
    "    if former_obj == []: \n",
    "        return True\n",
    "    else: \n",
    "        return distance_between_objects(actual_obj['pose'], former_obj['pose']) > threshold\n",
    "\n",
    "\n",
    "def is_too_close(obj: Dict, x_lim: float = 0.15, y_lim: float = 0.15) -> bool:\n",
    "    if obj['pose'][0,3] < x_lim and abs(obj['pose'][1,3]) < y_lim:\n",
    "        print(f\"Object {obj['name']} at {obj['pose'][:3,3]} is too close to the robot\")\n",
    "        return True\n",
    "    return False\n",
    "\n",
    "print(\"Functions object_in_target, is_a_new_object and is_too_close defined.\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, we can define our global function, that will test for each fruit the distance with its corresponding effector and the compliance to the conditions (done by an internal function). It will return the closest object as a dictionnary and the target side as a string. It will print the closest fruit and its position in Reachy's frame. If no object respects the conditions, it will return an empty list."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_closest_object(\n",
    "    reachy: ReachySDK,  \n",
    "    former_object : Dict, \n",
    "    obj_to_left: List[dict], \n",
    "    obj_to_right: List[dict], \n",
    "    left_target: Dict, \n",
    "    right_target: Dict, \n",
    "    dist_threshold : float\n",
    ") -> Tuple[Optional[Dict], Optional[str]]:\n",
    "\n",
    "    def find_closest(objects, effector_pos, side_name):\n",
    "        #the closest_object is an object that is not already in one of the targets, not too close to the robot and not the same as the former_object\n",
    "        nonlocal closest_object, min_dist, side\n",
    "        for obj in objects:\n",
    "            dist_with_effector = distance_between_objects(obj['pose'], effector_pos)\n",
    "            if (dist_with_effector < min_dist and \n",
    "                not object_in_target(obj, left_target, dist_threshold) and\n",
    "                not object_in_target(obj, right_target, dist_threshold) and \n",
    "                not is_too_close(obj) and \n",
    "                is_a_new_object(obj, former_object, fruit_radius)):\n",
    "                \n",
    "                closest_object = obj\n",
    "                min_dist = dist_with_effector\n",
    "                side = side_name\n",
    "\n",
    "    closest_object, side = [], None\n",
    "    min_dist = np.inf\n",
    "    fruit_radius = 0.02\n",
    "\n",
    "    #Get position of the end effectors\n",
    "    position_l_effector = reachy.l_arm.forward_kinematics()\n",
    "    position_r_effector = reachy.r_arm.forward_kinematics()\n",
    "    \n",
    "    #if the containers are detected, we look for the closest object with its relative effector\n",
    "    if left_target : \n",
    "        find_closest(obj_to_left, position_l_effector, 'left')\n",
    "\n",
    "    if right_target:\n",
    "        find_closest(obj_to_right, position_r_effector, 'right')\n",
    "\n",
    "    if closest_object:\n",
    "        print(f\"Closest fruit is {closest_object['name']} at the coordinates {closest_object['pose'][:3,3]}\")\n",
    "    else: \n",
    "        print(\"No fruit detected in the workspace\")\n",
    "\n",
    "    return closest_object, side\n",
    "\n",
    "print(\"Function get_closest_object defined.\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now it's your turn to determine which fruit is the closest : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "former_object = []\n",
    "closest_object, target_side = get_closest_object(reachy, former_object, oranges, apples, plate, bowl, 0.15)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 4.4.3. Grasp the fruit\n",
    "\n",
    "Now that we know which is the closest fruit, we need to make Reachy catch it. For that, we have to follow some steps : "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First, we need to determine the grasp pose for the corresponding effector from the fruit centroid.\n",
    "\n",
    "- For the position, we use offsets for each axis for the object to be in the middle of the gripper (the origin of the effector frame is located before the gripper)\n",
    "\n",
    "- For the rotation :\n",
    "    - we want the effector to be parallel to the ground, so the rotation in y axis is -90° (0 is with effector pointing to the ground).\n",
    "    - we want the effector to orient towards the object. For that, we use the bent_position, meaning the position of the effectors when Reachy has its elbows facing torso, bent at 90°. We will use the angle between the line formed by the forearm in the bent_position and the line between the robot's elbow when facing the torso, and the object to be reached. \n",
    "\n",
    "    <p align=\"center\"><img src=\"images/angle.jpg\" alt=\"Image angle\" style=\"width: 20%; ; margin-right: 10px;\"/></p>\n",
    "\n",
    "This function needs to get the dictionnary of the selected object and the target side, to return the matrix 4x4 of the grasp pose. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from reachy2_sdk.utils.utils import get_pose_matrix\n",
    "\n",
    "def get_goal_pose(object_pose_dict: Dict, target_side: str) -> npt.NDArray[np.float64]: \n",
    "    #we define the effectors position when the robot has its forearms parallel to the ground\n",
    "    bent_position = np.array([0.38622, 0.22321, -0.27036]) if target_side == 'left' else np.array([0.38622, -0.22321, -0.27036])\n",
    "    \n",
    "    object_pose = np.copy(object_pose_dict['pose'])\n",
    "    \n",
    "    # for the goal position to be in the gripper center\n",
    "    object_pose [0,3] -= 0.04\n",
    "    object_pose [1,3] = object_pose [1,3] + 0.01 if target_side == 'left' else object_pose [1,3] - 0.01\n",
    "    object_pose [2,3] -= 0.04\n",
    "\n",
    "    #set the orientation of the effector to reach the object\n",
    "    dy = object_pose[1,3] - bent_position[1]\n",
    "    dx = object_pose[0,3]\n",
    "\n",
    "    angle_x_rad = np.arctan2(dx, dy)\n",
    "    angle_x = 90 - np.degrees(angle_x_rad)\n",
    "\n",
    "    target_pose = get_pose_matrix(object_pose[:3,3], [angle_x, -90, 0])\n",
    "\n",
    "    print(f\"goal position {target_pose[:3,3]}, goal rotation {[angle_x, -90, 0]}\\n\")\n",
    "    \n",
    "    return target_pose\n",
    "\n",
    "print(\"Function get_goal_pose defined.\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We are going to try it on our closest fruit to get the grasp pose needed to catch it : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "grasp_pose = get_goal_pose(closest_object, target_side)\n",
    "print(f'The grasp pose is  :\\n {grasp_pose}')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.r_arm.forward_kinematics()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "But if you try to go directly from the waiting pose to the grasp pose, you'll see that sometimes the trajectory may bump into the object, and the movement may need to pass through an intermediate point to reach the fruit optimally. \n",
    "\n",
    "Here, we are going to add a pregrasp pose, meaning a pose that is close to the fruit, to make the next movement easier and more precise to achieve its goal. \n",
    "We set it a few centimeters before the fruit, and we define new function to calculate this pose, from the grasping pose. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from scipy.spatial.transform import Rotation as R\n",
    "\n",
    "def get_pregrasping_pose(goal_pose: npt.NDArray[np.float64], target_side : str) -> None:\n",
    "    bent_position = np.array([0.38622, 0.22321, -0.27036]) if target_side == 'left' else np.array([0.38622, -0.22321, -0.27036])\n",
    "    pregrasp_pose = goal_pose.copy()\n",
    "\n",
    "    #get the x orientation of the target pose\n",
    "    rotation = R.from_matrix(goal_pose[:3,:3]).as_euler('xyz', degrees=False)[0]\n",
    "    \n",
    "    #pregrasp pose is 8cm before the goal pose\n",
    "    pregrasp_pose[0,3] -= 0.08\n",
    "    # y value is calculated from the new x value and the effector orientation needed to grasp the object\n",
    "    pregrasp_pose[1,3] = pregrasp_pose[0,3] * np.tan(rotation) + bent_position[1]\n",
    "\n",
    "    return pregrasp_pose\n",
    "\n",
    "print(\"Function get_pregrasping_pose defined.\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We calculate the pregrasp pose for our closest fruit : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pregrasp_pose = get_pregrasping_pose(grasp_pose, target_side)\n",
    "print(f'The pregrasp pose is  : {pregrasp_pose}')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, we have our grasp pose and an intermediate pose to reach the object. So we can make Reachy move !\n",
    "\n",
    "To do that, we will convert the end effector poses into joints values, using the SDK method *inverse_kinematics* : we will get the 7 joint values of the arm needed to reach this pose. This way, we are gonna be sure that the pose is reachable by the arm, and then, we'll be able to give those commands to the robot. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#we set the controled arm to the left or right arm depending on the target side\n",
    "arm = reachy.l_arm if target_side == 'left' else reachy.r_arm\n",
    "\n",
    "joints_to_pregrasp = arm.inverse_kinematics(pregrasp_pose)\n",
    "joints_to_grasp = arm.inverse_kinematics(grasp_pose)\n",
    "\n",
    "print(f\"Joints to pregrasp : {joints_to_pregrasp}\\n Joints to grasp : {joints_to_grasp}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you have an error on this command, thats means that one of the poses is not reachable by Reachy, so you can move the object on the table and retry by starting back there : \n",
    "> Execute the cell below only if you had an error above and you moved the fruits. (*select all the cell and press Ctrl + K + U*)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# oranges, apples, plate, bowl = get_selected_objects(perception, obj_to_left, obj_to_right, left_obj_target, right_obj_target, detection_threshold)\n",
    "# closest_object, target_side = get_closest_object(reachy, former_object, oranges, apples, plate, bowl, 0.15)\n",
    "# grasp_pose = get_goal_pose(closest_object, target_side)\n",
    "# pregrasp_pose = get_pregrasping_pose(grasp_pose, target_side)\n",
    "\n",
    "# arm = reachy.l_arm if target_side == 'left' else reachy.r_arm\n",
    "# joints_to_pregrasp = arm.inverse_kinematics(pregrasp_pose)\n",
    "# joints_to_grasp = arm.inverse_kinematics(grasp_pose)\n",
    "# print(f\"Joints to pregrasp : {joints_to_pregrasp}\\n Joints to grasp : {joints_to_grasp}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then, we give Reachy the command, thanks to the SDK method *goto()*: \n",
    "- first, it will go the pregrasp pose\n",
    "- then, to the grasp pose\n",
    "\n",
    "Make the robot move now : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "move_to_pregrasp_id = arm.goto(joints_to_pregrasp, duration = 2)\n",
    "move_to_grasp_id = arm.goto(joints_to_grasp, duration = 2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now that Reachy got its gripper around the object, we can close it to grasp the fruit. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "arm.gripper.close()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then, we can raise the arm, using the SDK method translate_by() : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "arm.translate_by(0,0,0.1, duration = 1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Perfect, we made it ! \n",
    "\n",
    "But as you can see, Reachy seems a bit sad, keeping its head still. We will add a head movement for Reachy to look at the fruit it wants to catch. \n",
    "We use the method *forward_kinematics()* to get the pose of the effector in Reachy's frame, and we make the head look at its position. \n",
    "\n",
    "Make the robot move now : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "actual_pose = arm.forward_kinematics()\n",
    "reachy.head.look_at(actual_pose[0,3], actual_pose[1,3], actual_pose[2,3], 2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "That's much better, don't you think ? \n",
    "\n",
    "We are going to define a function for it, to execute it easily, just giving it the ReachySDK and the object we want it to look at as parameters. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def move_head(reachy: ReachySDK, target_obj: Dict) -> None:\n",
    "    target_pose = target_obj['pose']\n",
    "    reachy.head.look_at(target_pose[0,3], target_pose[1,3], target_pose[2,3], 2)\n",
    "\n",
    "print(\"Function move_head defined.\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So now we have all the building blocks we need to construct a function to link this entire sequence together.\n",
    "\n",
    "Some adjustements have been made for all the moves to work finely together : \n",
    "- on the grasping movement, we use a blocking parameter (*wait=True*), for the gripper to close only when the effector is at the right place\n",
    "- we wait until the gripper finished to close before raising the arm\n",
    "- we use a blocking parameter also on the last translation for the next moves to wait for the end of all this sequence. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def move_to_grasp(reachy: ReachySDK, obj_to_catch: Dict, target_side: str) -> None:\n",
    "    print(\"Move sequence to grasp : started.\")\n",
    "\n",
    "    move_head(reachy, obj_to_catch)\n",
    "\n",
    "    grasp_pose = get_goal_pose(obj_to_catch, target_side)\n",
    "    pregrasp_pose = get_pregrasping_pose(grasp_pose, target_side)\n",
    "    \n",
    "    arm = reachy.l_arm if target_side == 'left' else reachy.r_arm\n",
    "    joints_to_pregrasp = arm.inverse_kinematics(pregrasp_pose)\n",
    "    joints_to_grasp = arm.inverse_kinematics(grasp_pose)\n",
    "\n",
    "    move_to_pregrasp_id = arm.goto(joints_to_pregrasp)\n",
    "    move_to_grasp_id = arm.goto(joints_to_grasp, wait=True)\n",
    "        \n",
    "    #we wait for the gripper to end closing before raising the arm\n",
    "    arm.gripper.close()\n",
    "    while arm.gripper.is_moving():\n",
    "        time.sleep(0.5)\n",
    "    arm.translate_by(x=0, y=0, z=0.1, duration = 1)\n",
    "    time.sleep(1)\n",
    "\n",
    "    print(\"Move sequence to grasp : done.\")\n",
    "\n",
    "print(\"Function move_to_grasp defined.\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To try it, we are gonna first drop the fruit and go back to waiting position.\n",
    "\n",
    "Make the robot move now : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "translation_move = arm.translate_by(0,0,-0.1, duration = 1)\n",
    "time.sleep(1)\n",
    "arm.gripper.open()\n",
    "while arm.gripper.is_moving():\n",
    "    time.sleep(0.1)\n",
    "get_to_waiting_pose(reachy, duration = 3)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now with only 3 functions, we are going to : \n",
    "1. do a new object detection\n",
    "2. select the new closest object \n",
    "3. make Reachy do the move sequence to grasp it. \n",
    "\n",
    "Make the robot move now : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "oranges, apples, plate, bowl = get_selected_objects(perception, obj_to_left, obj_to_right, left_obj_target, right_obj_target, detection_threshold)\n",
    "closest_object, target_side = get_closest_object(reachy, former_object, oranges, apples, plate, bowl, 0.15)\n",
    "move_to_grasp(reachy, closest_object, target_side)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Well done ! Now let's continue to the drop sequence. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 4.4.4. Drop the fruit\n",
    "\n",
    "We are going to build a sequence similar to the previous one to drop the grasped fruit in its target object. Hopefully, we defined a lot of functions that will be helpful in this part too !\n",
    "\n",
    "The first step is to determine the drop pose. To do that, we are gonna use the centroid of the target object, that we determined thanks to the target_side of the closest object. \n",
    "\n",
    "As we want it to drop the fruit on the object, and not catch the object, we are going to virtually increase the z value of the centroid, for the effector to get on top of the object and to avoid any collision. Here, we add 15 cm. \n",
    "> Of course, it will depend on the type of object you're using (something rather flat or hollow), you can adjust the value it yourself after testing it. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# set the arm and the target object depending on the side of the target\n",
    "arm = reachy.l_arm if target_side == 'left' else reachy.r_arm\n",
    "target_obj = plate.copy() if target_side == 'left' else bowl.copy()\n",
    "\n",
    "#raise virtually the target object by 15cm\n",
    "target_obj['pose'][2,3] += 0.15 \n",
    "\n",
    "print(f\"{target_obj['name']} at {target_obj['pose'][:3,3]}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We make Reachy look at its new target, with the function *move_head* that we defined ealier. \n",
    "\n",
    "Make it move now : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "move_head(reachy, target_obj)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then, we can use the function *get_goal_pose* we defined earlier to get the drop pose as a 4x4 matrix : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "drop_pose = get_goal_pose(target_obj, target_side)\n",
    "print(f'The drop pose is  : {drop_pose}')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can get the joints value needed to reach this pose using the inverse kinematics, as we saw before. That will also check if that pose is reachable for Reachy."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "joints_to_drop = arm.inverse_kinematics(drop_pose)\n",
    "print(f\"Joints to drop pose : {joints_to_drop}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you have an error on the cell above, that means that your target object is unreachable for Reachy's arm. If so, you can try moving the objet and execute the cell below until you don't have any more error : \n",
    "\n",
    "> Execute the cell below only if you had an error on the cell above and you did move the target object. (*select all the cell and press Ctrl + K + U*)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# oranges, apples, plate, bowl = get_selected_objects(perception, obj_to_left, obj_to_right, left_obj_target, right_obj_target, detection_threshold)\n",
    "# target_obj = plate.copy() if target_side == 'left' else bowl.copy()\n",
    "# target_obj['pose'][2,3] += 0.15 \n",
    "# drop_pose = get_goal_pose(target_obj, target_side)\n",
    "# joints_to_drop = arm.inverse_kinematics(drop_pose)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, you can send the command to the robot and make it move : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "arm.goto(joints_to_drop)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The effector is now on top of the object, but maybe a bit too high : we can translate it a bit closer.\n",
    "> In the same way, you can adjust the translation distance to suit your environment.\n",
    "\n",
    "Make it move now : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "arm.translate_by(0,0,-0.03, duration = 1)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we can set the object free, by opening the gripper !"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "arm.gripper.open()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We raise the arm a little to avoid collisions : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "arm.translate_by(x=0, y=0, z=0.05, duration = 1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And we get back to the waiting pose :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "get_to_waiting_pose(reachy)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And that's it, well done !\n",
    "\n",
    "Now we can put it all together in a function. \n",
    "Some adjustments have been made for all the moves to work finely together : \n",
    "- on the dropping movement, we use a blocking parameter (*wait=True*), for the gripper to open only when the effector is at the right place\n",
    "- we wait until the gripper finished to open before raising the arm\n",
    "- we use a blocking parameter also on the last translation for the next moves to wait for the end of all this sequence. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def move_to_drop(reachy: ReachySDK, target_side: str) -> None:\n",
    "    print(\"Move sequence to drop : started.\")\n",
    "    \n",
    "    arm = reachy.l_arm if target_side == 'left' else reachy.r_arm\n",
    "    target_obj = plate.copy() if target_side == 'left' else bowl.copy()\n",
    "    \n",
    "    target_obj['pose'][2,3] += 0.15\n",
    "\n",
    "    move_head(reachy, target_obj)\n",
    "    drop_pose = get_goal_pose(target_obj, target_side)\n",
    "    joints_to_drop = arm.inverse_kinematics(drop_pose)\n",
    "    move_to_predrop_id = arm.goto(joints_to_drop)\n",
    "    move_to_drop_id = arm.translate_by(0,0,-0.03, duration = 1, wait = True)\n",
    "\n",
    "    arm.gripper.open()\n",
    "    while arm.gripper.is_moving():\n",
    "        time.sleep(0.1)\n",
    "    arm.translate_by(0,0,0.05, duration = 1, wait = True)\n",
    "\n",
    "    print(\"Move sequence to drop : done\")\n",
    "\n",
    "print(\"Function move_to_drop defined.\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's try it ! Place the fruit on the table in front of Reachy. And we'll start from the beginning.\n",
    "Reachy is going to detect the objects, select the closest one, grasp it and drop it in its target object. \n",
    "\n",
    "Make it move now : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "oranges, apples, plate, bowl = get_selected_objects(perception, obj_to_left, obj_to_right, left_obj_target, right_obj_target, detection_threshold)\n",
    "closest_object, target_side = get_closest_object(reachy, former_object, oranges, apples, plate, bowl, 0.15)\n",
    "move_to_grasp(reachy, closest_object, target_side)\n",
    "move_to_drop(reachy, target_side)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 4.4.5. Get back to the waiting position\n",
    "\n",
    "We only have to use our function : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "get_to_waiting_pose(reachy, duration = 3)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4.5. Make it work autonomously\n",
    "\n",
    "We have all our functions working. Now we want Reachy to do all of this sequence autonomously, and sort fruits until there are none left. \n",
    "For that, we need to construct a loop and that requires to deal first with the unreachability errors that can happen. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 4.5.1. Manage reachability errors\n",
    "\n",
    "If a fruit or a target object is out of reach for Reachy's arm, there will be an error in the code when you call the inverse_kinematics() with the pose matrix of the object. To deal with that, we are going to add a handling of the exceptions.\n",
    "When an error raises, we want Reachy to notify us that he can't go further. \n",
    "\n",
    "For that, we are gonna add a <code>try/except</code> in the loop and define a function for Reachy to make no from head. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def make_no_from_head(reachy : ReachySDK)-> None:\n",
    "    print(\"Making a no from the head\")\n",
    "    reachy.head.look_at(x=0.5, y=0.15, z=0.15, duration=1)\n",
    "    reachy.head.look_at(x=0.5, y=-0.15, z=0.15, duration=1)\n",
    "    reachy.head.look_at(x=0.5, y=0.15, z=0.15, duration=1)\n",
    "    reachy.head.look_at(x=0.5, y=0, z=-0.25, duration=1, wait=True)\n",
    "\n",
    "print(\"Function make_no_from_head defined.\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can try it on Reachy : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "make_no_from_head(reachy)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When Reachy can't grasp the fruit, it will make no from head and that's it.\n",
    "\n",
    "But when he can't reach the target object, it will already have the object in the gripper, so it also needs to put it back on the table. So we are going to define a function to organize the sequence :\n",
    "- make no from head\n",
    "- lower the effector \n",
    "- open the gripper\n",
    "- go back to the waiting pose. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def target_object_unreachable(reachy: ReachySDK, target_side: str) -> None:\n",
    "    make_no_from_head(reachy)\n",
    "    arm = reachy.l_arm if target_side == 'left' else reachy.r_arm\n",
    "    arm.translate_by(0,0,-0.1, wait = True)\n",
    "    arm.gripper.open()\n",
    "    if arm.gripper.is_moving() : \n",
    "        time.sleep(0.1)\n",
    "    get_to_waiting_pose(reachy)\n",
    "\n",
    "print(\"Function target_object_unreachable defined.\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 4.5.2. Make the loop\n",
    "\n",
    "So now, we can build the autonomous program !"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's put all together the different functions we define earlier, to make it work autonomously. \n",
    "\n",
    "Until you press the 'interrupt' button, the program will : \n",
    "- detect the objects, with the detection threshold you set (*be careful, it waits to detect the two target objects before moving*)\n",
    "- select the closest fruit if there is still fruit to grasp\n",
    "- move to grasp pose, unless it's not reachable (Reachy will make no from head and go back to the loop beginning)\n",
    "- move to drop pose, unless it's not reachable (it will drop back the fruit and make no from head before getting back to its waiting position)\n",
    "- get back to its waiting position and do it again\n",
    "\n",
    "If there is no fruit left, the program will wait until you interrupt it. \n",
    "\n",
    "\n",
    "**So you can try different things :**\n",
    "> - give a fruit to the robot and wait for it to grasp it. \n",
    "> - put a fruit far from the robot,\n",
    "> - put a fruit in a reachable place but move the target object far from the robot."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "closest_object = []\n",
    "detection_threshold = 0.2\n",
    "#definition of the target object radius, used for the threshold of object_in_target function\n",
    "target_object_radius = 0.12\n",
    "\n",
    "try:\n",
    "    while True:\n",
    "        former_object = closest_object\n",
    "        oranges, apples, plate, bowl = get_selected_objects(perception, obj_to_left, obj_to_right, left_obj_target, right_obj_target, detection_threshold)\n",
    "\n",
    "        #get the closest object to its corresponding end-effector \n",
    "        closest_object, target_side = get_closest_object(reachy, former_object, oranges, apples, plate, bowl, target_object_radius)\n",
    "        print('closest object', closest_object['name'] if closest_object else None)\n",
    "        \n",
    "        if closest_object : \n",
    "            print(f\"target_side : {target_side} for {closest_object['name']}\")\n",
    "\n",
    "            try : \n",
    "                move_to_grasp(reachy, closest_object, target_side)\n",
    "\n",
    "            except ValueError as e: \n",
    "                print(\"Grasping poses not reachable.\")\n",
    "                make_no_from_head(reachy)\n",
    "                time.sleep(3)\n",
    "                continue\n",
    "\n",
    "\n",
    "            try :\n",
    "                move_to_drop(reachy, target_side)\n",
    "                get_to_waiting_pose(reachy)\n",
    "            \n",
    "            except ValueError as e: \n",
    "                print(\"Dropping poses not reachable.\")\n",
    "                target_object_unreachable(reachy,target_side)\n",
    "                time.sleep(3)\n",
    "        \n",
    "        else : \n",
    "            print(\"No new object to grasp, waiting for a new one.\")\n",
    "            time.sleep(3)\n",
    "\n",
    "except KeyboardInterrupt:\n",
    "    pass\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once you have finished detecting fruits, you can stop the loop by pressing the \"interrupt\" button and press \"q\" on the annotated image.\n",
    "\n",
    "Then we are going to make Reachy go step back from the table and going back to a standard pose, to be able to switch off safely.\n",
    "\n",
    "Make it move now : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.mobile_base.translate_by(-0.2,0, wait=True)\n",
    "time.sleep(1)\n",
    "reachy.goto_posture('default', duration = 2, wait = True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then we turn Reachy off smoothly and disconnect from it. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.turn_off()\n",
    "reachy.disconnect()\n",
    "print(\"Reachy is disconnected.\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Well done, you did it, you have trained a Reachy greengrocer !"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 5. Final tips\n",
    "\n",
    "Now, you've learned how to build a sequence on Reachy using the SDK Client and Pollen-Vision : \n",
    "- object detection\n",
    "- synchronization between Reachy's parts\n",
    "- grasping movements\n",
    "- combination of perception and movement\n",
    "\n",
    "\n",
    "You can now use this sequence as a starting point to create other complex behaviors on the robot. Feel free to modify the movements, the duration of the movements, the order of the movements, etc. to get more familiar with the SDK, the robot and to check whether you can make Reachy do what you want it to do ! \n",
    "\n",
    "Here are some general tips to keep in mind to be the best at implementing complex behaviors on Reachy:\n",
    "\n",
    "- **Always test behavior** on a fake robot before running it on the real robot! This will help you check if the behavior is doing what you expect it to do and to avoid any potential damage to the robot.\n",
    "- If you are working on the real robot, make sure that it has enough space around it to move its arms and head, and **always** have the emergency shutdown nearby. Especially, make sure that the arms will not be blocked by objects such as a table as there are no safety yet preventing the robot for colliding with its environment.\n",
    "- Split the behavior you wish to develop into smaller parts and implement them one by one. Once each part is working, you can combine them to create the full sequence. Go slow and test each part before moving on to the next one. \n",
    "\n",
    "\n",
    "## 6. Skip to the next tutorial ! \n",
    "\n",
    "Here, we've covered just a few of the methods that can be used on Reachy. To discover more ways of controlling the robot, don't hesitate to continue following the tutorials ! \n",
    "\n",
    "1. Reachy's awakening (with SDK only)\n",
    "\n",
    "2. Reachy the mime (with SDK only)\n",
    "\n",
    "**3. Reachy the greengrocer (with SDK & Pollen-Vision)**\n",
    "\n",
    "\n",
    "Keep up and you'll be soon an expert to control Reachy ! "
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "reachy10",
   "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.10.14"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}