| Name | stucco JSON |
| Version |
1.2.1
JSON |
| download |
| home_page | |
| Summary | Soft Tracking Using Contacts for Cluttered Objects |
| upload_time | 2023-12-15 19:12:17 |
| maintainer | |
| docs_url | None |
| author | |
| requires_python | >=3.6 |
| license | Copyright (c) 2023 University of Michigan ARM Lab Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. |
| keywords |
robotics
perception
tracking
pytorch
contact
|
| VCS |
 |
| bugtrack_url |
|
| requirements |
No requirements were recorded.
|
| Travis-CI |
No Travis.
|
| coveralls test coverage |
No coveralls.
|
# STUCCO
This is the official library code for the paper [Soft Tracking Using Contacts for Cluttered Objects (STUCCO) to Perform Blind Object Retrieval](https://ieeexplore.ieee.org/document/9696372).
If you use it, please cite
```
@article{zhong2022soft,
title={Soft tracking using contacts for cluttered objects to perform blind object retrieval},
author={Zhong, Sheng and Fazeli, Nima and Berenson, Dmitry},
journal={IEEE Robotics and Automation Letters},
volume={7},
number={2},
pages={3507--3514},
year={2022},
publisher={IEEE}
}
```
## Installation
```pip install stucco```
## Usage
This package is meant as a light-weight library for usage in your projects.
See the [website](https://johnsonzhong.me/projects/stucco/) for videos and a high level introduction.
To reproduce the results from the paper, see [stucco_experiments](https://github.com/UM-ARM-Lab/stucco_experiments).
This library provides code for both 1) contact detection and isolation, and 2) contact tracking. However, they can be
used independently of each other; i.e. you can supply the contact point manually to update the tracker instead of getting
it from the detector.
This section describes how to use each component, and provide implementation tips. The `pxpen` function measuring
distance between contact points and robot surfaces in given configurations need to be efficient, and we provide a guide
on how to implement them. The other key function, `pxdyn`, just needs to be callable with signature
```
(B x N x 3 points, B x N x SE(3) poses, B x N x se(3) change in poses) -> (B x N x 3 new points, B x N x SE(3) new poses)
```
Where `B` represent arbitrary batch dimension(s), `N` represent a number of contact points per step, some of which may
be missing or 1 and should behave under standard broadcasting rules.
### Contact Detection and Isolation
Detection and isolation uses the momentum observer. At high frequency, we get residual feedback that estimates applied
external wrench (force and torque) at the end effector. In simulation, we can get applied forces directly.
To manage the process, we have a `ContactDetector` object, created like:
```python
from stucco.detection import ContactDetector
from stucco.detection import ResidualPlanarContactSensor
import numpy as np
# sample points on the robot surface and the associated surface normals (your function)
# these should be in link frame
surface_points, surface_normals = get_robot_points()
# for end-effector force-torque residual, torque magnitudes are a lot smaller
# in sim without noise, select a precision to balance out the magnitudes
residual_precision = np.diag([1, 1, 1, 50, 50, 50])
residual_threshold = 3
# the Planar version is concretely implemented; a similar one could be implemented to handle more general cases
contact_detector = ContactDetector(residual_precision)
sensor = ResidualPlanarContactSensor(surface_points, surface_normals, residual_threshold)
contact_detector.register_contact_sensor(sensor)
```
You then feed this object high frequency residual data along with end-effector poses
```python
# get reaction force and reaction torque at end-effector
if contact_detector.observe_residual(np.r_[reaction_force, reaction_torque], pose):
contact_detector.observe_dx(dx)
# other book-keeping in case of making a contact
```
This object can later be queried like `contact_detector.in_contact()` and passed to update the tracking
### Contact Point Tracking
The tracking is performed through the `ContactSetSoft` object, created like:
```python
from stucco.tracking import ContactSetSoft, ContactParameters, LinearTranslationalDynamics
from stucco.movable_sdf import PlanarMovableSDF
# tune through maximizing median FMI and minimizing median contact error on a training set
contact_params = ContactParameters(length=0.02,
penetration_length=0.002,
hard_assignment_threshold=0.4,
intersection_tolerance=0.002)
# need an efficient implementation of pxpen; point to robot surface distance at a certain config
# see section below for how to implement one
# here we pass in a cached discretized signed distance field and its description
pxpen = PlanarMovableSDF(d_cache, min_x, min_y, max_x, max_y, cache_resolution, cache_y_len)
# pxdyn is LinearTranslationalDynamics by default, here we are making it explicit
contact_set = ContactSetSoft(pxpen, contact_params, pxdyn=LinearTranslationalDynamics())
```
You then update it every control step with robot pose and contact point info
```python
# get latest contact point through the contact detector
# (or can be supplied manually through other means)
# supplying None indicates we are not in contact
# also retrieve dx for each p
p, dx = contact_detector.get_last_contact_location()
# observed current x
contact_set.update(x, dx, p)
```
Segment the belief into hard assignments of objects for downstream usage:
```python
# MAP particle
pts = contact_set.get_posterior_points()
# contact parameters are stored in contact_set.p
# list of indices; each element of list corresponds to an object
groups = contact_set.get_hard_assignment(contact_set.p.hard_assignment_threshold)
for group in groups:
object_pts = pts[group]
# use points associated with the object downstream
```
### Implementing `pxpen` (point to robot surface distance)
Our recommendation for this function is to discretize and cache the signed distance function (SDF)
of the robot end effector in link frame. To support this, we provide the base class `PlanarPointToConfig` that supplies
all the other functionality when provided the SDF cache and accompanying information.
Here are some tips for how to create this discretized SDF:
```python
import os
import torch
import numpy as np
from stucco.movable_sdf import PlanarMovableSDF
# note that this is for a planar environment with fixed orientation;
# however, it is very easy to extend to 3D and free rotations;
# the extension to free rotations will require a parallel way to perform rigid body transforms
# on multiple points, which can be provided by pytorch_kinematics.transforms
class SamplePointToConfig(PlanarMovableSDF):
def __init__(self):
# save cache to file for easy loading (use your own path)
fullname = 'sample_point_to_config.pkl'
if os.path.exists(fullname):
super().__init__(*torch.load(fullname))
else:
# first time creating cache
# we need some environment where we can get its bounding box and query an SDF
# create robot in simulation (use your own function)
robot_id, gripper_id, pos = create_sim_robot()
# get axis-aligned bounding box values
aabb_min, aabb_max = get_aabb()
min_x, min_y = aabb_min[:2]
max_x, max_y = aabb_max[:2]
# select a cache resolution (doesn't have to be very small)
cache_resolution = 0.001
# create mesh grid
x = np.arange(min_x, max_x + cache_resolution, cache_resolution)
y = np.arange(min_y, max_y + cache_resolution, cache_resolution)
cache_y_len = len(y)
d = np.zeros((len(x), len(y)))
for i, xi in enumerate(x):
for j, yj in enumerate(y):
pt = [xi, yj, pos[2]]
# point query of SDF (use your own function)
d[i, j] = closest_point_on_surface(robot_id, pt)
# flatten to allow parallel query of multiple indices
d_cache = d.reshape(-1)
# save things in (rotated) link frame
min_x -= pos[0]
max_x -= pos[0]
min_y -= pos[1]
max_y -= pos[1]
data = [d_cache, min_x, min_y, max_x, max_y, cache_resolution, cache_y_len]
torch.save(data, fullname)
super().__init__(*data)
```
Raw data
{
"_id": null,
"home_page": "",
"name": "stucco",
"maintainer": "",
"docs_url": null,
"requires_python": ">=3.6",
"maintainer_email": "Sheng Zhong <zhsh@umich.edu>",
"keywords": "robotics,perception,tracking,pytorch,contact",
"author": "",
"author_email": "Sheng Zhong <zhsh@umich.edu>",
"download_url": "https://files.pythonhosted.org/packages/fc/80/7da9f512a37b94106c9aa68e4a7f3ab7b52fcfa2425b70653d4da8479199/stucco-1.2.1.tar.gz",
"platform": null,
"description": "# STUCCO\nThis is the official library code for the paper [Soft Tracking Using Contacts for Cluttered Objects (STUCCO) to Perform Blind Object Retrieval](https://ieeexplore.ieee.org/document/9696372).\nIf you use it, please cite\n\n```\n@article{zhong2022soft,\n title={Soft tracking using contacts for cluttered objects to perform blind object retrieval},\n author={Zhong, Sheng and Fazeli, Nima and Berenson, Dmitry},\n journal={IEEE Robotics and Automation Letters},\n volume={7},\n number={2},\n pages={3507--3514},\n year={2022},\n publisher={IEEE}\n}\n```\n\n## Installation\n```pip install stucco```\n\n\n## Usage\nThis package is meant as a light-weight library for usage in your projects. \nSee the [website](https://johnsonzhong.me/projects/stucco/) for videos and a high level introduction.\nTo reproduce the results from the paper, see [stucco_experiments](https://github.com/UM-ARM-Lab/stucco_experiments).\n\nThis library provides code for both 1) contact detection and isolation, and 2) contact tracking. However, they can be\nused independently of each other; i.e. you can supply the contact point manually to update the tracker instead of getting\nit from the detector.\n\nThis section describes how to use each component, and provide implementation tips. The `pxpen` function measuring\ndistance between contact points and robot surfaces in given configurations need to be efficient, and we provide a guide\non how to implement them. The other key function, `pxdyn`, just needs to be callable with signature\n\n```\n(B x N x 3 points, B x N x SE(3) poses, B x N x se(3) change in poses) -> (B x N x 3 new points, B x N x SE(3) new poses)\n```\n\nWhere `B` represent arbitrary batch dimension(s), `N` represent a number of contact points per step, some of which may\nbe missing or 1 and should behave under standard broadcasting rules.\n\n### Contact Detection and Isolation\n\nDetection and isolation uses the momentum observer. At high frequency, we get residual feedback that estimates applied\nexternal wrench (force and torque) at the end effector. In simulation, we can get applied forces directly.\n\nTo manage the process, we have a `ContactDetector` object, created like:\n\n```python\nfrom stucco.detection import ContactDetector\nfrom stucco.detection import ResidualPlanarContactSensor\nimport numpy as np\n\n# sample points on the robot surface and the associated surface normals (your function)\n# these should be in link frame\nsurface_points, surface_normals = get_robot_points()\n\n# for end-effector force-torque residual, torque magnitudes are a lot smaller\n# in sim without noise, select a precision to balance out the magnitudes\nresidual_precision = np.diag([1, 1, 1, 50, 50, 50])\nresidual_threshold = 3\n\n# the Planar version is concretely implemented; a similar one could be implemented to handle more general cases\ncontact_detector = ContactDetector(residual_precision)\nsensor = ResidualPlanarContactSensor(surface_points, surface_normals, residual_threshold)\ncontact_detector.register_contact_sensor(sensor)\n```\n\nYou then feed this object high frequency residual data along with end-effector poses\n\n```python\n# get reaction force and reaction torque at end-effector \nif contact_detector.observe_residual(np.r_[reaction_force, reaction_torque], pose):\n contact_detector.observe_dx(dx)\n # other book-keeping in case of making a contact\n```\n\nThis object can later be queried like `contact_detector.in_contact()` and passed to update the tracking\n\n### Contact Point Tracking\n\nThe tracking is performed through the `ContactSetSoft` object, created like:\n\n```python\nfrom stucco.tracking import ContactSetSoft, ContactParameters, LinearTranslationalDynamics\nfrom stucco.movable_sdf import PlanarMovableSDF\n\n# tune through maximizing median FMI and minimizing median contact error on a training set\ncontact_params = ContactParameters(length=0.02,\n penetration_length=0.002,\n hard_assignment_threshold=0.4,\n intersection_tolerance=0.002)\n\n# need an efficient implementation of pxpen; point to robot surface distance at a certain config\n# see section below for how to implement one\n# here we pass in a cached discretized signed distance field and its description\npxpen = PlanarMovableSDF(d_cache, min_x, min_y, max_x, max_y, cache_resolution, cache_y_len)\n\n# pxdyn is LinearTranslationalDynamics by default, here we are making it explicit\ncontact_set = ContactSetSoft(pxpen, contact_params, pxdyn=LinearTranslationalDynamics())\n```\n\nYou then update it every control step with robot pose and contact point info\n\n```python\n# get latest contact point through the contact detector \n# (or can be supplied manually through other means)\n# supplying None indicates we are not in contact\n# also retrieve dx for each p\np, dx = contact_detector.get_last_contact_location()\n# observed current x\ncontact_set.update(x, dx, p)\n```\n\nSegment the belief into hard assignments of objects for downstream usage:\n\n```python\n# MAP particle\npts = contact_set.get_posterior_points()\n# contact parameters are stored in contact_set.p\n# list of indices; each element of list corresponds to an object\ngroups = contact_set.get_hard_assignment(contact_set.p.hard_assignment_threshold)\n\nfor group in groups:\n object_pts = pts[group]\n # use points associated with the object downstream\n```\n\n### Implementing `pxpen` (point to robot surface distance)\n\nOur recommendation for this function is to discretize and cache the signed distance function (SDF)\nof the robot end effector in link frame. To support this, we provide the base class `PlanarPointToConfig` that supplies\nall the other functionality when provided the SDF cache and accompanying information.\n\nHere are some tips for how to create this discretized SDF:\n\n```python\nimport os\nimport torch\nimport numpy as np\nfrom stucco.movable_sdf import PlanarMovableSDF\n\n\n# note that this is for a planar environment with fixed orientation; \n# however, it is very easy to extend to 3D and free rotations; \n# the extension to free rotations will require a parallel way to perform rigid body transforms \n# on multiple points, which can be provided by pytorch_kinematics.transforms\nclass SamplePointToConfig(PlanarMovableSDF):\n def __init__(self):\n # save cache to file for easy loading (use your own path)\n fullname = 'sample_point_to_config.pkl'\n if os.path.exists(fullname):\n super().__init__(*torch.load(fullname))\n else:\n # first time creating cache\n # we need some environment where we can get its bounding box and query an SDF\n # create robot in simulation (use your own function)\n robot_id, gripper_id, pos = create_sim_robot()\n # get axis-aligned bounding box values\n aabb_min, aabb_max = get_aabb()\n min_x, min_y = aabb_min[:2]\n max_x, max_y = aabb_max[:2]\n\n # select a cache resolution (doesn't have to be very small)\n cache_resolution = 0.001\n # create mesh grid\n x = np.arange(min_x, max_x + cache_resolution, cache_resolution)\n y = np.arange(min_y, max_y + cache_resolution, cache_resolution)\n cache_y_len = len(y)\n\n d = np.zeros((len(x), len(y)))\n for i, xi in enumerate(x):\n for j, yj in enumerate(y):\n pt = [xi, yj, pos[2]]\n # point query of SDF (use your own function)\n d[i, j] = closest_point_on_surface(robot_id, pt)\n # flatten to allow parallel query of multiple indices\n d_cache = d.reshape(-1)\n # save things in (rotated) link frame\n min_x -= pos[0]\n max_x -= pos[0]\n min_y -= pos[1]\n max_y -= pos[1]\n data = [d_cache, min_x, min_y, max_x, max_y, cache_resolution, cache_y_len]\n torch.save(data, fullname)\n super().__init__(*data)\n```\n",
"bugtrack_url": null,
"license": "Copyright (c) 2023 University of Michigan ARM Lab Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the \"Software\"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. ",
"summary": "Soft Tracking Using Contacts for Cluttered Objects",
"version": "1.2.1",
"project_urls": {
"Bug Reports": "https://github.com/UM-ARM-Lab/stucco/issues",
"Homepage": "https://github.com/UM-ARM-Lab/stucco",
"Source": "https://github.com/UM-ARM-Lab/stucco"
},
"split_keywords": [
"robotics",
"perception",
"tracking",
"pytorch",
"contact"
],
"urls": [
{
"comment_text": "",
"digests": {
"blake2b_256": "2f66d03ed3a31c2de572f59c4476f9c71f3a4117b97e98d4e452117e80790ab1",
"md5": "c61bca497201a25a7fb02446455918ce",
"sha256": "9ad72bab5bdc34d6891cb4a15fd5c894312cf7d5c17f5339c7d24a1108ee8595"
},
"downloads": -1,
"filename": "stucco-1.2.1-py3-none-any.whl",
"has_sig": false,
"md5_digest": "c61bca497201a25a7fb02446455918ce",
"packagetype": "bdist_wheel",
"python_version": "py3",
"requires_python": ">=3.6",
"size": 16365,
"upload_time": "2023-12-15T19:12:15",
"upload_time_iso_8601": "2023-12-15T19:12:15.957259Z",
"url": "https://files.pythonhosted.org/packages/2f/66/d03ed3a31c2de572f59c4476f9c71f3a4117b97e98d4e452117e80790ab1/stucco-1.2.1-py3-none-any.whl",
"yanked": false,
"yanked_reason": null
},
{
"comment_text": "",
"digests": {
"blake2b_256": "fc807da9f512a37b94106c9aa68e4a7f3ab7b52fcfa2425b70653d4da8479199",
"md5": "d1253a0159aba2beafa937ca19e09bf8",
"sha256": "07f0505565f6548a63512694b921cbbd4a6a0db0dc5c0ad16abf2406d60fcdc3"
},
"downloads": -1,
"filename": "stucco-1.2.1.tar.gz",
"has_sig": false,
"md5_digest": "d1253a0159aba2beafa937ca19e09bf8",
"packagetype": "sdist",
"python_version": "source",
"requires_python": ">=3.6",
"size": 22384,
"upload_time": "2023-12-15T19:12:17",
"upload_time_iso_8601": "2023-12-15T19:12:17.300263Z",
"url": "https://files.pythonhosted.org/packages/fc/80/7da9f512a37b94106c9aa68e4a7f3ab7b52fcfa2425b70653d4da8479199/stucco-1.2.1.tar.gz",
"yanked": false,
"yanked_reason": null
}
],
"upload_time": "2023-12-15 19:12:17",
"github": true,
"gitlab": false,
"bitbucket": false,
"codeberg": false,
"github_user": "UM-ARM-Lab",
"github_project": "stucco",
"travis_ci": false,
"coveralls": false,
"github_actions": false,
"lcname": "stucco"
}
