notebooks/04_mobile_platform.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Mobile platform tutorial 🚗\n",
    "\n",
    "This notebook shows how to construct a mobile platform with a UR5e arm mounted on top.\n",
    "\n",
    "It is recommended that you follow the `01_building_a_scene.ipynb` notebook before this one."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Building the `scene` 🎭\n",
    "\n",
    "The mobile platform that is supported in airo-drake is based on the KELO modular platform.\n",
    "To create a scene with a mobile robot, you need to supply which bricks should go where. The API currently assumes that you have one or more drive bricks and at least a battery and a CPU brick.\n",
    "\n",
    "Moreover, for collision checking, you can define the dimensions of the entire mobile platform. Boxes will be added to model the sides and roof of the robot.\n",
    "\n",
    "At AIRO, we have a specific configuration of the KELO bricks, and the default values of the function you will see below reflect our platform."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from pydrake.planning import RobotDiagramBuilder\n",
    "from pydrake.math import RigidTransform, RollPitchYaw\n",
    "from airo_drake import MobilePlatformWithSingleArmScene, add_mobile_platform, finish_build, add_meshcat, add_manipulator, attach_mobile_platform_to_world\n",
    "\n",
    "robot_diagram_builder = RobotDiagramBuilder()\n",
    "\n",
    "meshcat = add_meshcat(robot_diagram_builder)\n",
    "mobi_index, _ = add_mobile_platform(robot_diagram_builder, drive_positions=(np.array([1, -0.5]), np.array([1, 0.5]), np.array([-1, -0.5]),\n",
    "                                                np.array([-1, 0.5])), cpu_position=np.array([0, -0.5]), battery_position=np.array([0, 0.5]))\n",
    "mobi_frame = robot_diagram_builder.plant().GetFrameByName(\"base_link\", mobi_index)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The mobile platform itself has been constructed. We still need to perform two steps:\n",
    "\n",
    "1. We should add the UR5e arm to the robot\n",
    "2. We should attach the robot to the world\n",
    "\n",
    "In AIRO's robot, the UR5e arm should be mounted on top of the roof, at `(x, y) = (0.2445, 0)`. The arm is rotated by -90 degrees."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# For these two values, see the API of `add_mobile_platform`.\n",
    "side_height = 0.43\n",
    "roof_thickness = 0.03\n",
    "arm_transform = RigidTransform(p=[0.2445, 0, side_height + roof_thickness], rpy=RollPitchYaw([0, 0, -np.pi / 2]))\n",
    "\n",
    "arm_index, gripper_index = add_manipulator(robot_diagram_builder, \"ur5e\", \"robotiq_2f_85\", arm_transform, static_gripper=True, parent_frame=mobi_frame)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We attach the mobile platform to the world by means of a `PlanarJoint`. This allows the robot to move with linear `x` and `y` velocities and an angular velocity `theta`. airo-drake provides a function for this."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "static_platform = False  # We want to move the platform later below. Setting this to False welds the platform to the world with a planar joint.\n",
    "attach_mobile_platform_to_world(robot_diagram_builder, mobi_index, static_platform)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot_diagram, context = finish_build(robot_diagram_builder)\n",
    "\n",
    "scene = MobilePlatformWithSingleArmScene(robot_diagram, mobi_index, arm_index, gripper_index, meshcat)\n",
    "scene"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can now look at the mobile robot in meshcat by clicking the link in the first cell's output."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Moving the mobile platform 🛞\n",
    "\n",
    "Since we attached the mobile platform to the world by means of a planar joint, we can manipulate the pose of the robot by setting the values of this joint.\n",
    "When we query the plant's positions, we see that we have 9 values. There are six values for the UR5e's joints, and 3 for the mobile platform's joint."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "diagram = scene.robot_diagram\n",
    "plant = diagram.plant()\n",
    "context = diagram.CreateDefaultContext()\n",
    "plant_context = plant.GetMyContextFromRoot(context)\n",
    "\n",
    "len(plant.GetPositions(plant_context))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plant.SetPositions(plant_context, [1, 0.5, np.pi / 2, 0, 0, 0, 0, 0, 0])\n",
    "diagram.ForcedPublish(context)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Looking at meshcat, you'll see that the platform has moved 1 meter along x, 50 centimeter along y and rotated 90 degrees.\n",
    "Remark how the pivot is at the center of the platform."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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": 4
}