applications/Reachy and vision.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Simple visual servoing example "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This notebook demonstrates how to setup a simple visual servoing demo where Reachy's arm follow the position of a yellow ball. As to keep the servoing simple, we only use relative positions and movement."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "\n",
    "import cv2\n",
    "import matplotlib.pyplot as plt"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First load Leachy as usual."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from reachy import Leachy\n",
    "\n",
    "leachy = Leachy(brunel_hand='/dev/tty.usbmodem14141')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Move it to the base position"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for m in leachy.motors:\n",
    "    m.compliant = False\n",
    "\n",
    "leachy.l_elbow_pitch.goto_position(-90, 2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We now define a very simple ball tracking algorithm using OpenCV. It's filter on a specific RGB range and find the center of the detected area."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cap = cv2.VideoCapture(0)\n",
    "\n",
    "def track_ball(img):\n",
    "    yellow_lower = (0, 125, 150)\n",
    "    yellow_upper = (100, 255, 255)\n",
    "    \n",
    "    mask = cv2.inRange(img, yellow_lower, yellow_upper)\n",
    "    mask = cv2.erode(mask, None, iterations=2)\n",
    "    mask = cv2.dilate(mask, None, iterations=2)\n",
    "\n",
    "    M = cv2.moments(mask)\n",
    "    center = (int(M[\"m10\"] / M[\"m00\"]), int(M[\"m01\"] / M[\"m00\"]))\n",
    "    \n",
    "    return center"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will use two motors to control the end effector on a plan.\n",
    "\n",
    "* l_arm_yaw will be used to move along the x axis\n",
    "* l_elbow_pitch will be used to move along the y axis\n",
    "\n",
    "We then find the x, y position of the ball, normalized in [-1, 1], and apply it to the motor position."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "X_range, X_offset = 30, 0\n",
    "Y_range, Y_offset = 30, -90\n",
    "\n",
    "leachy.l_arm_yaw.moving_speed = 50\n",
    "leachy.l_elbow_pitch.moving_speed = 50\n",
    "\n",
    "import time\n",
    "\n",
    "while True:\n",
    "    _, img = cap.read()\n",
    "\n",
    "    x, y = track_ball(img)\n",
    "    x /= img.shape[1]\n",
    "    y /= img.shape[0]\n",
    "\n",
    "    x = x * 2 - 1\n",
    "    leachy.l_arm_yaw.goal_position = -x * X_range + X_offset\n",
    "    leachy.l_elbow_pitch.goal_position = y * Y_range + Y_offset \n",
    "\n",
    "    time.sleep(1 / 50)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once we are done, we clean everything."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cap.release()\n",
    "leachy.close()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}