notebooks/04_mobile_platform_planning.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Mobile platform planning 🏎️\n",
    "\n",
    "This notebook illustrates how to do motion planning for a mobile platform. We only show how to do motion planning for the platform (3 DOF) without any mounted UR arm (6 DOF). Full platform motion planning (with 9 DOF) is not yet implemented, but should be possible to do with a similar set-up."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Setup 🏗️️\n",
    "\n",
    "### 1.1 Building our mobile platform scene"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from pydrake.planning import RobotDiagramBuilder, SceneGraphCollisionChecker\n",
    "from pydrake.math import RigidTransform, RollPitchYaw\n",
    "from airo_drake import MobilePlatformWithSingleArmScene, add_wall, 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)\n",
    "\n",
    "attach_mobile_platform_to_world(robot_diagram_builder, mobi_index)\n",
    "\n",
    "# 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",
    "# We make the arm static, to do motion planning purely with the platform.\n",
    "arm_index, gripper_index = add_manipulator(robot_diagram_builder, \"ur5e\", \"robotiq_2f_85\", arm_transform, static_arm=True, static_gripper=True, parent_frame=mobi_frame)\n",
    "\n",
    "# We also add an obstacle to the scene for the platform to avoid.\n",
    "add_wall(robot_diagram_builder, \"YZ\", position=np.array([1.0, 0.0, 0.0]))\n",
    "\n",
    "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": [
    "### 1.2 Mobile platform `SceneGraphCollisionChecker` 💥\n",
    "\n",
    "We need to provide the model indices for the mobile robot. We only motion plan the mobile platform for now, not everything at once."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "collision_checker = SceneGraphCollisionChecker(\n",
    "    model=scene.robot_diagram,\n",
    "    robot_model_instances=[\n",
    "        scene.mobile_platform_index,\n",
    "        scene.arm_index,\n",
    "        scene.gripper_index\n",
    "    ],\n",
    "    edge_step_size=0.125,  # Arbitrary value: we don't use the CheckEdgeCollisionFree\n",
    "    env_collision_padding=0.005,\n",
    "    self_collision_padding=0.005,\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "scene.robot_diagram.plant().num_positions()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "collision_checker.CheckConfigCollisionFree([1.0, 0.0, 0]), collision_checker.CheckConfigCollisionFree([1.0, 0.0, np.pi/2])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plant = scene.robot_diagram.plant()\n",
    "plant_context = plant.GetMyContextFromRoot(context)\n",
    "\n",
    "plant.SetPositions(plant_context, [1.0, 0.0, np.pi/2])\n",
    "scene.robot_diagram.ForcedPublish(context)  # updates the meshcat visualization"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1.3 Inverse kinematics for the mobile robot 🧮\n",
    "\n",
    "The mobile robot's 3 DOF are represented in Drake with a `PlanarJoint` with no bounds on the `x`, `y` and `theta` values.\n",
    "This means that the forward and inverse kinematics of the platform can be implemented with identity functions.\n",
    "\n",
    "In other words, we don't even need to define an IK function!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1.4 `MobilePlatformOmplPlanner` 🧭"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from airo_planner import MobilePlatformOmplPlanner\n",
    "\n",
    "planner = MobilePlatformOmplPlanner(\n",
    "    is_state_valid_fn=collision_checker.CheckConfigCollisionFree\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Moving around 🚶\n",
    "\n",
    "In this section, we demonstrate the `airo-planner` API by moving around the wall with the planner."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "start_pose = np.array([-1.0, 0, np.pi / 2])\n",
    "goal_pose = np.array([5.0, 0, 0])\n",
    "\n",
    "plant.SetPositions(plant_context, start_pose)\n",
    "scene.robot_diagram.ForcedPublish(context)  # updates the meshcat visualization"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "path = planner.plan_to_pose(\n",
    "    start_pose, goal_pose\n",
    ")\n",
    "print(path.shape)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from airo_drake import time_parametrize_toppra_mobile_platform, animate_mobile_platform_trajectory\n",
    "\n",
    "trajectory = time_parametrize_toppra_mobile_platform(scene.robot_diagram.plant(), path)\n",
    "\n",
    "animate_mobile_platform_trajectory(\n",
    "    scene.meshcat, scene.robot_diagram, scene.mobile_platform_index, trajectory\n",
    ")"
   ]
  },
  {
   "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.11.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}