apps/simple_joint_control.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'/root/.cache/bazel/_bazel_root/0b0fc44d1fc1b8c273ad4738eeb3ffe3/execroot/com_nvidia_isaac_sdk/bazel-out/k8-opt/bin/packages/universal_robots/apps/simple_joint_control.runfiles/com_nvidia_isaac_sdk'"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import os\n",
    "# set the current working directory to the deployed package folder. This is required by isaac.\n",
    "# This cell should only run once.\n",
    "os.chdir(\"../../../\")\n",
    "os.getcwd()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "from IPython.display import display\n",
    "import json\n",
    "import numpy as np\n",
    "import time\n",
    "\n",
    "from packages.pyalice import Application, Codelet, Message\n",
    "from packages.pyalice.gui.composite_widget import CompositeWidget\n",
    "\n",
    "np.set_printoptions(precision=3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Obtain the IP from UR Console and update this\n",
    "IP = \"192.168.56.101\"\n",
    "# Robot type that you are going to control (ur3, ur5, ur10, ur3e, ur5e, ur10e, ur16e)\n",
    "ROBOT_MODEL = \"ur5e\"\n",
    "\n",
    "# Set kinematic file and subgraph, based on robot model\n",
    "if ROBOT_MODEL == \"ur3\":\n",
    "    ur_subgraph = \"packages/universal_robots/ur_robot_driver/apps/ur3.subgraph.json\"\n",
    "    kinematic_file = \"packages/universal_robots/ur_robot_driver/config/ur3.kinematic.json\"\n",
    "elif ROBOT_MODEL == \"ur5\":\n",
    "    ur_subgraph = \"packages/universal_robots/ur_robot_driver/apps/ur5.subgraph.json\"\n",
    "    kinematic_file = \"packages/universal_robots/ur_robot_driver/config/ur5.kinematic.json\"\n",
    "elif ROBOT_MODEL == \"ur10\":\n",
    "    ur_subgraph = \"packages/universal_robots/ur_robot_driver/apps/ur10.subgraph.json\"\n",
    "    kinematic_file = \"packages/universal_robots/ur_robot_driver/config/ur10.kinematic.json\"\n",
    "elif ROBOT_MODEL == \"ur3e\":\n",
    "    ur_subgraph =\"packages/universal_robots/ur_robot_driver/apps/ur3e.subgraph.json\"\n",
    "    kinematic_file = \"packages/universal_robots/ur_robot_driver/config/ur3e.kinematic.json\"\n",
    "elif ROBOT_MODEL == \"ur5e\":\n",
    "    ur_subgraph = \"packages/universal_robots/ur_robot_driver/apps/ur5e.subgraph.json\"\n",
    "    kinematic_file = \"packages/universal_robots/ur_robot_driver/config/ur5e.kinematic.json\"\n",
    "elif ROBOT_MODEL == \"ur10e\":\n",
    "    ur_subgraph = \"packages/universal_robots/ur_robot_driver/apps/ur10e.subgraph.json\"\n",
    "    kinematic_file = \"packages/universal_robots/ur_robot_driver/config/ur10e.kinematic.json\"\n",
    "elif ROBOT_MODEL == \"ur16e\":\n",
    "    ur_subgraph =\"packages/universal_robots/ur_robot_driver/apps/ur16e.subgraph.json\"\n",
    "    kinematic_file = \"packages/universal_robots/ur_robot_driver/config/ur16e.kinematic.json\"\n",
    "else:\n",
    "    Exception(\"Unknown robot model\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']\n"
     ]
    }
   ],
   "source": [
    "# A Python codelet for joint control through widget\n",
    "class JointPositionControl(Codelet):\n",
    "    def start(self):\n",
    "        self.rx = self.isaac_proto_rx(\"CompositeProto\", \"state\")\n",
    "        self.tx = self.isaac_proto_tx(\"CompositeProto\", \"command\")\n",
    "        \n",
    "        joints = self.config.joints\n",
    "        limits = self.config.limits\n",
    "        measure = self.config.measure\n",
    "        self.stop_control = 3\n",
    "        self._widget = CompositeWidget(joints, measure, limits)\n",
    "        if self._widget is None:\n",
    "            report_failure(\"Cannot create valid widget\")\n",
    "            return\n",
    "        display(self._widget.panel)\n",
    "\n",
    "        self.tick_periodically(0.1)\n",
    "\n",
    "    def tick(self):\n",
    "        state_msg = self.rx.message\n",
    "        if state_msg is None:\n",
    "            return\n",
    "        self._widget.composite = state_msg\n",
    "\n",
    "        self.tx._msg = self._widget.composite\n",
    "        if self.tx._msg is not None:\n",
    "            self.tx.publish()\n",
    "            \n",
    "# Get list of joints\n",
    "joints = []\n",
    "with open(kinematic_file,'r') as fd:\n",
    "    kt = json.load(fd)\n",
    "    for link in kt['links']:\n",
    "        if 'motor' in link and link['motor']['type'] != 'constant':\n",
    "            joints.append(link['name'])\n",
    "print(joints)\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "UR10 Hardware Joint Control\n",
    "======"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "2021-12-21 13:30:49,944 DEBUG Binding PyCodelet command_generator/PyCodelet\n",
      "2021-12-21 13:30:49,948 DEBUG Launching isaac core\n",
      "2021-12-21 13:30:49,952 DEBUG Launching pycodelet threads\n",
      "2021-12-21 13:30:49,954 DEBUG Launching command_generator/PyCodelet\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "39a70766ceed4472b22563f41898d357",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "VBox(children=(HBox(children=(Label(value='shoulder_pan_joint', layout=Layout(width='150px')), FloatSlider(val…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "app = Application(name=\"simple_joint_control_ur_hardware\", modules=[\"sight\"])\n",
    "\n",
    "app.load(filename=ur_subgraph, prefix=\"ur\")\n",
    "\n",
    "# Load components for configuration\n",
    "ur_interface = app.nodes[\"ur.subgraph\"][\"interface\"]\n",
    "ur_controller = app.nodes[\"ur.controller\"][\"ScaledMultiJointController\"]\n",
    "ur_driver = app.nodes[\"ur.universal_robots\"][\"UniversalRobots\"]\n",
    "\n",
    "# configs\n",
    "ur_controller.config.control_mode = \"joint position\"\n",
    "ur_driver.config.control_mode = \"joint position\"\n",
    "ur_driver.config.robot_ip = IP\n",
    "ur_driver.config.headless_mode = False\n",
    "\n",
    "# add pycodelet for joint position control\n",
    "widget_node = app.add(\"command_generator\")\n",
    "joint_commander = widget_node.add(JointPositionControl)\n",
    "joint_commander.config.joints = joints\n",
    "joint_commander.config.limits = [[-2*np.pi, 2*np.pi]] * len(joints)\n",
    "joint_commander.config.measure = 'position'\n",
    "\n",
    "# edges\n",
    "app.connect(joint_commander, \"command\", ur_interface, \"joint_target\")\n",
    "app.connect(ur_interface, \"arm_state\", joint_commander, \"state\")\n",
    "\n",
    "# Enable sight\n",
    "widget = app.add(\"sight\").add(app.registry.isaac.sight.SightWidget, \"simple_joint_control\")\n",
    "widget.config.type = \"plot\"\n",
    "widget.config.channels = [\n",
    "  {\n",
    "    \"name\": \"ur.universal_robots/UniversalRobots/shoulder_pan_joint\"\n",
    "  },\n",
    "  {\n",
    "    \"name\": \"ur.universal_robots/UniversalRobots/shoulder_lift_joint\"\n",
    "  },\n",
    "  {\n",
    "    \"name\": \"ur.universal_robots/UniversalRobots/elbow_joint\"\n",
    "  },\n",
    "  {\n",
    "    \"name\": \"ur.universal_robots/UniversalRobots/wrist_1_joint\"\n",
    "  },\n",
    "  {\n",
    "    \"name\": \"ur.universal_robots/UniversalRobots/wrist_2_joint\"\n",
    "  },\n",
    "  {\n",
    "    \"name\": \"ur.universal_robots/UniversalRobots/wrist_4_joint\"\n",
    "  }\n",
    "]\n",
    "\n",
    "# start application\n",
    "app.start()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "2021-12-21 13:32:39,185 DEBUG Stopped command_generator/PyCodelet\n",
      "2021-12-21 13:32:39,186 DEBUG Python Codelets All stopped...\n"
     ]
    }
   ],
   "source": [
    "app.stop()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "UR10 Digital IO Control\n",
    "====="
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "2021-12-21 14:05:48,835 DEBUG Binding PyCodelet command_generator/PyCodelet\n",
      "2021-12-21 14:05:48,839 DEBUG Launching isaac core\n",
      "2021-12-21 14:05:48,841 DEBUG Launching pycodelet threads\n",
      "2021-12-21 14:05:48,843 DEBUG Launching command_generator/PyCodelet\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "7e910b78a4984c43959b80c80ad5b765",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "VBox(children=(HBox(children=(Label(value='tool_digital_out_0', layout=Layout(width='150px')), FloatSlider(val…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "app = Application(name=\"io_control_ur_hardware\", modules=[\"sight\"])\n",
    "\n",
    "app.load(filename=ur_subgraph, prefix=\"ur\")\n",
    "\n",
    "# Load components for configuration\n",
    "ur_interface = app.nodes[\"ur.subgraph\"][\"interface\"]\n",
    "ur_driver = app.nodes[\"ur.universal_robots\"][\"UniversalRobots\"]\n",
    "\n",
    "io_names = ur_driver.config.tool_digital_out_names\n",
    "ur_driver.config.robot_ip = IP\n",
    "\n",
    "# add pycodelet for digitalIO control\n",
    "widget_node = app.add(\"command_generator\")\n",
    "io_commander = widget_node.add(JointPositionControl)\n",
    "io_commander.config.joints = io_names\n",
    "io_commander.config.limits = [[0, 1]] * len(io_names)\n",
    "io_commander.config.measure = 'none'\n",
    "\n",
    "# Edges\n",
    "app.connect(io_commander, \"command\", ur_interface, \"io_command\")\n",
    "app.connect(ur_interface, \"io_state\", io_commander, \"state\")\n",
    "\n",
    "# Enable sight\n",
    "widget = app.add(\"sight\").add(app.registry.isaac.sight.SightWidget, \"shuffle_box_hardware\")\n",
    "widget.config.type = \"plot\"\n",
    "widget.config.channels = [\n",
    "  {\n",
    "    \"name\": \"ur.universal_robots/UniversalRobots/tool_digital_out_0\"\n",
    "  },\n",
    "  {\n",
    "    \"name\": \"ur.universal_robots/UniversalRobots/tool_digital_out_1\"\n",
    "  },\n",
    "  {\n",
    "    \"name\": \"ur.universal_robots/UniversalRobots/tool_digital_in_0\"\n",
    "  },\n",
    "  {\n",
    "    \"name\": \"ur.universal_robots/UniversalRobots/tool_digital_in_1\"\n",
    "  }\n",
    "]\n",
    "\n",
    "app.start()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "2021-12-21 14:06:16,643 DEBUG Stopped command_generator/PyCodelet\n",
      "2021-12-21 14:06:16,644 DEBUG Python Codelets All stopped...\n"
     ]
    }
   ],
   "source": [
    "app.stop()"
   ]
  }
 ],
 "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.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}