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

Real data added to head scanning - not yet working

pull/16/head
Binit Shah 1 year ago
parent
120a6b5179
commit
4e48da0626
6 changed files with 252 additions and 237 deletions
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +1
    -1
      stretch_body/jupyter/example_head_scan.yaml
  2. BIN
     
  3. BIN
     
  4. BIN
     
  5. BIN
     
  6. +251
    -236
      stretch_body/jupyter/funmap_experiments.ipynb

+ 1
- 1
stretch_body/jupyter/example_head_scan.yaml View File

@ -105,4 +105,4 @@ robot_ang_rad: -0.0
robot_xy_pix:
- 1331.3333333333333
- 1331.3333333333333
timestamp: 1681960427.1463504
timestamp: 1683802162.1055086

BIN
View File


BIN
View File


BIN
View File


BIN
View File


+ 251
- 236
stretch_body/jupyter/funmap_experiments.ipynb View File

@ -21,19 +21,33 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 1,
"id": "3f861f1c",
"metadata": {
"tags": []
},
"outputs": [],
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"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-yq8q8atk\n",
" Running command git clone --filter=blob:none --quiet https://github.com/safijari/tiny_tf.git /tmp/pip-req-build-yq8q8atk\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.23.5)\n"
]
}
],
"source": [
"!pip install git+https://github.com/safijari/tiny_tf.git"
]
},
{
"cell_type": "code",
"execution_count": 1,
"execution_count": 2,
"id": "e870dd4a",
"metadata": {
"tags": []
@ -79,12 +93,108 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 3,
"id": "7460aa2c",
"metadata": {
"tags": []
},
"outputs": [],
"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"
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"Hardstop detected at motor position (rad) 98.10670471191406\n",
"Marking Lift position to 1.097231 (m)\n",
"Marking Lift position to 0.000000 (m)\n",
"Lift homing successful\n",
"--------- Homing Arm ----\n",
"Homing Arm...\n",
"Hardstop detected at motor position (rad) 1.6194909811019897\n",
"Marking Arm position to 0.000000 (m)\n"
]
},
{
"name": "stderr",
"output_type": "stream",
"text": [
"[INFO] [robot_monitor]: Guarded contact arm\n",
"[INFO] [robot_monitor]: Wrist single tap: 15\n",
"[INFO] [robot_monitor]: Wrist single tap: 16\n"
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"Arm homing successful\n",
"Moving to first hardstop...\n",
"First hardstop contact at position (ticks): -25\n",
"-----\n",
"Homing offset was 1488\n",
"Marking current position to zero ticks\n",
"Homing offset is now 1510 (ticks)\n",
"-----\n",
"Current position (ticks): 40\n",
"Moving to calibrated zero: (rad)\n",
"Moving to first hardstop...\n",
"First hardstop contact at position (ticks): -26\n",
"-----\n",
"Homing offset was 2249\n",
"Marking current position to zero ticks\n",
"Homing offset is now 2269 (ticks)\n",
"-----\n",
"Current position (ticks): 62\n",
"Moving to calibrated zero: (rad)\n"
]
},
{
"name": "stderr",
"output_type": "stream",
"text": [
"[INFO] [robot_monitor]: Base bump event\n"
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"--------- Stowing Arm ----\n",
"Warning: Rate of calls to Pimu:trigger_motor_sync rate of 148.350157 above maximum frequency of 20.00 Hz. Motor commands dropped: 1\n"
]
},
{
"name": "stderr",
"output_type": "stream",
"text": [
"[INFO] [robot_monitor]: Base bump event\n",
"[INFO] [robot_monitor]: Wrist single tap: 17\n"
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"--------- Stowing ToolStretchDexWrist ----\n",
"--------- Stowing Lift ----\n"
]
}
],
"source": [
"# Setup the Python API to Stretch\n",
"robot = Robot()\n",
@ -100,12 +210,21 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 4,
"id": "033774e1",
"metadata": {
"tags": []
},
"outputs": [],
"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 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",
@ -122,7 +241,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 5,
"id": "96f891c9",
"metadata": {
"tags": []
@ -179,16 +298,6 @@
" }"
]
},
{
"cell_type": "markdown",
"id": "866c112b-86b7-4f0e-8b68-90cf8d0b3813",
"metadata": {
"tags": []
},
"source": [
"## Realsense"
]
},
{
"cell_type": "markdown",
"id": "e5782197-e4fd-48e6-be71-050e7318ed8e",
@ -199,7 +308,7 @@
},
{
"cell_type": "code",
"execution_count": 2,
"execution_count": 6,
"id": "221818a7-11d1-440d-86d8-b71e521b5487",
"metadata": {
"tags": []
@ -208,10 +317,10 @@
{
"data": {
"text/plain": [
"<pyrealsense2.pyrealsense2.pipeline_profile at 0x7f1235ee51f0>"
"<pyrealsense2.pyrealsense2.pipeline_profile at 0x7ff540ff5630>"
]
},
"execution_count": 2,
"execution_count": 6,
"metadata": {},
"output_type": "execute_result"
}
@ -221,6 +330,8 @@
"resolution_color = [1280, 720] # [1920, 1080], [1280, 720], [640, 480]\n",
"resolution_depth = [1280, 720] # [1280, 720], [640, 480]\n",
"\n",
"pc = rs.pointcloud()\n",
"align = rs.align(rs.stream.depth)\n",
"pipeline = rs.pipeline()\n",
"config = rs.config()\n",
"config.enable_stream(rs.stream.depth, resolution_depth[0], resolution_depth[1], rs.format.z16, fps_color)\n",
@ -228,176 +339,6 @@
"pipeline.start(config)"
]
},
{
"cell_type": "code",
"execution_count": 4,
"id": "43472f59-9907-4e7f-8cc8-31ee4a129b79",
"metadata": {
"tags": []
},
"outputs": [],
"source": [
"pc = rs.pointcloud()\n",
"align = rs.align(rs.stream.depth)"
]
},
{
"cell_type": "code",
"execution_count": 37,
"id": "b881bc97-71da-4cbb-a452-ff135ff6dd63",
"metadata": {
"tags": []
},
"outputs": [],
"source": [
"# Get the latest frames from the camera\n",
"frames = pipeline.wait_for_frames()\n",
"aligned = align.process(frames)\n",
"depth_frame = frames.get_depth_frame()\n",
"color_aligned_to_depth_frame = aligned.first(rs.stream.color)"
]
},
{
"cell_type": "code",
"execution_count": 38,
"id": "18115b02-d2d2-4f04-a203-ea196c637862",
"metadata": {
"tags": []
},
"outputs": [],
"source": [
"# Convert images to numpy arrays\n",
"depth_image = np.asanyarray(depth_frame.get_data())\n",
"color_image = np.asanyarray(color_aligned_to_depth_frame.get_data())"
]
},
{
"cell_type": "code",
"execution_count": 39,
"id": "335d7679-bda4-4f0d-a086-b40e42e78804",
"metadata": {
"tags": []
},
"outputs": [],
"source": [
"points = pc.calculate(depth_frame)\n",
"pc.map_to(color_aligned_to_depth_frame)"
]
},
{
"cell_type": "code",
"execution_count": 40,
"id": "962dfa2f-1090-45a1-ace5-f7ccc58462b9",
"metadata": {
"tags": []
},
"outputs": [],
"source": [
"v = points.get_vertices()\n",
"xyz = np.asanyarray(v).view(np.float32).reshape(-1, 3) # xyz"
]
},
{
"cell_type": "code",
"execution_count": 41,
"id": "c00858fc-7067-4b58-bc0f-4282725a0f89",
"metadata": {
"tags": []
},
"outputs": [],
"source": [
"t = points.get_texture_coordinates()\n",
"texcoords = np.asanyarray(t).view(np.float32).reshape(-1, 2) # uv\n",
"cw, ch = color_image.shape[:2][::-1]\n",
"v, u = (texcoords * (cw, ch) + 0.5).astype(np.uint32).T\n",
"np.clip(u, 0, ch-1, out=u)\n",
"np.clip(v, 0, cw-1, out=v)\n",
"rgb = color_image[u, v] # rgb"
]
},
{
"cell_type": "code",
"execution_count": 42,
"id": "55cd4bc4-1ae8-48bf-a649-d0e93203daf4",
"metadata": {
"tags": []
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Frame #: 11631\n",
"Timestamp: 1683796286702.8328\n",
"Num points: 921600\n",
"(921600, 3)\n",
"(921600, 3)\n"
]
}
],
"source": [
"print(f'Frame #: {aligned.get_frame_number()}')\n",
"print(f'Timestamp: {aligned.timestamp}')\n",
"print(f'Num points: {points.size()}')\n",
"print(xyz.shape)\n",
"print(rgb.shape)"
]
},
{
"cell_type": "code",
"execution_count": 43,
"id": "b727f246-b611-4713-9c15-f1eeb80dff16",
"metadata": {
"tags": []
},
"outputs": [],
"source": [
"points_arr = np.zeros((points.size(),), dtype=[\n",
" ('x', np.float32),\n",
" ('y', np.float32),\n",
" ('z', np.float32),\n",
" ('r', np.uint8),\n",
" ('g', np.uint8),\n",
" ('b', np.uint8)])\n",
"points_arr['x'] = xyz[:, 0]\n",
"points_arr['y'] = xyz[:, 1]\n",
"points_arr['z'] = xyz[:, 2]\n",
"points_arr['r'] = rgb[:, 0]\n",
"points_arr['g'] = rgb[:, 1]\n",
"points_arr['b'] = rgb[:, 2]"
]
},
{
"cell_type": "code",
"execution_count": 44,
"id": "d236b294-b03c-4bb3-8fe7-a1bd745c4682",
"metadata": {
"tags": []
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"done\n"
]
}
],
"source": [
"pc_msg = ros_numpy.msgify(PointCloud2, points_arr)\n",
"print('done')"
]
},
{
"cell_type": "markdown",
"id": "241ea842-c238-4099-91cd-42e19ef0409e",
"metadata": {
"tags": []
},
"source": [
"## temp"
]
},
{
"cell_type": "markdown",
"id": "490e9415",
@ -408,7 +349,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 7,
"id": "c443b5c1",
"metadata": {
"tags": []
@ -420,24 +361,11 @@
" 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.point_cloud = None\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.float32)])\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",
@ -452,8 +380,48 @@
" 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",
" # 1. Get the latest frames from the camera\n",
" frames = pipeline.wait_for_frames()\n",
" aligned = align.process(frames)\n",
" depth_frame = frames.get_depth_frame()\n",
" color_aligned_to_depth_frame = aligned.first(rs.stream.color)\n",
"\n",
" # 2. Create point cloud from depth image\n",
" points = pc.calculate(depth_frame)\n",
" pc.map_to(color_aligned_to_depth_frame)\n",
"\n",
" # 3. Create xyz matrix\n",
" v = points.get_vertices()\n",
" xyz = np.asanyarray(v).view(np.float32).reshape(-1, 3) # xyz\n",
"\n",
" # 4. Create rgb matrix\n",
" color_image = np.asanyarray(color_aligned_to_depth_frame.get_data())\n",
" t = points.get_texture_coordinates()\n",
" texcoords = np.asanyarray(t).view(np.float32).reshape(-1, 2) # uv\n",
" cw, ch = color_image.shape[:2][::-1]\n",
" v, u = (texcoords * (cw, ch) + 0.5).astype(np.uint32).T\n",
" np.clip(u, 0, ch-1, out=u)\n",
" np.clip(v, 0, cw-1, out=v)\n",
" rgb = color_image[u, v] # rgb\n",
"\n",
" # 5. Create point cloud matrix\n",
" points_arr = np.zeros((points.size(),), dtype=[\n",
" ('x', np.float32),\n",
" ('y', np.float32),\n",
" ('z', np.float32),\n",
" ('r', np.uint8),\n",
" ('g', np.uint8),\n",
" ('b', np.uint8)])\n",
" points_arr['x'] = xyz[:, 0]\n",
" points_arr['y'] = xyz[:, 1]\n",
" points_arr['z'] = xyz[:, 2]\n",
" points_arr['r'] = rgb[:, 2]\n",
" points_arr['g'] = rgb[:, 1]\n",
" points_arr['b'] = rgb[:, 0]\n",
" points_arr = ros_numpy.point_cloud2.merge_rgb_fields(points_arr)\n",
"\n",
" # 6. Create PointCloud2 message\n",
" self.point_cloud = ros_numpy.msgify(PointCloud2, points_arr, stamp=time.time(), 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",
@ -491,7 +459,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 8,
"id": "228b8825-320a-442b-827c-4aede6e4e644",
"metadata": {
"tags": []
@ -581,7 +549,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 9,
"id": "e2396e2f",
"metadata": {
"tags": []
@ -726,7 +694,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 10,
"id": "1fed3d42",
"metadata": {
"tags": []
@ -749,7 +717,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 11,
"id": "e889d16e",
"metadata": {
"tags": []
@ -1227,7 +1195,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 12,
"id": "f5b4b144",
"metadata": {
"tags": []
@ -1643,7 +1611,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 13,
"id": "92ad0117",
"metadata": {
"tags": []
@ -1816,7 +1784,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 14,
"id": "37c0e390",
"metadata": {
"tags": []
@ -1905,7 +1873,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 15,
"id": "d046c1d0",
"metadata": {
"tags": []
@ -1983,7 +1951,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 16,
"id": "f5610c35",
"metadata": {
"tags": []
@ -2249,7 +2217,6 @@
"cell_type": "markdown",
"id": "5f5a7ef5-852d-43e6-82ce-1d21b286e5ad",
"metadata": {
"jp-MarkdownHeadingCollapsed": true,
"tags": []
},
"source": [
@ -2258,12 +2225,46 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 17,
"id": "c67281e1",
"metadata": {
"tags": []
},
"outputs": [],
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"MaxHeightImage information:\n",
" image.shape = (2667, 2667)\n",
" image.dtype = uint8\n",
" m_per_pix = 0.006\n",
" m_per_height_unit = 0.0046456692913385824\n",
" voi.x_in_m = 16.0\n",
" voi.y_in_m = 16.0\n",
" voi.z_in_m = 1.18\n",
"Moving to {'joint_head_pan': -3.6, 'joint_head_tilt': -0.8}\n",
"FakeRobot.move_to_pose: sleeping 1 second\n",
"Moving to {'joint_head_pan': -3.6}\n",
"FakeRobot.move_to_pose: sleeping 1 second\n",
"Moving to {'joint_head_pan': -2.7583333333333333}\n",
"FakeRobot.move_to_pose: sleeping 1 second\n",
"Moving to {'joint_head_pan': -1.9166666666666667}\n",
"FakeRobot.move_to_pose: sleeping 1 second\n",
"Moving to {'joint_head_pan': -1.0750000000000002}\n",
"FakeRobot.move_to_pose: sleeping 1 second\n",
"Moving to {'joint_head_pan': -0.2333333333333334}\n",
"FakeRobot.move_to_pose: sleeping 1 second\n",
"Moving to {'joint_head_pan': 0.608333333333333}\n",
"FakeRobot.move_to_pose: sleeping 1 second\n",
"Moving to {'joint_head_pan': 1.45}\n",
"FakeRobot.move_to_pose: sleeping 1 second\n",
"Moving to {'joint_head_pan': 0.1, 'joint_head_tilt': -1.2}\n",
"FakeRobot.move_to_pose: sleeping 1 second\n",
"via rospy.loginfo: The head scan took 25.813185453414917 seconds.\n"
]
}
],
"source": [
"node = FakeNode()\n",
"head_scan = HeadScan(voi_side_m=16.0)\n",
@ -2272,34 +2273,48 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 18,
"id": "7a0f7a53",
"metadata": {
"tags": []
},
"outputs": [],
"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": {
"tags": []
},
"outputs": [],
"source": [
"node.point_cloud"
]
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 20,
"id": "3cd042ff-9ddb-4276-ab8c-a1210bd1f866",
"metadata": {},
"outputs": [],
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"Exception in thread Thread-7:\n",
"Traceback (most recent call last):\n",
" File \"/usr/lib/python3.8/threading.py\", line 932, in _bootstrap_inner\n",
" self.run()\n",
" File \"/usr/lib/python3.8/threading.py\", line 870, in run\n",
" self._target(*self._args, **self._kwargs)\n",
" File \"/tmp/ipykernel_16300/3221415467.py\", line 26, in _spin\n",
"RuntimeError: wait_for_frames cannot be called before start()\n"
]
}
],
"source": [
"robot.stop()\n",
"pipeline.stop()"

Write Preview
Loading…
Cancel
Save