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

saved example map

pull/16/head
Binit Shah 1 year ago
parent
723563f888
commit
a1b89a758a
7 changed files with 267 additions and 272 deletions
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +108
    -0
      stretch_body/jupyter/example_head_scan.yaml
  2. +36
    -0
      stretch_body/jupyter/example_head_scan_mhi.yaml
  3. BIN
     
  4. BIN
     
  5. BIN
     
  6. BIN
     
  7. +123
    -272
      stretch_body/jupyter/funmap_experiments.ipynb

+ 108
- 0
stretch_body/jupyter/example_head_scan.yaml View File

@ -0,0 +1,108 @@
base_link_to_image_mat:
- - 166.66666666666666
- 0.0
- 0.0
- 1331.3333333333333
- - 0.0
- -166.66666666666666
- 0.0
- 1331.3333333333333
- - 0.0
- 0.0
- 215.2542372881356
- 11.762711864406782
- - 0.0
- 0.0
- 0.0
- 1.0
base_link_to_map_mat:
- - 1.0
- 0.0
- 0.0
- 0.0
- - 0.0
- 1.0
- 0.0
- 0.0
- - 0.0
- 0.0
- 1.0
- 0.0
- - 0.0
- 0.0
- 0.0
- 1.0
image_to_base_link_mat:
- - 0.006
- 0.0
- 0.0
- -7.988
- - 0.0
- -0.006
- 0.0
- 7.9879999999999995
- - 0.0
- 0.0
- 0.0046456692913385824
- -0.054645669291338586
- - 0.0
- 0.0
- 0.0
- 1.0
image_to_map_mat:
- - 0.006
- 0.0
- 0.0
- -7.988
- - 0.0
- -0.006
- 0.0
- 7.9879999999999995
- - 0.0
- 0.0
- 0.0046456692913385824
- -0.054645669291338586
- - 0.0
- 0.0
- 0.0
- 1.0
map_to_base_mat:
- - 1.0
- 0.0
- 0.0
- 0.0
- - 0.0
- 1.0
- 0.0
- 0.0
- - 0.0
- 0.0
- 1.0
- 0.0
- - 0.0
- 0.0
- 0.0
- 1.0
map_to_image_mat:
- - 166.66666666666666
- 0.0
- 0.0
- 1331.3333333333333
- - 0.0
- -166.66666666666666
- 0.0
- 1331.3333333333333
- - 0.0
- 0.0
- 215.2542372881356
- 11.762711864406782
- - 0.0
- 0.0
- 0.0
- 1.0
max_height_image_base_filename: /home/hello-robot/repos/stretch_tutorials/stretch_body/jupyter/example_head_scan_mhi
robot_ang_rad: -0.0
robot_xy_pix:
- 1331.3333333333333
- 1331.3333333333333
timestamp: 1681960427.1463504

+ 36
- 0
stretch_body/jupyter/example_head_scan_mhi.yaml View File

@ -0,0 +1,36 @@
camera_depth_image_filename: /home/hello-robot/repos/stretch_tutorials/stretch_body/jupyter/example_head_scan_mhi_camera_depth.png
image.dtype: uint8
image.shape:
- 2667
- 2667
image_filename: /home/hello-robot/repos/stretch_tutorials/stretch_body/jupyter/example_head_scan_mhi_image.npy.gz
image_origin:
- 0.012
- 15.988
- 0.0
m_per_height_unit: 0.0046456692913385824
m_per_pix: 0.006
np.max(image): null
rgb_image_filename: /home/hello-robot/repos/stretch_tutorials/stretch_body/jupyter/example_head_scan_mhi_rgb.png
transform_corrected_to_original: null
transform_original_to_corrected: null
visualization_filename: /home/hello-robot/repos/stretch_tutorials/stretch_body/jupyter/example_head_scan_mhi_visualization.png
voi_data:
axes:
- - 1.0
- 0.0
- 0.0
- - 0.0
- 1.0
- 0.0
- - 0.0
- 0.0
- 1.0
frame_id: map
origin:
- -8.0
- -8.0
- -0.05
x_in_m: 16.0
y_in_m: 16.0
z_in_m: 1.18

BIN
View File


BIN
View File


BIN
View File


BIN
View File


+ 123
- 272
stretch_body/jupyter/funmap_experiments.ipynb View File

