From 723563f88826305ba44c9150b70e69ed854f83a8 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Wed, 19 Apr 2023 19:57:26 -0700 Subject: [PATCH] cell reorg --- stretch_body/jupyter/funmap_experiments.ipynb | 831 +++++++++--------- 1 file changed, 438 insertions(+), 393 deletions(-) diff --git a/stretch_body/jupyter/funmap_experiments.ipynb b/stretch_body/jupyter/funmap_experiments.ipynb index cdd006d..ed1e810 100644 --- a/stretch_body/jupyter/funmap_experiments.ipynb +++ b/stretch_body/jupyter/funmap_experiments.ipynb @@ -1,5 +1,24 @@ { "cells": [ + { + "cell_type": "markdown", + "id": "c15bee22", + "metadata": { + "tags": [] + }, + "source": [ + "# PyFUNMAP Experiments\n", + "This notebook experiments with running FUNMAP outside of the ROS ecosystem.\n", + "\n", + "**How to run:**\n", + "\n", + " 1. Power on and connect to Stretch (keyboard/mouse/HDMI or via SSH)\n", + " 2. Navigate to the directory this notebook lives in and run `jupyter notebook --no-browser`\n", + " 3. If SSH-ed, open a tunnel for Jupyter using `ssh -L 8888:localhost:8888 `\n", + " 4. Click on Jupyter's link (e.g. looks like `http://localhost:8888/?token=`)\n", + " 5. Shift + Enter to execute the cells below" + ] + }, { "cell_type": "code", "execution_count": 1, @@ -12,11 +31,11 @@ "text": [ "Defaulting to user installation because normal site-packages is not writeable\n", "Collecting git+https://github.com/safijari/tiny_tf.git\n", - " Cloning https://github.com/safijari/tiny_tf.git to /tmp/pip-req-build-pld9sgvq\n", - " Running command git clone --filter=blob:none --quiet https://github.com/safijari/tiny_tf.git /tmp/pip-req-build-pld9sgvq\n", + " Cloning https://github.com/safijari/tiny_tf.git to /tmp/pip-req-build-192cxjlh\n", + " Running command git clone --filter=blob:none --quiet https://github.com/safijari/tiny_tf.git /tmp/pip-req-build-192cxjlh\n", " Resolved https://github.com/safijari/tiny_tf.git to commit 6d28b69dfde971f5ed63ff24de5c84b950690997\n", " Preparing metadata (setup.py) ... \u001b[?25ldone\n", - "\u001b[?25hRequirement already satisfied: numpy in ./.local/lib/python3.8/site-packages (from tiny-tf==1.2.0) (1.22.4)\n" + "\u001b[?25hRequirement already satisfied: numpy in /home/hello-robot/.local/lib/python3.8/site-packages (from tiny-tf==1.2.0) (1.22.4)\n" ] } ], @@ -26,25 +45,337 @@ }, { "cell_type": "code", - "execution_count": 62, + "execution_count": 2, "id": "e870dd4a", "metadata": {}, "outputs": [], "source": [ + "# FUNMAP Imports\n", "import cv2\n", "import time\n", "import math\n", + "import pathlib\n", "import ros_numpy\n", "import numpy as np\n", "from copy import deepcopy\n", "from numba import jit, njit\n", "\n", + "# Robot imports\n", + "import threading\n", + "from urdfpy import URDF\n", + "import stretch_body.hello_utils as hu\n", + "from tiny_tf.tf import TFTree, Transform\n", + "from stretch_body.robot import Robot\n", + "\n", + "# ROS Message imports\n", "from sensor_msgs.msg import PointCloud2" ] }, + { + "cell_type": "markdown", + "id": "ccc1cae3", + "metadata": { + "jp-MarkdownHeadingCollapsed": true, + "tags": [] + }, + "source": [ + "## Emulating ROS\n", + "\n", + "We'll set up some infrastructure to emulate some of the ROS components that FUNMAP relies on. Namely, we emulate the TF2 library's `lookup_transform()` method." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "id": "7460aa2c", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "--------- Homing Head ----\n", + "--------- Homing Lift ----\n", + "Homing Lift...\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[INFO] [robot_monitor]: Guarded contact lift\n", + "[INFO] [robot_monitor]: Wrist single tap: 4\n", + "[INFO] [robot_monitor]: Base bump event\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Hardstop detected at motor position (rad) 88.93499755859375\n", + "Marking Lift position to 1.096228 (m)\n", + "Marking Lift position to 0.000000 (m)\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[INFO] [robot_monitor]: Base bump event\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Lift homing successful\n", + "--------- Homing Arm ----\n", + "Homing Arm...\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[INFO] [robot_monitor]: Guarded contact arm\n", + "[INFO] [robot_monitor]: Wrist single tap: 6\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Hardstop detected at motor position (rad) 1.4693927764892578\n", + "Marking Arm position to 0.000000 (m)\n", + "Arm homing successful\n", + "--------- Homing stretch_gripper ----\n", + "Moving to first hardstop...\n", + "First hardstop contact at position (ticks): 4098\n", + "-----\n", + "Homing offset was 4401\n", + "Marking current position to zero ticks\n", + "Homing offset is now 303 (ticks)\n", + "-----\n", + "Current position (ticks): 13\n", + "Moving to calibrated zero: (rad)\n", + "--------- Homing wrist_yaw ----\n", + "Moving to first hardstop...\n", + "First hardstop contact at position (ticks): -4102\n", + "-----\n", + "Homing offset was -2156\n", + "Marking current position to zero ticks\n", + "Homing offset is now 1938 (ticks)\n", + "-----\n", + "Current position (ticks): 41\n", + "Moving to calibrated zero: (rad)\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[INFO] [robot_monitor]: Wrist single tap: 8\n", + "[INFO] [robot_monitor]: Wrist single tap: 9\n", + "[INFO] [robot_monitor]: Wrist single tap: 12\n", + "[INFO] [robot_monitor]: Wrist single tap: 16\n", + "[INFO] [robot_monitor]: Wrist single tap: 17\n" + ] + } + ], + "source": [ + "# Setup the Python API to Stretch\n", + "robot = Robot()\n", + "if not robot.startup():\n", + " print(\"Failed to open connection to the robot\")\n", + "\n", + "# Ensure robot is homed\n", + "if not robot.is_calibrated():\n", + " robot.home()" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "id": "033774e1", + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "/home/hello-robot/.local/lib/python3.8/site-packages/urdfpy/urdf.py:2169: RuntimeWarning: invalid value encountered in true_divide\n", + " value = value / np.linalg.norm(value)\n" + ] + } + ], + "source": [ + "# Load Stretch's URDF\n", + "urdf_path = str((pathlib.Path(hu.get_fleet_directory()) / 'exported_urdf' / 'stretch.urdf').absolute())\n", + "urdf = URDF.load(urdf_path)" + ] + }, + { + "cell_type": "markdown", + "id": "19948277", + "metadata": {}, + "source": [ + "We'll write a method `get_current_configuration()` that returns a dictionary of the robot's current configuration. We will be able to pass this configuration into Stretch's URDF to get transforms from every link to \"base_link\"." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "id": "96f891c9", + "metadata": {}, + "outputs": [], + "source": [ + "def get_current_configuration(tool):\n", + " def bound_range(name, value):\n", + " return min(max(value, urdf.joint_map[name].limit.lower), urdf.joint_map[name].limit.upper)\n", + "\n", + " if tool == 'tool_stretch_gripper':\n", + " q_lift = bound_range('joint_lift', robot.lift.status['pos'])\n", + " q_arml = bound_range('joint_arm_l0', robot.arm.status['pos'] / 4.0)\n", + " q_yaw = bound_range('joint_wrist_yaw', robot.end_of_arm.status['wrist_yaw']['pos'])\n", + " q_pan = bound_range('joint_head_pan', robot.head.status['head_pan']['pos'])\n", + " q_tilt = bound_range('joint_head_tilt', robot.head.status['head_tilt']['pos'])\n", + " return {\n", + " 'joint_left_wheel': 0.0,\n", + " 'joint_right_wheel': 0.0,\n", + " 'joint_lift': q_lift,\n", + " 'joint_arm_l0': q_arml,\n", + " 'joint_arm_l1': q_arml,\n", + " 'joint_arm_l2': q_arml,\n", + " 'joint_arm_l3': q_arml,\n", + " 'joint_wrist_yaw': q_yaw,\n", + " 'joint_gripper_finger_left': 0.0,\n", + " 'joint_gripper_finger_right': 0.0,\n", + " 'joint_head_pan': q_pan,\n", + " 'joint_head_tilt': q_tilt\n", + " }\n", + " elif tool == 'tool_stretch_dex_wrist':\n", + " q_lift = bound_range('joint_lift', robot.lift.status['pos'])\n", + " q_arml = bound_range('joint_arm_l0', robot.arm.status['pos'] / 4.0)\n", + " q_yaw = bound_range('joint_wrist_yaw', robot.end_of_arm.status['wrist_yaw']['pos'])\n", + " q_pitch = bound_range('joint_wrist_pitch', robot.end_of_arm.status['wrist_pitch']['pos'])\n", + " q_roll = bound_range('joint_wrist_roll', robot.end_of_arm.status['wrist_roll']['pos'])\n", + " q_pan = bound_range('joint_head_pan', robot.head.status['head_pan']['pos'])\n", + " q_tilt = bound_range('joint_head_tilt', robot.head.status['head_tilt']['pos'])\n", + " return {\n", + " 'joint_left_wheel': 0.0,\n", + " 'joint_right_wheel': 0.0,\n", + " 'joint_lift': q_lift,\n", + " 'joint_arm_l0': q_arml,\n", + " 'joint_arm_l1': q_arml,\n", + " 'joint_arm_l2': q_arml,\n", + " 'joint_arm_l3': q_arml,\n", + " 'joint_wrist_yaw': q_yaw,\n", + " 'joint_wrist_pitch': q_pitch,\n", + " 'joint_wrist_roll': q_roll,\n", + " 'joint_gripper_finger_left': 0.0,\n", + " 'joint_gripper_finger_right': 0.0,\n", + " 'joint_head_pan': q_pan,\n", + " 'joint_head_tilt': q_tilt\n", + " }" + ] + }, + { + "cell_type": "markdown", + "id": "490e9415", + "metadata": {}, + "source": [ + "FUNMAP relies on a ROS node that can execute position commands for the robot. We'll create a fake ROS node for it now." + ] + }, { "cell_type": "code", - "execution_count": 63, + "execution_count": 8, + "id": "c443b5c1", + "metadata": {}, + "outputs": [], + "source": [ + "class FakeNode:\n", + " \n", + " def __init__(self):\n", + "# self.tf2_buffer = None\n", + " self.joint_state = None\n", + " self.point_cloud = ros_numpy.msgify(PointCloud2, self.makeArray(100))\n", + " self.thread = threading.Thread(target=self._spin)\n", + " self.thread.setDaemon(True)\n", + " self.thread.start()\n", + "\n", + " def makeArray(self, npoints):\n", + " points_arr = np.zeros((npoints,), dtype=[\n", + " ('x', np.float32),\n", + " ('y', np.float32),\n", + " ('z', np.float32),\n", + " ('rgb', np.uint32)])\n", + " points_arr['x'] = np.random.random((npoints,))\n", + " points_arr['y'] = np.random.random((npoints,))\n", + " points_arr['z'] = np.random.random((npoints,))\n", + " points_arr['rgb'] = 255\n", + "\n", + " return points_arr \n", + "\n", + " def _spin(self):\n", + " while True:\n", + " # update self.tf2_buffer\n", + " tree = TFTree()\n", + " q_curr = get_current_configuration(tool=robot.end_of_arm.name)\n", + " fk_curr = urdf.link_fk(cfg=q_curr)\n", + " for l in urdf.links:\n", + " if l.name == \"base_link\":\n", + " continue\n", + " tree.add_transform('base_link', l.name, Transform.from_matrix(fk_curr[l]))\n", + " tree.add_transform('map', 'base_link', Transform())\n", + " self.tf2_buffer = tree\n", + "\n", + " # update self.point_cloud\n", + " self.point_cloud.header.stamp = time.time()\n", + " self.point_cloud.header.frame_id = 'camera_color_optical_frame'\n", + "\n", + " def move_to_pose(self, pose, return_before_done=False, custom_contact_thresholds=False):\n", + " if custom_contact_thresholds:\n", + " raise Exception('FakeRobot.move_to_pose: helpppp! dont support contact thresholds yet')\n", + " print(f'Moving to {pose}')\n", + " if 'joint_lift' in pose:\n", + " robot.lift.move_to(pose['joint_lift'])\n", + " if 'wrist_extension' in pose:\n", + " robot.arm.move_to(pose['wrist_extension'])\n", + " robot.push_command()\n", + " if 'joint_wrist_yaw' in pose:\n", + " robot.end_of_arm.move_to('wrist_yaw', pose['joint_wrist_yaw'])\n", + " if 'joint_wrist_pitch' in pose:\n", + " robot.end_of_arm.move_to('wrist_pitch', pose['joint_wrist_pitch'])\n", + " if 'joint_wrist_roll' in pose:\n", + " robot.end_of_arm.move_to('wrist_roll', pose['joint_wrist_roll'])\n", + " if 'joint_head_pan' in pose:\n", + " robot.head.move_to('head_pan', pose['joint_head_pan'])\n", + " if 'joint_head_tilt' in pose:\n", + " robot.head.move_to('head_tilt', pose['joint_head_tilt'])\n", + " if 'joint_gripper_finger_left' in pose or 'joint_gripper_finger_right' in pose:\n", + " print('FakeRobot.move_to_pose: gripper not converted')\n", + " if not return_before_done:\n", + " print('FakeRobot.move_to_pose: sleeping 1 second')\n", + " time.sleep(1)" + ] + }, + { + "cell_type": "markdown", + "id": "390a3f70-b7ed-4a08-b811-fd6819c2810a", + "metadata": { + "jp-MarkdownHeadingCollapsed": true, + "tags": [] + }, + "source": [ + "## FUNMAP Code\n", + "\n", + "Lots of code." + ] + }, + { + "cell_type": "code", + "execution_count": 9, "id": "e2396e2f", "metadata": {}, "outputs": [], @@ -187,7 +518,7 @@ }, { "cell_type": "code", - "execution_count": 64, + "execution_count": 10, "id": "1fed3d42", "metadata": {}, "outputs": [], @@ -208,7 +539,7 @@ }, { "cell_type": "code", - "execution_count": 65, + "execution_count": 11, "id": "f06a3958", "metadata": {}, "outputs": [], @@ -283,7 +614,7 @@ }, { "cell_type": "code", - "execution_count": 66, + "execution_count": 12, "id": "e889d16e", "metadata": {}, "outputs": [], @@ -759,7 +1090,7 @@ }, { "cell_type": "code", - "execution_count": 67, + "execution_count": 13, "id": "f5b4b144", "metadata": {}, "outputs": [], @@ -1173,7 +1504,7 @@ }, { "cell_type": "code", - "execution_count": 75, + "execution_count": 14, "id": "92ad0117", "metadata": {}, "outputs": [], @@ -1344,7 +1675,7 @@ }, { "cell_type": "code", - "execution_count": 69, + "execution_count": 15, "id": "37c0e390", "metadata": {}, "outputs": [], @@ -1431,7 +1762,7 @@ }, { "cell_type": "code", - "execution_count": 70, + "execution_count": 16, "id": "d046c1d0", "metadata": {}, "outputs": [], @@ -1507,7 +1838,7 @@ }, { "cell_type": "code", - "execution_count": 71, + "execution_count": 17, "id": "f5610c35", "metadata": {}, "outputs": [], @@ -1768,255 +2099,16 @@ ] }, { - "cell_type": "code", - "execution_count": 10, - "id": "aec2661b", + "cell_type": "markdown", + "id": "5f5a7ef5-852d-43e6-82ce-1d21b286e5ad", "metadata": {}, - "outputs": [], "source": [ - "from tiny_tf.tf import TFTree, Transform\n", - "from urdfpy import URDF\n", - "import pathlib\n", - "import stretch_body.hello_utils as hu\n", - "import threading" + "## Head Scanning Experiment" ] }, { "cell_type": "code", - "execution_count": 11, - "id": "79aaa4b7", - "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "/home/hello-robot/.local/lib/python3.8/site-packages/urdfpy/urdf.py:2169: RuntimeWarning: invalid value encountered in true_divide\n", - " value = value / np.linalg.norm(value)\n" - ] - } - ], - "source": [ - "urdf_path = str((pathlib.Path(hu.get_fleet_directory()) / 'exported_urdf' / 'stretch.urdf').absolute())\n", - "urdf = URDF.load(urdf_path)" - ] - }, - { - "cell_type": "code", - "execution_count": 12, - "id": "feed9cd0", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "True" - ] - }, - "execution_count": 12, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "from stretch_body.robot import Robot\n", - "robot = Robot()\n", - "robot.startup()" - ] - }, - { - "cell_type": "code", - "execution_count": 13, - "id": "1846e9c1", - "metadata": {}, - "outputs": [], - "source": [ - "def get_current_configuration(tool):\n", - " def bound_range(name, value):\n", - " return min(max(value, urdf.joint_map[name].limit.lower), urdf.joint_map[name].limit.upper)\n", - "\n", - " if tool == 'tool_stretch_gripper':\n", - " q_lift = bound_range('joint_lift', robot.lift.status['pos'])\n", - " q_arml = bound_range('joint_arm_l0', robot.arm.status['pos'] / 4.0)\n", - " q_yaw = bound_range('joint_wrist_yaw', robot.end_of_arm.status['wrist_yaw']['pos'])\n", - " q_pan = bound_range('joint_head_pan', robot.head.status['head_pan']['pos'])\n", - " q_tilt = bound_range('joint_head_tilt', robot.head.status['head_tilt']['pos'])\n", - " return {\n", - " 'joint_left_wheel': 0.0,\n", - " 'joint_right_wheel': 0.0,\n", - " 'joint_lift': q_lift,\n", - " 'joint_arm_l0': q_arml,\n", - " 'joint_arm_l1': q_arml,\n", - " 'joint_arm_l2': q_arml,\n", - " 'joint_arm_l3': q_arml,\n", - " 'joint_wrist_yaw': q_yaw,\n", - " 'joint_gripper_finger_left': 0.0,\n", - " 'joint_gripper_finger_right': 0.0,\n", - " 'joint_head_pan': q_pan,\n", - " 'joint_head_tilt': q_tilt\n", - " }\n", - " elif tool == 'tool_stretch_dex_wrist':\n", - " q_lift = bound_range('joint_lift', robot.lift.status['pos'])\n", - " q_arml = bound_range('joint_arm_l0', robot.arm.status['pos'] / 4.0)\n", - " q_yaw = bound_range('joint_wrist_yaw', robot.end_of_arm.status['wrist_yaw']['pos'])\n", - " q_pitch = bound_range('joint_wrist_pitch', robot.end_of_arm.status['wrist_pitch']['pos'])\n", - " q_roll = bound_range('joint_wrist_roll', robot.end_of_arm.status['wrist_roll']['pos'])\n", - " q_pan = bound_range('joint_head_pan', robot.head.status['head_pan']['pos'])\n", - " q_tilt = bound_range('joint_head_tilt', robot.head.status['head_tilt']['pos'])\n", - " return {\n", - " 'joint_left_wheel': 0.0,\n", - " 'joint_right_wheel': 0.0,\n", - " 'joint_lift': q_lift,\n", - " 'joint_arm_l0': q_arml,\n", - " 'joint_arm_l1': q_arml,\n", - " 'joint_arm_l2': q_arml,\n", - " 'joint_arm_l3': q_arml,\n", - " 'joint_wrist_yaw': q_yaw,\n", - " 'joint_wrist_pitch': q_pitch,\n", - " 'joint_wrist_roll': q_roll,\n", - " 'joint_gripper_finger_left': 0.0,\n", - " 'joint_gripper_finger_right': 0.0,\n", - " 'joint_head_pan': q_pan,\n", - " 'joint_head_tilt': q_tilt\n", - " }" - ] - }, - { - "cell_type": "code", - "execution_count": 14, - "id": "539deec3", - "metadata": {}, - "outputs": [], - "source": [ - "# robot.stow()\n", - "q_curr = get_current_configuration(tool=robot.end_of_arm.name)" - ] - }, - { - "cell_type": "code", - "execution_count": 15, - "id": "cdc9ab31", - "metadata": {}, - "outputs": [], - "source": [ - "tree = TFTree()\n", - "fk_curr = urdf.link_fk(cfg=q_curr)\n", - "for l in urdf.links:\n", - " if l.name == \"base_link\":\n", - " continue\n", - " tree.add_transform('base_link', l.name, Transform.from_matrix(fk_curr[l]))" - ] - }, - { - "cell_type": "code", - "execution_count": 16, - "id": "9463cce1", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "array([[-0.23877914, -0.97033812, -0.03779491, -0.08155349],\n", - " [ 0.97071945, -0.23956263, 0.01770604, 0.05906881],\n", - " [-0.02623509, -0.03246042, 0.99912864, 0.47051695],\n", - " [ 0. , 0. , 0. , 1. ]])" - ] - }, - "execution_count": 16, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "tree.lookup_transform('link_grasp_center', 'base_link').matrix" - ] - }, - { - "cell_type": "code", - "execution_count": 76, - "id": "c0e9b71b", - "metadata": {}, - "outputs": [], - "source": [ - "def makeArray(npoints):\n", - " points_arr = np.zeros((npoints,), dtype=[\n", - " ('x', np.float32),\n", - " ('y', np.float32),\n", - " ('z', np.float32),\n", - " ('rgb', np.uint32)])\n", - " points_arr['x'] = np.random.random((npoints,))\n", - " points_arr['y'] = np.random.random((npoints,))\n", - " points_arr['z'] = np.random.random((npoints,))\n", - " points_arr['rgb'] = 255\n", - "\n", - " return points_arr\n", - "\n", - "class FakeRobot:\n", - " \n", - " def __init__(self):\n", - "# self.tf2_buffer = None\n", - " self.joint_state = None\n", - " self.point_cloud = ros_numpy.msgify(PointCloud2, makeArray(100))\n", - " self.thread = threading.Thread(target=self._spin)\n", - " self.thread.setDaemon(True)\n", - " self.thread.start()\n", - "\n", - " def _spin(self):\n", - " while True:\n", - " # update self.tf2_buffer\n", - " tree = TFTree()\n", - " q_curr = get_current_configuration(tool=robot.end_of_arm.name)\n", - " fk_curr = urdf.link_fk(cfg=q_curr)\n", - " for l in urdf.links:\n", - " if l.name == \"base_link\":\n", - " continue\n", - " tree.add_transform('base_link', l.name, Transform.from_matrix(fk_curr[l]))\n", - " tree.add_transform('map', 'base_link', Transform())\n", - " self.tf2_buffer = tree\n", - "\n", - " # update self.point_cloud\n", - " self.point_cloud.header.stamp = time.time()\n", - " self.point_cloud.header.frame_id = 'camera_color_optical_frame'\n", - "\n", - " def move_to_pose(self, pose, return_before_done=False, custom_contact_thresholds=False):\n", - " if custom_contact_thresholds:\n", - " raise Exception('FakeRobot.move_to_pose: helpppp! dont support contact thresholds yet')\n", - " print(f'Moving to {pose}')\n", - " if 'joint_lift' in pose:\n", - " robot.lift.move_to(pose['joint_lift'])\n", - " if 'wrist_extension' in pose:\n", - " robot.arm.move_to(pose['wrist_extension'])\n", - " robot.push_command()\n", - " if 'joint_wrist_yaw' in pose:\n", - " robot.end_of_arm.move_to('wrist_yaw', pose['joint_wrist_yaw'])\n", - " if 'joint_wrist_pitch' in pose:\n", - " robot.end_of_arm.move_to('wrist_pitch', pose['joint_wrist_pitch'])\n", - " if 'joint_wrist_roll' in pose:\n", - " robot.end_of_arm.move_to('wrist_roll', pose['joint_wrist_roll'])\n", - " if 'joint_head_pan' in pose:\n", - " robot.head.move_to('head_pan', pose['joint_head_pan'])\n", - " if 'joint_head_tilt' in pose:\n", - " robot.head.move_to('head_tilt', pose['joint_head_tilt'])\n", - " if 'joint_gripper_finger_left' in pose or 'joint_gripper_finger_right' in pose:\n", - " print('FakeRobot.move_to_pose: gripper not converted')\n", - " if not return_before_done:\n", - " print('FakeRobot.move_to_pose: sleeping 1 second')\n", - " time.sleep(1)" - ] - }, - { - "cell_type": "code", - "execution_count": 77, - "id": "ca0a1185", - "metadata": {}, - "outputs": [], - "source": [ - "node = FakeRobot()" - ] - }, - { - "cell_type": "code", - "execution_count": 79, + "execution_count": 19, "id": "c67281e1", "metadata": {}, "outputs": [ @@ -2036,165 +2128,118 @@ "FakeRobot.move_to_pose: sleeping 1 second\n", "Moving to {'joint_head_pan': -3.6}\n", "FakeRobot.move_to_pose: sleeping 1 second\n", - "Sleeping 0.5 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.5 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", "Moving to {'joint_head_pan': -2.7583333333333333}\n", "FakeRobot.move_to_pose: sleeping 1 second\n", - "Sleeping 0.5 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.5 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", "Moving to {'joint_head_pan': -1.9166666666666667}\n", "FakeRobot.move_to_pose: sleeping 1 second\n", - "Sleeping 0.5 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.5 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", "Moving to {'joint_head_pan': -1.0750000000000002}\n", "FakeRobot.move_to_pose: sleeping 1 second\n", - "Sleeping 0.5 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.5 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", "Moving to {'joint_head_pan': -0.2333333333333334}\n", "FakeRobot.move_to_pose: sleeping 1 second\n", - "Sleeping 0.5 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.5 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", "Moving to {'joint_head_pan': 0.608333333333333}\n", "FakeRobot.move_to_pose: sleeping 1 second\n", - "Sleeping 0.5 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.5 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", "Moving to {'joint_head_pan': 1.45}\n", "FakeRobot.move_to_pose: sleeping 1 second\n", - "Sleeping 0.5 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.5 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", "Moving to {'joint_head_pan': 0.1, 'joint_head_tilt': -1.2}\n", "FakeRobot.move_to_pose: sleeping 1 second\n", - "Sleeping 0.5 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "Sleeping 0.06666666666666667 seconds\n", - "via rospy.loginfo: The head scan took 23.410109281539917 seconds.\n" + "via rospy.sleep: sleeping 0.5 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.sleep: sleeping 0.06666666666666667 seconds\n", + "via rospy.loginfo: The head scan took 20.203986644744873 seconds.\n" ] } ], "source": [ + "node = FakeNode()\n", "head_scan = HeadScan(voi_side_m=16.0)\n", "head_scan.execute_full(node, fast_scan=False)" ] }, - { - "cell_type": "code", - "execution_count": 21, - "id": "169f2e47", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "0\n", - "1\n", - "2\n", - "3\n", - "4\n", - "5\n", - "6\n", - "7\n", - "8\n", - "9\n", - "10\n", - "11\n", - "12\n", - "13\n", - "14\n", - "15\n", - "16\n", - "17\n", - "18\n", - "19\n", - "20\n", - "21\n", - "22\n", - "23\n", - "24\n", - "25\n", - "26\n", - "27\n", - "28\n", - "29\n" - ] - } - ], - "source": [ - "for i in range(30):\n", - " print(i)" - ] - }, { "cell_type": "code", "execution_count": null,