dynamic-data/sipp

environment/dynamic/dual_kuka_env.py

@@ -2,14 +2,14 @@
 from environment.dynamic_env import DynamicEnv
 from robot.multi_robot.dual_kuka_robot import DualKukaRobot
 
-class DualKuka5Env(DynamicEnv):
+class DualKukaEnv(DynamicEnv):
 
     def __init__(self, objects, robot_config=None):
         if robot_config is None:
             robot = DualKukaRobot()
         else:
             robot = DualKukaRobot(**robot_config)
-        super(DualKuka5Env, self).__init__(objects, robot)
+        super(DualKukaEnv, self).__init__(objects, robot)
 
     def set_camera_angle(self):
         p.resetDebugVisualizerCamera(

environment/dynamic/triple_simple2arm_env.py

@@ -2,14 +2,14 @@
 from environment.dynamic_env import DynamicEnv
 from robot.multi_robot.triple_simple2arm_robot import TripleSimple2ArmRobot
 
-class TripleKukaEnv(DynamicEnv):
+class TripleSimple2ArmEnv(DynamicEnv):
 
     def __init__(self, objects, robot_config=None):
         if robot_config is None:
             robot = TripleSimple2ArmRobot()
         else:
             robot = TripleSimple2ArmRobot(**robot_config)
-        super(TripleKukaEnv, self).__init__(objects, robot)
+        super(TripleSimple2ArmEnv, self).__init__(objects, robot)
 
     def set_camera_angle(self):
         p.resetDebugVisualizerCamera(

examples/dataset_dynamic.ipynb

@@ -0,0 +1,304 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 1,
+   "metadata": {
+    "collapsed": true,
+    "pycharm": {
+     "name": "#%%\n"
+    }
+   },
+   "outputs": [],
+   "source": [
+    "import pickle\n",
+    "import yaml\n",
+    "import numpy as np"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 14,
+   "outputs": [],
+   "source": [
+    "env_types = [\n",
+    "    'environment.dynamic.dual_simple2arm_env.DualSimple2ArmEnv',\n",
+    "    'environment.dynamic.dual_kuka4_env.DualKuka4Env',\n",
+    "    'environment.dynamic.dual_kuka5_env.DualKuka5Env',\n",
+    "    'environment.dynamic.dual_kuka_env.DualKukaEnv',\n",
+    "    'environment.dynamic.triple_kuka_env.TripleKukaEnv',\n",
+    "    'environment.dynamic.triple_simple2arm_env.TripleSimple2ArmEnv',\n",
+    "]\n"
+   ],
+   "metadata": {
+    "collapsed": false,
+    "pycharm": {
+     "name": "#%%\n"
+    }
+   }
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 6,
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "bit_star_planner.ipynb         load_robot.ipynb\r\n",
+      "dataset.ipynb                  look_dataset.py\r\n",
+      "dataset_dynamic.ipynb          object_follow_trajectory.ipynb\r\n",
+      "grouping_robot.ipynb           robot_follow_trajectory.ipynb\r\n",
+      "load_environment.ipynb         rrt_star_planner.ipynb\r\n",
+      "load_object.ipynb              \u001B[34myaml_dataset\u001B[m\u001B[m\r\n"
+     ]
+    }
+   ],
+   "source": [],
+   "metadata": {
+    "collapsed": false,
+    "pycharm": {
+     "name": "#%%\n"
+    }
+   }
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 33,
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "1183\n"
+     ]
+    }
+   ],
+   "source": [
+    "# with open('../local_dataset/arm2_1000_test_long_bidirectional.pkl', 'rb') as dataset_pkl:\n",
+    "#     dataset_pkl = pickle.load(dataset_pkl)\n",
+    "\n",
+    "obs_files = [\n",
+    "    # '../local_dataset/arm2_env_1000_test_long_bidirectional.npz',\n",
+    "    '../local_dataset/arm2_env_1000_train_long_hard_bidirectional_part1.npz',\n",
+    "    '../local_dataset/arm2_env_1000_train_long_hard_bidirectional_part2.npz',\n",
+    "    '../local_dataset/arm2_env_1000_train_long_hard_bidirectional_part3.npz',\n",
+    "]\n",
+    "\n",
+    "init_states_list = []\n",
+    "goal_states_list = []\n",
+    "obs_pos_list = []\n",
+    "obs_ori_list = []\n",
+    "obs_traj_list = []\n",
+    "for i in range(len(obs_files)):\n",
+    "    with np.load(obs_files[i]) as f:\n",
+    "        init_states_list.append(f['init_states'])\n",
+    "        goal_states_list.append(f['goal_states'])\n",
+    "        obs_pos_list.append(f['obs_pos'])\n",
+    "        obs_ori_list.append(f['obs_ori'])\n",
+    "        obs_traj_list.append(f['obs_traj'])\n",
+    "        # init_states_list = f['init_states']\n",
+    "        # goal_states_list = f['goal_states']\n",
+    "        # obs_pos_list = f['obs_pos']\n",
+    "        # obs_ori_list = f['obs_ori']\n",
+    "        # obs_traj_list = f['obs_traj']\n",
+    "\n",
+    "\n",
+    "init_states = np.concatenate(init_states_list)\n",
+    "goal_states = np.concatenate(goal_states_list)\n",
+    "obs_pos = np.concatenate(obs_pos_list)\n",
+    "obs_ori = np.concatenate(obs_ori_list)\n",
+    "obs_traj = np.concatenate(obs_traj_list)\n",
+    "\n",
+    "print(len(init_states))"
+   ],
+   "metadata": {
+    "collapsed": false,
+    "pycharm": {
+     "name": "#%%\n"
+    }
+   }
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "outputs": [],
+   "source": [],
+   "metadata": {
+    "collapsed": false,
+    "pycharm": {
+     "name": "#%%\n"
+    }
+   }
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 34,
+   "outputs": [],
+   "source": [
+    "\n",
+    "env_type_id = 0\n",
+    "for id in range(min(len(obs_pos),1000)):\n",
+    "    obstacle = dict(\n",
+    "        type = 'objects.dynamic_object.DynamicObject',\n",
+    "        item = 'robot.simple2arm_robot.Simple2ArmRobot',\n",
+    "        trajectory = dict(\n",
+    "            type = 'objects.trajectory.WaypointLinearTrajectory',\n",
+    "            waypoints = obs_traj[id].tolist(),\n",
+    "        )\n",
+    "    )\n",
+    "\n",
+    "\n",
+    "\n",
+    "    data = dict(\n",
+    "        env = dict(\n",
+    "            type=env_types[env_type_id],\n",
+    "            objects=[obstacle],\n",
+    "        ),\n",
+    "        start = init_states[id].tolist(),\n",
+    "        goal = goal_states[id].tolist(),\n",
+    "    )\n",
+    "    with open(f'./yaml_dataset/dual_simple2arm_env/hard/{id}.yml', 'w') as outfile:\n",
+    "        yaml.dump(data, outfile, default_flow_style=False)\n"
+   ],
+   "metadata": {
+    "collapsed": false,
+    "pycharm": {
+     "name": "#%%\n"
+    }
+   }
+  },
+  {
+   "cell_type": "markdown",
+   "source": [
+    "# 3 simple"
+   ],
+   "metadata": {
+    "collapsed": false,
+    "pycharm": {
+     "name": "#%% md\n"
+    }
+   }
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "outputs": [],
+   "source": [
+    "\n",
+    "obs_files = [\n",
+    "    # '../kuka_arm2_env_1000_test_long_bidirectional_01.npz',\n",
+    "    '../kuka_arm2_env_1000_long_bidirectional_hard_n2g_02.npz',\n",
+    "    '../kuka_arm2_env_1000_long_bidirectional_hard_n2g_03.npz',\n",
+    "    '../kuka_arm2_env_1000_long_bidirectional_hard_n2g_04.npz',\n",
+    "    '../kuka_arm2_env_1000_long_bidirectional_hard_n2g_05.npz',\n",
+    "]\n",
+    "\n",
+    "graph_files = [\n",
+    "    # '../kuka_arm2_1000_test_long_bidirectional_01.pkl',\n",
+    "    '../kuka_arm2_1000_long_bidirectional_hard_n2g_02.pkl',\n",
+    "    '../kuka_arm2_1000_long_bidirectional_hard_n2g_03.pkl',\n",
+    "    '../kuka_arm2_1000_long_bidirectional_hard_n2g_04.pkl',\n",
+    "    '../kuka_arm2_1000_long_bidirectional_hard_n2g_05.pkl',\n",
+    "\n",
+    "    ]\n",
+    "\n",
+    "\n",
+    "env_type_id = 3\n",
+    "\n",
+    "\n",
+    "graphs = []\n",
+    "init_states_list = []\n",
+    "goal_states_list = []\n",
+    "obs_pos_list = []\n",
+    "obs_ori_list = []\n",
+    "obs_traj_list = []\n",
+    "\n",
+    "for file_idx in range(len(obs_files)):\n",
+    "\n",
+    "    with np.load(obs_files[file_idx]) as f:\n",
+    "        init_states_list.append(f['init_states'])\n",
+    "        goal_states_list.append(f['goal_states'])\n",
+    "        obs_pos_list.append(f['obs_pos'])\n",
+    "        obs_ori_list.append(f['obs_ori'])\n",
+    "        obs_traj_list.append(f['obs_traj'])\n",
+    "\n",
+    "    with open(graph_files[file_idx], 'rb') as f:\n",
+    "        load_graph = pickle.load(f)\n",
+    "        graphs.extend(load_graph)\n",
+    "\n",
+    "init_states = np.concatenate(init_states_list)\n",
+    "goal_states = np.concatenate(goal_states_list)\n",
+    "obs_pos = np.concatenate(obs_pos_list)\n",
+    "obs_ori = np.concatenate(obs_ori_list)\n",
+    "obs_traj = np.concatenate(obs_traj_list)\n",
+    "\n",
+    "for id in range(min(len(obs_pos),1000)):\n",
+    "\n",
+    "    robot_obstacle = dict(\n",
+    "        type = 'objects.dynamic_object.DynamicObject',\n",
+    "        item='robot.kuka_robot.KukaRobot',\n",
+    "        trajectory = dict(\n",
+    "            type = 'objects.trajectory.WaypointLinearTrajectory',\n",
+    "            waypoints = obs_traj[id].tolist(),\n",
+    "        )\n",
+    "    )\n",
+    "\n",
+    "    _, _, halfExtents_list, basePosition_list, _, _, _, _, _, _, _ = graphs[id]\n",
+    "\n",
+    "\n",
+    "    voxel_obstacles = list()\n",
+    "    for i in range(len(halfExtents_list)):\n",
+    "        voxel_obstacles.append(dict(\n",
+    "            type = 'objects.static.voxel.VoxelObject',\n",
+    "            half_extents = halfExtents_list[i].tolist(),\n",
+    "            base_position = basePosition_list[i].tolist(),\n",
+    "            base_orientation=[0, 0, 0, 1],\n",
+    "        ))\n",
+    "\n",
+    "    all_obstacles = [robot_obstacle] + voxel_obstacles\n",
+    "\n",
+    "\n",
+    "    data = dict(\n",
+    "        env = dict(\n",
+    "            type=env_types[env_type_id],\n",
+    "            objects=all_obstacles,\n",
+    "        ),\n",
+    "        start = init_states[id].tolist(),\n",
+    "        goal = goal_states[id].tolist(),\n",
+    "    )\n",
+    "\n",
+    "\n",
+    "    with open(f'../hard/{id}.yml', 'w') as outfile:\n",
+    "        yaml.dump(data, outfile, default_flow_style=False)"
+   ],
+   "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
+}

