cccccccc
/
stretch_tutorials
mirror of https://github.com/hello-robot/stretch_tutorials.git
1
0
Fork 0
Code Issues 0 Projects 0 Releases 0 Wiki Activity
Browse Source

cell reorg

pull/16/head
Binit Shah 1 year ago
parent
45a247e0d2
commit
723563f888
1 changed files with 438 additions and 393 deletions
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +438
    -393
      stretch_body/jupyter/funmap_experiments.ipynb

+ 438
- 393
stretch_body/jupyter/funmap_experiments.ipynb View File

@ -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 <ip addr>`\n",
" 4. Click on Jupyter's link (e.g. looks like `http://localhost:8888/?token=<some 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,

Write Preview
Loading…
Cancel
Save