Skip to content

Commit

Permalink
first commit
Browse files Browse the repository at this point in the history
  • Loading branch information
@happygaoxiao
happygaoxiao committed May 31, 2024
0 parents commit 5522580
Show file tree
Hide file tree
Showing 485 changed files with 554,788 additions and 0 deletions.
248 changes: 248 additions & 0 deletions PRM_test.ipynb
Original file line number Diff line number Diff line change
@@ -0,0 +1,248 @@
{
"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
}
Loading

0 comments on commit 5522580

Please sign in to comment.