{"id":19134006,"url":"https://github.com/um-arm-lab/stucco","last_synced_at":"2025-05-06T19:25:51.184Z","repository":{"id":41138141,"uuid":"406548529","full_name":"UM-ARM-Lab/stucco","owner":"UM-ARM-Lab","description":"Soft Tracking Using Contacts for Cluttered Objects","archived":false,"fork":false,"pushed_at":"2024-06-13T22:23:06.000Z","size":78096,"stargazers_count":3,"open_issues_count":0,"forks_count":0,"subscribers_count":10,"default_branch":"master","last_synced_at":"2025-05-06T19:25:45.139Z","etag":null,"topics":["perception","pytorch","robotics","tracking"],"latest_commit_sha":null,"homepage":"https://johnsonzhong.me/projects/stucco/","language":"Python","has_issues":true,"has_wiki":null,"has_pages":null,"mirror_url":null,"source_name":null,"license":"mit","status":null,"scm":"git","pull_requests_enabled":true,"icon_url":"https://github.com/UM-ARM-Lab.png","metadata":{"files":{"readme":"README.md","changelog":null,"contributing":null,"funding":null,"license":"LICENSE.txt","code_of_conduct":null,"threat_model":null,"audit":null,"citation":null,"codeowners":null,"security":null,"support":null,"governance":null,"roadmap":null,"authors":null,"dei":null,"publiccode":null,"codemeta":null}},"created_at":"2021-09-14T23:19:36.000Z","updated_at":"2023-12-16T00:01:16.000Z","dependencies_parsed_at":"2024-11-09T06:25:52.868Z","dependency_job_id":"a2598de7-c2ca-45ca-95af-4ba760f430d3","html_url":"https://github.com/UM-ARM-Lab/stucco","commit_stats":{"total_commits":493,"total_committers":1,"mean_commits":493.0,"dds":0.0,"last_synced_commit":"007e9e872f13a1451f84d1aea8aff522bd09fa21"},"previous_names":[],"tags_count":0,"template":false,"template_full_name":null,"repository_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/UM-ARM-Lab%2Fstucco","tags_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/UM-ARM-Lab%2Fstucco/tags","releases_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/UM-ARM-Lab%2Fstucco/releases","manifests_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/UM-ARM-Lab%2Fstucco/manifests","owner_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners/UM-ARM-Lab","download_url":"https://codeload.github.com/UM-ARM-Lab/stucco/tar.gz/refs/heads/master","host":{"name":"GitHub","url":"https://github.com","kind":"github","repositories_count":252752161,"owners_count":21798743,"icon_url":"https://github.com/github.png","version":null,"created_at":"2022-05-30T11:31:42.601Z","updated_at":"2022-07-04T15:15:14.044Z","host_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub","repositories_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories","repository_names_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repository_names","owners_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners"}},"keywords":["perception","pytorch","robotics","tracking"],"created_at":"2024-11-09T06:24:42.362Z","updated_at":"2025-05-06T19:25:51.130Z","avatar_url":"https://github.com/UM-ARM-Lab.png","language":"Python","funding_links":[],"categories":[],"sub_categories":[],"readme":"# 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) -\u003e (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```","project_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fum-arm-lab%2Fstucco","html_url":"https://awesome.ecosyste.ms/projects/github.com%2Fum-arm-lab%2Fstucco","lists_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fum-arm-lab%2Fstucco/lists"}