{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Pick up test\n",
    "This example will show a simple pickup test in addition to the TF Broadcaster to visualize the result in rviz. \n",
    "This should be usable inside a binderhub using webviz\n",
    "\n"
   ],
   "id": "caee925fbec69e2f"
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Bulletworld"
   ],
   "id": "7ccb3b66cb8fba5e"
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "outputs": [],
   "source": [
    "from rospy import get_param\n",
    "from pycram.bullet_world import BulletWorld, Object\n",
    "from pycram.pose import Pose\n",
    "\n",
    "from pycram.process_module import simulated_robot, with_simulated_robot\n",
    "from pycram.language import macros, par\n",
    "from pycram.designators.location_designator import *\n",
    "from pycram.designators.action_designator import *\n",
    "from pycram.enums import Arms\n",
    "from pycram.designators.object_designator import *\n",
    "from pycram.designators.object_designator import BelieveObject\n",
    "from pycram.plan_failures import IKError\n",
    "import math\n",
    "\n",
    "try:\n",
    "    robot = get_param('/nbparam_robot')\n",
    "    environment = get_param('/nbparam_environment')\n",
    "except Exception as e:\n",
    "    robot = 'pr2'\n",
    "    environment = 'kitchen'\n",
    "    \n",
    "print(f\"Robot: {robot}\")\n",
    "print(f\"Environment: {environment}\")\n",
    "\n",
    "# With Bulletworld\n",
    "world = BulletWorld()\n",
    "\n",
    "# Without Bulletworld (use rviz instead)\n",
    "world = BulletWorld('DIRECT')"
   ],
   "metadata": {
    "collapsed": false
   },
   "id": "78e92ab8"
  },
  {
   "cell_type": "markdown",
   "source": [
    "## Robot"
   ],
   "metadata": {
    "collapsed": false
   },
   "id": "221afda8aa837b03"
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "outputs": [],
   "source": [
    "robot = Object(\"pr2\", \"robot\", robot + \".urdf\")\n",
    "robot_desig = ObjectDesignatorDescription(names=[\"pr2\"]).resolve()"
   ],
   "metadata": {
    "collapsed": false
   },
   "id": "c30dad53f0748438"
  },
  {
   "cell_type": "markdown",
   "source": [
    "## Create Objects"
   ],
   "metadata": {
    "collapsed": false
   },
   "id": "1d853d1918f5621"
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "milk = Object(\"milk\", \"milk\", \"milk.stl\", pose=Pose([2, 0, 1]))\n",
    "milk_BO = BelieveObject(names=[\"milk\"])"
   ],
   "id": "dc2ee8cda121f29a"
  },
  {
   "cell_type": "markdown",
   "source": [
    "## TF Broadcaster"
   ],
   "metadata": {
    "collapsed": false
   },
   "id": "1a81217f4138801e"
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "outputs": [],
   "source": [
    "from pycram.ros.tf_broadcaster import TFBroadcaster\n",
    "from pycram.ros.joint_state_publisher import JointStatePublisher\n",
    "from pycram.ros.viz_marker_publisher import VizMarkerPublisher\n",
    "\n",
    "broadcaster = TFBroadcaster()\n",
    "joint_publisher = JointStatePublisher(\"joint_states\", 0.1)\n",
    "#v = VizMarkerPublisher(topic_name='viz_marker')"
   ],
   "metadata": {
    "collapsed": false
   },
   "id": "ce80da8da7aeb4c9"
  },
  {
   "cell_type": "markdown",
   "source": [
    "## Navigate to Object"
   ],
   "metadata": {
    "collapsed": false
   },
   "id": "63ceaf3d00d4d51c"
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "outputs": [],
   "source": [
    "with simulated_robot:\n",
    "    ParkArmsAction([Arms.BOTH]).resolve().perform()\n",
    "\n",
    "    MoveTorsoAction([0.33]).resolve().perform()\n",
    "\n",
    "    pickup_pose_knife = CostmapLocation(target=milk_BO.resolve(), reachable_for=robot_desig).resolve()\n",
    "    pickup_arm = pickup_pose_knife.reachable_arms[0]\n",
    "\n",
    "    NavigateAction(target_locations=[pickup_pose_knife.pose]).resolve().perform()"
   ],
   "metadata": {
    "collapsed": false
   },
   "id": "32f7278da4161468"
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Perform the Pickup action"
   ],
   "id": "9aa2490929d38ebd"
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "with simulated_robot:\n",
    "    PickUpAction(object_designator_description=milk_BO,\n",
    "                 arms=[\"left\"],\n",
    "                 grasps=[\"left\", \"right\"]).resolve().perform()"
   ],
   "id": "51e3d71cefcfd346"
  }
 ],
 "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.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}