add moving cube animation

aaucsd · Nov 14, 2022 · dad20b4 · dad20b4
1 parent feaf8a5
commit dad20b4
Show file tree

Hide file tree

Showing 5 changed files with 49 additions and 26 deletions.
diff --git a/.gitignore b/.gitignore
@@ -1,3 +1,6 @@
+# For project
+data/visualization
+
 # Byte-compiled / optimized / DLL files
 __pycache__/
 *.py[cod]

diff --git a/examples/grouping_robot.ipynb b/examples/grouping_robot.ipynb
@@ -66,6 +66,7 @@
     "    for timestep in np.linspace(0, len(robot.trajectory.waypoints)-1, 100):\n",
     "        robot.set_config_at_time(timestep)\n",
     "        p.performCollisionDetection()\n",
+    "        \n",
     "        sleep(0.1)"
    ]
   },

diff --git a/examples/object_follow_trajectory.ipynb b/examples/object_follow_trajectory.ipynb
@@ -13,20 +13,7 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "import os \n",
-    "import sys\n",
-    "import inspect\n",
-    "currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))\n",
-    "parentdir = os.path.dirname(currentdir)\n",
-    "sys.path.insert(0, parentdir)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 2,
-   "metadata": {},
-   "outputs": [],
-   "source": [
+    "%cd -q ..\n",
     "from objects.static.voxel import VoxelObject\n",
     "from objects.dynamic_object import MovableObjectFactory, DynamicObjectFactory, MovableBaseObject\n",
     "from objects.trajectory import WaypointLinearTrajectory\n",
@@ -36,26 +23,30 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 3,
+   "execution_count": 2,
    "metadata": {},
    "outputs": [],
    "source": [
     "import numpy as np\n",
+    "orientations = np.random.uniform(size=(3, 4))\n",
+    "orientations = orientations / np.linalg.norm(orientations, axis=-1, keepdims=True)\n",
+    "waypoints = np.concatenate((np.random.uniform(size=(3, 3)), orientations), axis=-1)\n",
     "dynamic_voxel = DynamicVoxel(base_position=[1, 1, 1],\n",
     "                             base_orientation=[0, 0, 0, 1],\n",
-    "                             half_extents=[0.5, 0.5, 0.5],\n",
-    "                             move_mode='p',\n",
-    "                             trajectory=WaypointLinearTrajectory(waypoints=np.random.uniform(size=(3, 3))))"
+    "                             half_extents=[0.2, 0.2, 0.2],\n",
+    "                             move_mode='po',\n",
+    "                             trajectory=WaypointLinearTrajectory(waypoints=waypoints))"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 4,
+   "execution_count": 3,
    "metadata": {},
    "outputs": [],
    "source": [
     "from time import sleep\n",
-    "def create_traj(dynamic_object, distance):    \n",
+    "def create_traj(dynamic_object, distance):  \n",
+    "    gifs = []    \n",
     "    p.resetSimulation()\n",
     "    p.setAdditionalSearchPath(pybullet_data.getDataPath())\n",
     "    plane = p.createCollisionShape(p.GEOM_PLANE)\n",
@@ -70,12 +61,15 @@
     "    for timestep in np.linspace(0, len(dynamic_object.trajectory.waypoints)-1, 100):\n",
     "        dynamic_object.set_config_at_time(timestep)\n",
     "        p.performCollisionDetection()\n",
-    "        sleep(0.1)"
+    "        sleep(0.1)\n",
+    "        gifs.append(p.getCameraImage(width=360, height=360, lightDirection=[1, 1, 1], shadow=1,\n",
+    "                                         renderer=p.ER_BULLET_HARDWARE_OPENGL)[2]) \n",
+    "    return gifs        "
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 5,
+   "execution_count": 4,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -84,11 +78,20 @@
     "import pybullet as p\n",
     "import pybullet_data\n",
     "import numpy as np\n",
+    "from utils.utils import save_gif\n",
     "p.connect(p.GUI)\n",
     "plt.clf()\n",
     "plt.close('all')\n",
-    "create_traj(dynamic_voxel, 2)\n",
-    "p.disconnect()    "
+    "gifs = create_traj(dynamic_voxel, 2)\n",
+    "p.disconnect()    \n",
+    "save_gif(gifs, 'data/visualization/moving_cube.gif')"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "![MovingCube](../data/visualization/moving_cube.gif \"moving_cube\")"
    ]
   }
  ],

diff --git a/examples/robot_follow_trajectory.ipynb b/examples/robot_follow_trajectory.ipynb
@@ -42,7 +42,8 @@
    "outputs": [],
    "source": [
     "from time import sleep\n",
-    "def create_traj(robot, distance):    \n",
+    "def create_traj(robot, distance):  \n",
+    "    gifs = []\n",
     "    p.resetSimulation()\n",
     "    p.setAdditionalSearchPath(pybullet_data.getDataPath())\n",
     "    plane = p.createCollisionShape(p.GEOM_PLANE)\n",
@@ -57,7 +58,10 @@
     "    for timestep in np.linspace(0, len(robot.trajectory.waypoints)-1, 100):\n",
     "        robot.set_config_at_time(timestep)\n",
     "        p.performCollisionDetection()\n",
-    "        sleep(0.1)"
+    "        sleep(0.1)\n",
+    "        gifs.append(p.getCameraImage(width=1100, height=900, lightDirection=[1, 1, 1], shadow=1,\n",
+    "                                                 renderer=p.ER_BULLET_HARDWARE_OPENGL)[2]) \n",
+    "    return gifs"
    ]
   },
   {

diff --git a/utils/utils.py b/utils/utils.py
@@ -0,0 +1,12 @@
+import numpy as np
+from PIL import Image
+
+def save_gif(imgs, gif_name):
+    # Setup the 4 dimensional array
+    a_frames = []
+    for img in imgs:
+        a_frames.append(np.asarray(img))
+    a = np.stack(a_frames)
+
+    ims = [Image.fromarray(a_frame) for a_frame in a]
+    ims[0].save(gif_name, save_all=True, append_images=ims[1:], loop=0, duration=50)