objects/dynamic_object.py

@@ -47,7 +47,7 @@ class MovableObjectFactory:
     @staticmethod
     def create_movable_object_class(ObjectX, MovableXObject):
         '''
-        Argument: turning a object class into a movable object class
+        Argument: turning an object class into a movable object class
         '''
         assert not issubclass(ObjectX, MovableObject)
         assert issubclass(MovableXObject, MovableObject)

planner/dist_utils.py

@@ -0,0 +1,47 @@
+import numpy as np
+
+INFINITY = np.inf
+
+def min_dist(q, dist):
+    """
+    Returns the node with the smallest distance in q.
+    """
+    min_node = None
+    for node in q:
+        if min_node is None:
+            min_node = node
+        elif dist[node] < dist[min_node]:
+            min_node = node
+
+    return min_node
+
+
+def dijkstra(nodes, edges, costs, source):
+    """
+    dijkstra search in configuration space without collision check
+    """
+
+    q = set()
+    dist = {}
+
+    for v in nodes:  # initialization
+        dist[v] = INFINITY  # unknown distance from goal to v
+        q.add(v)  # all nodes initially in q (unvisited nodes)
+
+    # distance from goal to every node
+    dist[source] = 0
+
+    while q:
+        # node with the least distance selected first
+        u = min_dist(q, dist)
+
+        q.remove(u)
+
+        for index, v in enumerate(edges[u]):
+            alt = dist[u] + costs[u][index]
+            if alt < dist[v]:
+                # a shorter path to v has been found
+                dist[v] = alt
+
+    return dist
+