@ -31,8 +31,8 @@
"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-192cxjlh\n",
" Running command git clone --filter=blob:none --quiet https://github.com/safijari/tiny_tf.git /tmp/pip-req-build-192cxjlh\n",
" Cloning https://github.com/safijari/tiny_tf.git to /tmp/pip-req-build-qaor8p3r\n",
" Running command git clone --filter=blob:none --quiet https://github.com/safijari/tiny_tf.git /tmp/pip-req-build-qaor8p3r\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 /home/hello-robot/.local/lib/python3.8/site-packages (from tiny-tf==1.2.0) (1.22.4)\n"
@ -54,6 +54,8 @@
"import cv2\n",
"import time\n",
"import math\n",
"import gzip\n",
"import yaml\n",
"import pathlib\n",
"import ros_numpy\n",
"import numpy as np\n",
@ -89,99 +91,7 @@
"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"
]
}
],
"outputs": [],
"source": [
"# Setup the Python API to Stretch\n",
"robot = Robot()\n",
@ -195,7 +105,7 @@
},
{
"cell_type": "code",
"execution_count": 6,
"execution_count": 4,
"id": "033774e1",
"metadata": {},
"outputs": [
@ -224,7 +134,7 @@
},
{
"cell_type": "code",
"execution_count": 7,
"execution_count": 5,
"id": "96f891c9",
"metadata": {},
"outputs": [],
@ -289,7 +199,7 @@
},
{
"cell_type": "code",
"execution_count": 8,
"execution_count": 6,
"id": "c443b5c1",
"metadata": {},
"outputs": [],
@ -360,6 +270,89 @@
" time.sleep(1)"
]
},
{
"cell_type": "markdown",
"id": "6b0c49ee-f327-45db-b328-ab20fde443e1",
"metadata": {},
"source": [
"Last up, we'll emulate some ROS messages and the rospy library."
]
},
{
"cell_type": "code",
"execution_count": 7,
"id": "228b8825-320a-442b-827c-4aede6e4e644",
"metadata": {},
"outputs": [],
"source": [
"class HeaderMsg:\n",
" def __init__(self):\n",
" self.seq = 0\n",
" self.stamp = time.time()\n",
" self.frame_id = ''\n",
"\n",
"class PointCloud2Msg:\n",
" def __init__(self):\n",
" self.header = HeaderMsg()\n",
" self.height = 1080\n",
" self.width = 1280\n",
" self.fields = None\n",
" self.is_bigendian = None\n",
" self.point_step = None\n",
" self.row_step = None\n",
" self.data = [0]\n",
" self.is_dense = None\n",
"\n",
"class Vector3Msg:\n",
" def __init__(self):\n",
" self.x = None\n",
" self.y = None\n",
" self.z = None\n",
"\n",
"class QuaternionMsg:\n",
" def __init__(self):\n",
" self.x = None\n",
" self.y = None\n",
" self.z = None\n",
" self.w = None\n",
"\n",
"class TransformMsg:\n",
" def __init__(self):\n",
" self.translation = Vector3Msg()\n",
" self.rotation = QuaternionMsg()\n",
"\n",
"class TransformStampedMsg:\n",
" def __init__(self):\n",
" self.header = HeaderMsg()\n",
" self.child_frame_id = 'fake_link'\n",
" self.transform = TransformMsg()\n",
"\n",
"class TF2Manager:\n",
" def __init__(self):\n",
" pass\n",
" def lookup_transform(self, from_frame_id, to_frame_id, lookup_time, timeout):\n",
" return TransformStampedMsg()\n",
"\n",
"class Time:\n",
" def __init__(self):\n",
" pass\n",
" def now(self):\n",
" return time.time()\n",
"\n",
"class RospyManager:\n",
" def __init__(self):\n",
" self.Time = Time()\n",
"\n",
" def sleep(self, x):\n",
" #print(f'via rospy.sleep: sleeping {x} seconds')\n",
" time.sleep(x)\n",
"\n",
" def loginfo(self, msg):\n",
" print('via rospy.loginfo: ' + msg)\n",
"\n",
"rospy = RospyManager()"
]
},
{
"cell_type": "markdown",
"id": "390a3f70-b7ed-4a08-b811-fd6819c2810a",
@ -375,7 +368,7 @@
},
{
"cell_type": "code",
"execution_count": 9,
"execution_count": 8,
"id": "e2396e2f",
"metadata": {},
"outputs": [],
@ -518,7 +511,7 @@
},
{
"cell_type": "code",
"execution_count": 10,
"execution_count": 9,
"id": "1fed3d42",
"metadata": {},
"outputs": [],
@ -539,82 +532,7 @@
},
{
"cell_type": "code",
"execution_count": 11,
"id": "f06a3958",
"metadata": {},
"outputs": [],
"source": [
"class HeaderMsg:\n",
" def __init__(self):\n",
" self.seq = 0\n",
" self.stamp = time.time()\n",
" self.frame_id = ''\n",
"\n",
"class PointCloud2Msg:\n",
" def __init__(self):\n",
" self.header = HeaderMsg()\n",
" self.height = 1080\n",
" self.width = 1280\n",
" self.fields = None\n",
" self.is_bigendian = None\n",
" self.point_step = None\n",
" self.row_step = None\n",
" self.data = [0]\n",
" self.is_dense = None\n",
"\n",
"class Vector3Msg:\n",
" def __init__(self):\n",
" self.x = None\n",
" self.y = None\n",
" self.z = None\n",
"\n",
"class QuaternionMsg:\n",
" def __init__(self):\n",
" self.x = None\n",
" self.y = None\n",
" self.z = None\n",
" self.w = None\n",
"\n",
"class TransformMsg:\n",
" def __init__(self):\n",
" self.translation = Vector3Msg()\n",
" self.rotation = QuaternionMsg()\n",
"\n",
"class TransformStampedMsg:\n",
" def __init__(self):\n",
" self.header = HeaderMsg()\n",
" self.child_frame_id = 'fake_link'\n",
" self.transform = TransformMsg()\n",
"\n",
"class TF2Manager:\n",
" def __init__(self):\n",
" pass\n",
" def lookup_transform(self, from_frame_id, to_frame_id, lookup_time, timeout):\n",
" return TransformStampedMsg()\n",
"\n",
"class Time:\n",
" def __init__(self):\n",
" pass\n",
" def now(self):\n",
" return time.time()\n",
"\n",
"class RospyManager:\n",
" def __init__(self):\n",
" self.Time = Time()\n",
"\n",
" def sleep(self, x):\n",
" print(f'via rospy.sleep: sleeping {x} seconds')\n",
" time.sleep(x)\n",
"\n",
" def loginfo(self, msg):\n",
" print('via rospy.loginfo: ' + msg)\n",
"\n",
"rospy = RospyManager()"
]
},
{
"cell_type": "code",
"execution_count": 12,
"execution_count": 10,
"id": "e889d16e",
"metadata": {},
"outputs": [],
@ -1090,7 +1008,7 @@
},
{
"cell_type": "code",
"execution_count": 13,
"execution_count": 11,
"id": "f5b4b144",
"metadata": {},
"outputs": [],
@ -1504,7 +1422,7 @@
},
{
"cell_type": "code",
"execution_count": 14,
"execution_count": 12,
"id": "92ad0117",
"metadata": {},
"outputs": [],
@ -1675,7 +1593,7 @@
},
{
"cell_type": "code",
"execution_count": 15,
"execution_count": 13,
"id": "37c0e390",
"metadata": {},
"outputs": [],
@ -1762,7 +1680,7 @@
},
{
"cell_type": "code",
"execution_count": 16,
"execution_count": 14,
"id": "d046c1d0",
"metadata": {},
"outputs": [],
@ -1838,7 +1756,7 @@
},
{
"cell_type": "code",
"execution_count": 17,
"execution_count": 15,
"id": "f5610c35",
"metadata": {},
"outputs": [],
@ -2108,7 +2026,7 @@
},
{
"cell_type": "code",
"execution_count": 19,
"execution_count": 16,
"id": "c67281e1",
"metadata": {},
"outputs": [
@ -2128,109 +2046,21 @@
"FakeRobot.move_to_pose: sleeping 1 second\n",
"Moving to {'joint_head_pan': -3.6}\n",
"FakeRobot.move_to_pose: sleeping 1 second\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",
"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",
"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",
"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",
"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",
"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",
"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",
"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"
"via rospy.loginfo: The head scan took 20.034795999526978 seconds.\n"
]
}
],
@ -2242,9 +2072,30 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 17,
"id": "7a0f7a53",
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"HeadScan: Saving to base_filename = /home/hello-robot/repos/stretch_tutorials/stretch_body/jupyter/example_head_scan\n",
"MaxHeightImage saving to base_filename = /home/hello-robot/repos/stretch_tutorials/stretch_body/jupyter/example_head_scan_mhi\n",
"Finished saving.\n",
"Finished saving.\n"
]
}
],
"source": [
"head_scan.save(\"/home/hello-robot/repos/stretch_tutorials/stretch_body/jupyter/example_head_scan\")"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "3fa6d347-1a0b-4303-b3f4-40a1d5dd3615",
"metadata": {},
"outputs": [],
"source": []
}

Write Preview
Loading…
Cancel
Save