PRM_test.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true,
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Jupyter environment detected. Enabling Open3D WebVisualizer.\n",
      "[Open3D INFO] WebRTC GUI backend enabled.\n",
      "[Open3D INFO] WebRTCWindowSystem: HTTP handshake server disabled.\n"
     ]
    }
   ],
   "source": [
    "from tools.PRM_tools import PRM\n",
    "\n",
    "from mujoco_sim import Robot\n",
    "\n",
    "import numpy as np\n",
    "import controller_utils\n",
    "import mujoco\n",
    "from mujoco import viewer\n",
    "import tools.rotations as rot\n",
    "%load_ext autoreload\n",
    "%autoreload 2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Order of objects: ['banana', 'bottle', 'chip_can', 'soft_scrub', 'sugar_box']\n",
      "Kinematic model has been loaded from kinematics/q2pose_left.txt\n"
     ]
    }
   ],
   "source": [
    "xml_path = 'description/iiwa7_allegro_ycb.xml'\n",
    "obj_name = ''\n",
    "model = mujoco.MjModel.from_xml_path(xml_path)\n",
    "data = mujoco.MjData(model)\n",
    "mujoco.mj_step(model, data)\n",
    "\n",
    "# viewer.launch(model, data)\n",
    "view = viewer.launch_passive(model, data)\n",
    "obj_names = ['banana', 'bottle', 'chip_can', 'soft_scrub', 'sugar_box']\n",
    "num = 0\n",
    "obj = obj_names[num]\n",
    "r = Robot(model, data, view, auto_sync=True, obj_names=obj_names)\n",
    "\n",
    "q0 = np.array(\n",
    "    [-0.32032434, 0.02706913, -0.22881953, -1.42621454, 1.3862661, 0.55966738, 1.79477984 - np.pi * 3 / 2])\n",
    "r.d.qpos[:7] = q0\n",
    "r.step()\n",
    "view.sync()\n",
    "\n",
    "\n"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Hello world\n",
      "joint_8.0 connects base_link to link_8.0\n",
      "joint_9.0 connects link_8.0 to link_9.0\n",
      "joint_10.0 connects link_9.0 to link_10.0\n",
      "joint_11.0 connects link_10.0 to link_11.0\n",
      "joint_11.0_tip connects link_11.0 to link_11.0_tip\n",
      "joint_4.0 connects base_link to link_4.0\n",
      "joint_5.0 connects link_4.0 to link_5.0\n",
      "joint_6.0 connects link_5.0 to link_6.0\n",
      "joint_7.0 connects link_6.0 to link_7.0\n",
      "joint_7.0_tip connects link_7.0 to link_7.0_tip\n",
      "joint_0.0 connects base_link to link_0.0\n",
      "joint_1.0 connects link_0.0 to link_1.0\n",
      "joint_2.0 connects link_1.0 to link_2.0\n",
      "joint_3.0 connects link_2.0 to link_3.0\n",
      "joint_3.0_tip connects link_3.0 to link_3.0_tip\n",
      "joint_12.0 connects base_link to link_12.0\n",
      "joint_13.0 connects link_12.0 to link_13.0\n",
      "joint_14.0 connects link_13.0 to link_14.0\n",
      "joint_15.0 connects link_14.0 to link_15.0\n",
      "joint_15.0_tip connects link_15.0 to link_15.0_tip\n",
      "['joint_0.0', 'joint_1.0', 'joint_2.0', 'joint_3.0', 'joint_3.0_tip', 'joint_4.0', 'joint_5.0', 'joint_6.0', 'joint_7.0', 'joint_7.0_tip', 'joint_8.0', 'joint_9.0', 'joint_10.0', 'joint_11.0', 'joint_11.0_tip', 'joint_12.0', 'joint_13.0', 'joint_14.0', 'joint_15.0', 'joint_15.0_tip']\n",
      "{'joint_8.0': 0.0, 'joint_9.0': 0.0, 'joint_10.0': 0.0, 'joint_11.0': 0.0, 'joint_11.0_tip': 0.0, 'joint_4.0': 0.0, 'joint_5.0': 0.0, 'joint_6.0': 0.0, 'joint_7.0': 0.0, 'joint_7.0_tip': 0.0, 'joint_0.0': 0.0, 'joint_1.0': 0.0, 'joint_2.0': 0.0, 'joint_3.0': 0.0, 'joint_3.0_tip': 0.0, 'joint_12.0': 0.0, 'joint_13.0': 0.0, 'joint_14.0': 0.0, 'joint_15.0': 0.0, 'joint_15.0_tip': 0.0}\n",
      "Loading neural network models ...\n",
      "Initializing Robot Model ...\n",
      "Hand-obj collision: load NN model from /home/xiao/research/lasa/multi_grasping/Neural-JSDF/JSDF/hand/NN_model/models/single_001_left.pt use_cuda= True\n",
      "Hand-obj collision: load NN model from /home/xiao/research/lasa/multi_grasping/Neural-JSDF/JSDF/hand/NN_model/models/single_101_left.pt use_cuda= True\n",
      "Hand-obj collision: load NN model from /home/xiao/research/lasa/multi_grasping/Neural-JSDF/JSDF/hand/NN_model/models/single_401_left.pt use_cuda= True\n",
      "joint_8.0 connects base_link to link_8.0\n",
      "joint_9.0 connects link_8.0 to link_9.0\n",
      "joint_10.0 connects link_9.0 to link_10.0\n",
      "joint_11.0 connects link_10.0 to link_11.0\n",
      "joint_11.0_tip connects link_11.0 to link_11.0_tip\n",
      "joint_4.0 connects base_link to link_4.0\n",
      "joint_5.0 connects link_4.0 to link_5.0\n",
      "joint_6.0 connects link_5.0 to link_6.0\n",
      "joint_7.0 connects link_6.0 to link_7.0\n",
      "joint_7.0_tip connects link_7.0 to link_7.0_tip\n",
      "joint_0.0 connects base_link to link_0.0\n",
      "joint_1.0 connects link_0.0 to link_1.0\n",
      "joint_2.0 connects link_1.0 to link_2.0\n",
      "joint_3.0 connects link_2.0 to link_3.0\n",
      "joint_3.0_tip connects link_3.0 to link_3.0_tip\n",
      "joint_12.0 connects base_link to link_12.0\n",
      "joint_13.0 connects link_12.0 to link_13.0\n",
      "joint_14.0 connects link_13.0 to link_14.0\n",
      "joint_15.0 connects link_14.0 to link_15.0\n",
      "joint_15.0_tip connects link_15.0 to link_15.0_tip\n",
      "['joint_0.0', 'joint_1.0', 'joint_2.0', 'joint_3.0', 'joint_3.0_tip', 'joint_4.0', 'joint_5.0', 'joint_6.0', 'joint_7.0', 'joint_7.0_tip', 'joint_8.0', 'joint_9.0', 'joint_10.0', 'joint_11.0', 'joint_11.0_tip', 'joint_12.0', 'joint_13.0', 'joint_14.0', 'joint_15.0', 'joint_15.0_tip']\n",
      "{'joint_8.0': 0.0, 'joint_9.0': 0.0, 'joint_10.0': 0.0, 'joint_11.0': 0.0, 'joint_11.0_tip': 0.0, 'joint_4.0': 0.0, 'joint_5.0': 0.0, 'joint_6.0': 0.0, 'joint_7.0': 0.0, 'joint_7.0_tip': 0.0, 'joint_0.0': 0.0, 'joint_1.0': 0.0, 'joint_2.0': 0.0, 'joint_3.0': 0.0, 'joint_3.0_tip': 0.0, 'joint_12.0': 0.0, 'joint_13.0': 0.0, 'joint_14.0': 0.0, 'joint_15.0': 0.0, 'joint_15.0_tip': 0.0}\n",
      "Trial 0\n",
      "Path length: 4\n",
      "PRM* by optimize the path\n",
      "[(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), (1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)]\n"
     ]
    }
   ],
   "source": [
    "start = np.zeros(23)\n",
    "goal = np.ones(23)\n",
    "PRM_test = PRM(start, goal)\n",
    "PRM_test.rerun_PRM(optimize=True)\n",
    "print(PRM_test.path)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "outputs": [],
   "source": [
    "for node in PRM_test.path:\n",
    "    r.iiwa_joint_space_reaching(np.array(node))\n",
    "\n"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "outputs": [
    {
     "ename": "ValueError",
     "evalue": "operands could not be broadcast together with shapes (0,) (16,) ",
     "output_type": "error",
     "traceback": [
      "\u001B[0;31m---------------------------------------------------------------------------\u001B[0m",
      "\u001B[0;31mValueError\u001B[0m                                Traceback (most recent call last)",
      "Cell \u001B[0;32mIn[13], line 1\u001B[0m\n\u001B[0;32m----> 1\u001B[0m \u001B[43mr\u001B[49m\u001B[38;5;241;43m.\u001B[39;49m\u001B[43miiwa_joint_space_reaching\u001B[49m\u001B[43m(\u001B[49m\u001B[43mq0\u001B[49m\u001B[43m)\u001B[49m\n",
      "File \u001B[0;32m~/research/lasa/multi_grasping/PRM_planning_23/mujoco_sim.py:456\u001B[0m, in \u001B[0;36mRobot.iiwa_joint_space_reaching\u001B[0;34m(self, q, qh, vel, coupling)\u001B[0m\n\u001B[1;32m    453\u001B[0m         for i in range(nums_h):\n\u001B[1;32m    454\u001B[0m             self.full_joint_space_control(q, qh_list[i, :])  # iiwa keeps static, move the hand only\n\u001B[0;32m--> 456\u001B[0m     # print(q - r.q)\n\u001B[1;32m    457\u001B[0m     # print(qh - r.qh)\n\u001B[1;32m    459\u001B[0m def iiwa_joint_space_test(self, i=0, t=10):\n\u001B[1;32m    461\u001B[0m     t0 = time.time()\n",
      "File \u001B[0;32m~/research/lasa/multi_grasping/PRM_planning_23/mujoco_sim.py:375\u001B[0m, in \u001B[0;36mRobot.full_joint_space_control\u001B[0;34m(self, q, qh)\u001B[0m\n\u001B[1;32m    372\u001B[0m     q \u001B[38;5;241m=\u001B[39m np\u001B[38;5;241m.\u001B[39mconcatenate([q, qh])\n\u001B[1;32m    374\u001B[0m tau \u001B[38;5;241m=\u001B[39m \u001B[38;5;28mself\u001B[39m\u001B[38;5;241m.\u001B[39miiwa_joint_space_PD(q[:\u001B[38;5;241m7\u001B[39m])\n\u001B[0;32m--> 375\u001B[0m tau_hand \u001B[38;5;241m=\u001B[39m \u001B[38;5;28;43mself\u001B[39;49m\u001B[38;5;241;43m.\u001B[39;49m\u001B[43mhand_move_torque\u001B[49m\u001B[43m(\u001B[49m\u001B[43mq\u001B[49m\u001B[43m[\u001B[49m\u001B[38;5;241;43m7\u001B[39;49m\u001B[43m:\u001B[49m\u001B[38;5;241;43m23\u001B[39;49m\u001B[43m]\u001B[49m\u001B[43m,\u001B[49m\u001B[43m \u001B[49m\u001B[43mkh_scale\u001B[49m\u001B[38;5;241;43m=\u001B[39;49m\u001B[43m[\u001B[49m\u001B[38;5;241;43m0.2\u001B[39;49m\u001B[43m,\u001B[49m\u001B[43m \u001B[49m\u001B[38;5;241;43m0.2\u001B[39;49m\u001B[43m,\u001B[49m\u001B[43m \u001B[49m\u001B[38;5;241;43m0.2\u001B[39;49m\u001B[43m,\u001B[49m\u001B[43m \u001B[49m\u001B[38;5;241;43m0.2\u001B[39;49m\u001B[43m]\u001B[49m\u001B[43m)\u001B[49m\n\u001B[1;32m    376\u001B[0m \u001B[38;5;28mself\u001B[39m\u001B[38;5;241m.\u001B[39msend_torque(np\u001B[38;5;241m.\u001B[39mconcatenate([tau, tau_hand]))\n",
      "File \u001B[0;32m~/research/lasa/multi_grasping/PRM_planning_23/mujoco_sim.py:263\u001B[0m, in \u001B[0;36mRobot.hand_move_torque\u001B[0;34m(self, qh, dqh, u_add, kh_scale)\u001B[0m\n\u001B[1;32m    260\u001B[0m \u001B[38;5;28;01mif\u001B[39;00m kh_scale \u001B[38;5;129;01mis\u001B[39;00m \u001B[38;5;28;01mNone\u001B[39;00m:\n\u001B[1;32m    261\u001B[0m     kh_scale \u001B[38;5;241m=\u001B[39m [\u001B[38;5;241m1\u001B[39m, \u001B[38;5;241m1\u001B[39m, \u001B[38;5;241m1\u001B[39m, \u001B[38;5;241m1.\u001B[39m]\n\u001B[0;32m--> 263\u001B[0m error_q \u001B[38;5;241m=\u001B[39m \u001B[43mqh\u001B[49m\u001B[43m \u001B[49m\u001B[38;5;241;43m-\u001B[39;49m\u001B[43m \u001B[49m\u001B[38;5;28;43mself\u001B[39;49m\u001B[38;5;241;43m.\u001B[39;49m\u001B[43mqh\u001B[49m\n\u001B[1;32m    264\u001B[0m error_dq \u001B[38;5;241m=\u001B[39m dqh \u001B[38;5;241m-\u001B[39m \u001B[38;5;28mself\u001B[39m\u001B[38;5;241m.\u001B[39mdqh\n\u001B[1;32m    265\u001B[0m u \u001B[38;5;241m=\u001B[39m np\u001B[38;5;241m.\u001B[39mzeros(\u001B[38;5;241m16\u001B[39m)\n",
      "\u001B[0;31mValueError\u001B[0m: operands could not be broadcast together with shapes (0,) (16,) "
     ]
    }
   ],
   "source": [
    "r.iiwa_joint_space_reaching(q0)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "outputs": [
    {
     "data": {
      "text/plain": "array([ 0.0031554 ,  0.00252099,  0.0034692 ,  0.00688929, -0.0088695 ,\n        0.00251338,  0.04543892])"
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "r.q - q0"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}