{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "5f3c5bbc",
   "metadata": {},
   "source": [
    "# PreciseFlex PF400 and PF3400 robots\n",
    "\n",
    "Connection: ethernet"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "594d1a91",
   "metadata": {},
   "outputs": [],
   "source": [
    "%load_ext autoreload\n",
    "%autoreload 2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ba84cba7",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pylabrobot.arms.scara import ExperimentalSCARA\n",
    "from pylabrobot.arms.precise_flex.pf_400 import PreciseFlex400Backend\n",
    "\n",
    "from pylabrobot.arms.precise_flex.coords import PreciseFlexCartesianCoords\n",
    "from pylabrobot.arms.precise_flex.joints import PreciseFlexJointCoords\n",
    "from pylabrobot.resources import Coordinate, Rotation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e83c35da",
   "metadata": {},
   "outputs": [],
   "source": [
    "backend = PreciseFlex400Backend(host=\"192.168.0.1\", port=10100, has_rail=False)\n",
    "arm = ExperimentalSCARA(backend=backend)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1a9eb932",
   "metadata": {},
   "outputs": [],
   "source": [
    "await arm.setup(skip_home=False)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "eb3a5aae",
   "metadata": {},
   "source": [
    "## Granular Robot control"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7e0cc273",
   "metadata": {},
   "source": [
    "### Gripper Control"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3741f8e2",
   "metadata": {},
   "source": [
    "The gripper can be controlled manually as follows:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "451d51c0",
   "metadata": {},
   "outputs": [],
   "source": [
    "await arm.close_gripper(gripper_width=80)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ec9cd5f2",
   "metadata": {},
   "outputs": [],
   "source": [
    "await arm.open_gripper(gripper_width=120)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "89517ae8",
   "metadata": {},
   "outputs": [],
   "source": [
    "await backend.is_gripper_closed()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6518bc78",
   "metadata": {},
   "source": [
    "### Movement"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "60b8cede",
   "metadata": {},
   "source": [
    "You can also arbitrarily move the arm to cartesian coordinates as well as joint coordinates:"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "51ea5969",
   "metadata": {},
   "source": [
    "```{warning}\n",
    "Depending on the current position, moving to a joint position might actually cause the arm to collide with its base! Be careful when using joint coordinates.\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0ebba776",
   "metadata": {},
   "outputs": [],
   "source": [
    "location = PreciseFlexJointCoords(\n",
    "  base=99.981,\n",
    "  shoulder=-36.206,\n",
    "  elbow=83.063,\n",
    "  wrist=-331.7,\n",
    "  gripper=126.084,\n",
    "  rail=0.0\n",
    ")\n",
    "await arm.move_to(location)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3e81b5a4",
   "metadata": {},
   "outputs": [],
   "source": [
    "location = PreciseFlexCartesianCoords(\n",
    "  location=Coordinate(x=290, y=659, z=100),\n",
    "  rotation=Rotation(x=-180.0, y=90.0, z=84.804)\n",
    ")\n",
    "await arm.move_to(location)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1addb341",
   "metadata": {},
   "source": [
    "## Getting current location"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "022458da",
   "metadata": {},
   "source": [
    "Get the joint angles of the robot's arm, including the rails if applicable and the gripper width:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b515597a",
   "metadata": {},
   "outputs": [],
   "source": [
    "await arm.get_joint_position()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f254c14c",
   "metadata": {},
   "source": [
    "Get the cartesian coordinates of the robot's end effector:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9d72277a",
   "metadata": {},
   "outputs": [],
   "source": [
    "await arm.get_cartesian_position()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6f8c1b6c",
   "metadata": {},
   "source": [
    "## Teaching positions using free mode"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e534f0b0",
   "metadata": {},
   "source": [
    "Use \"free mode\" to manually move the robot's arm to a desired position, and then read the current cartesian coordinates. You can use the cartesian coordinates to programmatically move the arm to that position later."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "65e03f11",
   "metadata": {},
   "outputs": [],
   "source": [
    "await backend.activate_free_mode()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "64fd60cf",
   "metadata": {},
   "outputs": [],
   "source": [
    "await arm.get_cartesian_position()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1b64e546",
   "metadata": {},
   "outputs": [],
   "source": [
    "await backend.deactivate_free_mode()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6ef53233",
   "metadata": {},
   "source": [
    "## Plate Movement"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "af6424a0",
   "metadata": {},
   "source": [
    "Below is an example of picking up and placing a plate using cartesian coordinates. You can call `move_to` in between to move to other locations as needed."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "88ae00e3",
   "metadata": {},
   "outputs": [],
   "source": [
    "location = PreciseFlexCartesianCoords(\n",
    "  location=Coordinate(x=650.74, y=-345.922, z=5.05),\n",
    "  rotation=Rotation(x=180.0, y=90.0, z=-9.921)\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "18c1a704",
   "metadata": {},
   "outputs": [],
   "source": [
    "await arm.pick_up_resource(\n",
    "  location,\n",
    "  plate_width=125,\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bfeefd6b",
   "metadata": {},
   "outputs": [],
   "source": [
    "await arm.drop_resource(location)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "62036558",
   "metadata": {},
   "source": [
    "## Miscellaneous commands"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "80c96216",
   "metadata": {},
   "source": [
    "Move the arm to its predefined home/safe position:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c8e1e7c7",
   "metadata": {},
   "outputs": [],
   "source": [
    "await arm.move_to_safe()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3d07da5b",
   "metadata": {},
   "source": [
    "Home the arm:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "78ab2382",
   "metadata": {},
   "outputs": [],
   "source": [
    "await arm.home()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "076b738d",
   "metadata": {},
   "source": [
    "Stop any ongoing movement of the arm:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "13b4e525",
   "metadata": {},
   "outputs": [],
   "source": [
    "await arm.halt()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "env",
   "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.9.24"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}