stretch vrb vis collision mesh

SemRoCo · Aug 16, 2024 · 60bf45b · 60bf45b
1 parent 1e1fcb7
commit 60bf45b
Show file tree

Hide file tree

Showing 2 changed files with 51 additions and 14 deletions.
diff --git a/scripts/giskard_examples.ipynb b/scripts/giskard_examples.ipynb
@@ -17,7 +17,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 1,
+   "execution_count": 10,
    "outputs": [],
    "source": [
     "from giskardpy.python_interface.python_interface import GiskardWrapper\n",
@@ -48,7 +48,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 2,
+   "execution_count": 11,
    "outputs": [],
    "source": [
     "rospy.init_node('giskard_examples')"
@@ -62,10 +62,35 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
-   "outputs": [],
+   "execution_count": 12,
+   "outputs": [
+    {
+     "data": {
+      "text/plain": "Dropdown(description='Select Robot:', layout=Layout(width='200px'), options=('PR2', 'Stretch'), style=Descript…",
+      "application/vnd.jupyter.widget-view+json": {
+       "version_major": 2,
+       "version_minor": 0,
+       "model_id": "41ecd0f04d9f41eebcad5e9a77faacb2"
+      }
+     },
+     "metadata": {},
+     "output_type": "display_data"
+    },
+    {
+     "data": {
+      "text/plain": "Button(button_style='success', description='Start Launch File', style=ButtonStyle())",
+      "application/vnd.jupyter.widget-view+json": {
+       "version_major": 2,
+       "version_minor": 0,
+       "model_id": "d778640d72e64143b3c36a52ec4244e2"
+      }
+     },
+     "metadata": {},
+     "output_type": "display_data"
+    }
+   ],
    "source": [
-    "global ROBOT\n",
+    "ROBOT = 'stretch'\n",
     "# List of available launch files\n",
     "launch_files = {\n",
     "    'PR2': 'package://giskardpy/launch/giskardpy_pr2_standalone_vrb.launch',\n",
@@ -91,7 +116,6 @@
     "# Function to start the selected ROS launch file\n",
     "def on_button_click(b):\n",
     "    selected_launch_file = launch_files[dropdown.value]\n",
-    "    global ROBOT\n",
     "    ROBOT = dropdown.value.lower()\n",
     "    try:\n",
     "        result = subprocess.run('rosnode kill /giskard', shell=True, check=True, stdout=subprocess.PIPE,\n",
@@ -121,7 +145,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 3,
+   "execution_count": 13,
    "outputs": [],
    "source": [
     "if ROBOT == 'stretch':\n",
@@ -142,7 +166,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 4,
+   "execution_count": 14,
    "outputs": [],
    "source": [
     "# Launch the giskardpy PR2 standalone launch file before this\n",
@@ -157,7 +181,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 5,
+   "execution_count": 15,
    "outputs": [],
    "source": [
     "# Define some helper functions\n",
@@ -272,7 +296,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 6,
+   "execution_count": 16,
    "outputs": [],
    "source": [
     "reset_giskard()\n",
@@ -304,8 +328,20 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 7,
-   "outputs": [],
+   "execution_count": 18,
+   "outputs": [
+    {
+     "ename": "AssertionError",
+     "evalue": "",
+     "output_type": "error",
+     "traceback": [
+      "\u001B[0;31m---------------------------------------------------------------------------\u001B[0m",
+      "\u001B[0;31mAssertionError\u001B[0m                            Traceback (most recent call last)",
+      "Cell \u001B[0;32mIn[18], line 23\u001B[0m\n\u001B[1;32m     20\u001B[0m gs\u001B[38;5;241m.\u001B[39mmonitors\u001B[38;5;241m.\u001B[39madd_end_motion(pose_monitor)\n\u001B[1;32m     21\u001B[0m \u001B[38;5;66;03m# in the case that there is no specific end motion condition the default end motion condition stops the motion when the robot stops moving i.e. the QP is in a local minimum.\u001B[39;00m\n\u001B[1;32m     22\u001B[0m \u001B[38;5;66;03m# gs.add_default_end_motion_conditions()\u001B[39;00m\n\u001B[0;32m---> 23\u001B[0m \u001B[38;5;28;01massert\u001B[39;00m gs\u001B[38;5;241m.\u001B[39mexecute()\u001B[38;5;241m.\u001B[39merror\u001B[38;5;241m.\u001B[39mcode \u001B[38;5;241m==\u001B[39m GiskardError\u001B[38;5;241m.\u001B[39mSUCCESS\n",
+      "\u001B[0;31mAssertionError\u001B[0m: "
+     ]
+    }
+   ],
    "source": [
     "reset_giskard()\n",
     "\n",

diff --git a/scripts/iai_robots/stretch/stretch_standalone_vrb.py b/scripts/iai_robots/stretch/stretch_standalone_vrb.py
@@ -6,14 +6,15 @@
 from giskardpy.configs.iai_robots.stretch import StretchCollisionAvoidanceConfig, StretchStandaloneInterface
 from giskardpy.configs.world_config import WorldWithDiffDriveRobot
 from giskardpy.configs.qp_controller_config import QPControllerConfig, SupportedQPSolver
-
+from giskardpy.model.ros_msg_visualization import VisualizationMode
 
 if __name__ == '__main__':
     rospy.init_node('giskard')
     giskard = Giskard(world_config=WorldWithDiffDriveRobot(),
                       collision_avoidance_config=StretchCollisionAvoidanceConfig(),
                       robot_interface_config=StretchStandaloneInterface(),
                       behavior_tree_config=StandAloneBTConfig(publish_js=False, publish_tf=True, simulation_max_hz=20,
-                                                              debug_mode=True),
+                                                              debug_mode=True,
+                                                              visualization_mode=VisualizationMode.CollisionsDecomposedFrameLocked),
                       qp_controller_config=QPControllerConfig(qp_solver=SupportedQPSolver.qpSWIFT))
     giskard.live()