From 41895eced8bbe0a9964c4a8d86ede02c8c435073 Mon Sep 17 00:00:00 2001 From: rainorangelemon Date: Mon, 14 Nov 2022 00:24:43 -0800 Subject: [PATCH] add grouping animation --- examples/grouping_robot.ipynb | 95 ++++++++++++++++++++++------------- robot/abstract_robot.py | 3 +- robot/grouping.py | 10 ++-- robot/individual_robot.py | 3 +- 4 files changed, 68 insertions(+), 43 deletions(-) diff --git a/examples/grouping_robot.ipynb b/examples/grouping_robot.ipynb index e40e49b..f720aaa 100644 --- a/examples/grouping_robot.ipynb +++ b/examples/grouping_robot.ipynb @@ -4,7 +4,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "> We will see one snake robot dancing with a ur5 robot" + "> We will see one kuka robot dancing with a ur5 robot" ] }, { @@ -13,12 +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)" + "%cd -q .." ] }, { @@ -27,7 +22,7 @@ "metadata": {}, "outputs": [], "source": [ - "from robot.snake_robot import SnakeRobot\n", + "from robot.kuka_robot import KukaRobot\n", "from robot.ur5_robot import UR5Robot\n", "from robot.grouping import RobotGroup\n", "from robot.abstract_robot import DynamicRobotFactory\n", @@ -39,57 +34,85 @@ "execution_count": 3, "metadata": {}, "outputs": [], - "source": [ - "snake = SnakeRobot()\n", - "ur5 = UR5Robot()" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "metadata": {}, - "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", - " p.createMultiBody(0, plane) \n", + " p.setAdditionalSearchPath(pybullet_data.getDataPath()) \n", " p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0, lightPosition = [0, 0, 0.1])\n", " p.resetDebugVisualizerCamera(\n", " cameraDistance=distance,\n", - " cameraYaw=10,\n", - " cameraPitch=-45,\n", - " cameraTargetPosition=[0, 0, 0]) \n", + " cameraYaw=0,\n", + " cameraPitch=0,\n", + " cameraTargetPosition=[0, 0, 0.5]) \n", " robot.load()\n", " 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)" + "# print(robot.no_collision(), robot.robots[0].no_collision(), robot.robots[1].no_collision())\n", + " gifs.append(p.getCameraImage(width=480, height=360, lightDirection=[1, 1, 1], shadow=1,\n", + " renderer=p.ER_BULLET_HARDWARE_OPENGL)[2]) \n", + " sleep(0.1)\n", + " return gifs" ] }, { "cell_type": "code", - "execution_count": 5, + "execution_count": 4, "metadata": {}, - "outputs": [], + "outputs": [ + { + "data": { + "text/html": [ + "" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], "source": [ "%matplotlib inline\n", "import matplotlib.pyplot as plt\n", "import pybullet as p\n", "import pybullet_data\n", "import numpy as np\n", - "p.connect(p.GUI)\n", + "from utils.utils import save_gif\n", + "from IPython.display import HTML\n", + "import base64\n", + "\n", "plt.clf()\n", "plt.close('all')\n", - "robot = RobotGroup([snake, ur5])\n", - "DynamicRobotGrup = DynamicRobotFactory.create_dynamic_robot_class(type(robot))\n", - "dynamic_robot = DynamicRobotGrup(robots=[snake, ur5], \n", - " trajectory=WaypointLinearTrajectory(np.random.uniform(low=robot.limits_low, high=robot.limits_high, size=(10, robot.config_dim))))\n", - "create_traj(dynamic_robot, 2)\n", - "p.disconnect() " + "DynamicRobotGrup = DynamicRobotFactory.create_dynamic_robot_class(RobotGroup)\n", + "\n", + "kuka = KukaRobot(base_position=[-0.3, 0, 0])\n", + "ur5 = UR5Robot(base_position=[0.3, 0, 0])\n", + "group_robot = RobotGroup(robots=[kuka, ur5], collision_eps=0.01)\n", + "\n", + "p.connect(p.DIRECT)\n", + "p.resetSimulation()\n", + "group_robot.load()\n", + "# generate collision-free trajectory\n", + "while True:\n", + " traj = WaypointLinearTrajectory(np.random.uniform(low=group_robot.limits_low, high=group_robot.limits_high, size=(4, group_robot.config_dim))) \n", + " if group_robot._edge_fp(traj.waypoints[0], traj.waypoints[1]) and group_robot._edge_fp(traj.waypoints[1], traj.waypoints[2]):\n", + " break\n", + "p.disconnect()\n", + "\n", + "p.connect(p.GUI, options='--background_color_red=1.0 --background_color_green=1.0 --background_color_blue=1.0')\n", + "dynamic_robot = DynamicRobotGrup(robots=[kuka, ur5], \n", + " trajectory=traj)\n", + "gifs = create_traj(dynamic_robot, 1.5)\n", + "p.disconnect() \n", + "\n", + "\n", + "save_gif(gifs, f'data/visualization/grouping_robot.gif')\n", + "b64 = base64.b64encode(open(f'data/visualization/grouping_robot.gif', 'rb').read()).decode('ascii')\n", + "display(HTML(f''))" ] } ], diff --git a/robot/abstract_robot.py b/robot/abstract_robot.py index e590261..dc914cd 100644 --- a/robot/abstract_robot.py +++ b/robot/abstract_robot.py @@ -8,9 +8,10 @@ class AbstractRobot(MovableObject, ABC): # An abstract robot - def __init__(self, limits_low, limits_high, **kwargs): + def __init__(self, limits_low, limits_high, collision_eps, **kwargs): self.limits_low = limits_low self.limits_high = limits_high + self.collision_eps = collision_eps self.config_dim = len(self.limits_low) super(AbstractRobot, self).__init__(**kwargs) diff --git a/robot/grouping.py b/robot/grouping.py index 247214d..5389e1e 100644 --- a/robot/grouping.py +++ b/robot/grouping.py @@ -10,7 +10,7 @@ class RobotGroup(AbstractRobot): Grouping multiple robots together into a meta-robot ''' - def __init__(self, robots, grouping_mask_fn=None, **kwargs): + def __init__(self, robots, grouping_mask_fn=None, collision_eps=None, **kwargs): ''' grouping mask function aims to assign the collision mask to each robot the argument to the function is an instance of the robot group @@ -19,10 +19,12 @@ def __init__(self, robots, grouping_mask_fn=None, **kwargs): assert np.all([isinstance(robot, IndividualRobot) for robot in robots]) self.robots = robots self.grouping_mask_fn = grouping_mask_fn - self.collision_eps = min([robot.collision_eps for robot in self.robots]) + if collision_eps is None: + collision_eps = min([robot.collision_eps for robot in self.robots]) limits_low, limits_high = self._get_limits() super(RobotGroup, self).__init__(limits_low=limits_low, - limits_high=limits_high, **kwargs) + limits_high=limits_high, + collision_eps=collision_eps, **kwargs) def _get_limits(self): all_limits_low = [] @@ -55,7 +57,7 @@ def set_config(self, config, item_ids=None): def no_collision(self): p.performCollisionDetection() - if np.all([len(p.getContactPoints(item_id)) == 0] for item_id in self.item_ids): + if np.all([len(p.getContactPoints(item_id)) == 0 for item_id in self.item_ids]): self.collision_check_count += 1 return True else: diff --git a/robot/individual_robot.py b/robot/individual_robot.py index 3bf376a..87cb66e 100644 --- a/robot/individual_robot.py +++ b/robot/individual_robot.py @@ -6,12 +6,11 @@ class IndividualRobot(AbstractRobot, ABC): # An individual robot - def __init__(self, base_position, base_orientation, urdf_file, collision_eps, **kwargs): + def __init__(self, base_position, base_orientation, urdf_file, **kwargs): # for loading the pybullet self.base_position = base_position self.base_orientation = base_orientation - self.collision_eps = collision_eps self.urdf_file = urdf_file joints, limits_low, limits_high = self._get_joints_and_limits(self.urdf_file)