Instructions to use INSAIT-Institute/LADDER-RoboLab-Long with libraries, inference providers, notebooks, and local apps. Follow these links to get started.
- Libraries
- LeRobot
How to use INSAIT-Institute/LADDER-RoboLab-Long with LeRobot:
# No code snippets available yet for this library. # To use this model, check the repository files and the library's documentation. # Want to help? PRs adding snippets are welcome at: # https://github.com/huggingface/huggingface.js
- Notebooks
- Google Colab
- Kaggle
Download ladder/components/inference/api/core/privileged_geometry.py from INSAIT-Institute/LADDER-RoboLab-Long: direct link, hf CLI and curl.
- Browser
- Download file 10.1 kB
-
https://huggingface.co/INSAIT-Institute/LADDER-RoboLab-Long/resolve/main/ladder/components/inference/api/core/privileged_geometry.py
- Command line
-
hf download hf://INSAIT-Institute/LADDER-RoboLab-Long/ladder/components/inference/api/core/privileged_geometry.py
-
curl -L -o privileged_geometry.py https://huggingface.co/INSAIT-Institute/LADDER-RoboLab-Long/resolve/main/ladder/components/inference/api/core/privileged_geometry.py
10.1 kB
| from typing import Any, Dict, List, Optional, Sequence, Tuple | |
| import numpy as np | |
| from ladder.components.inference.api.core.privileged_state import ( | |
| encode_condition, | |
| encode_object, | |
| encode_state, | |
| ) | |
| _NEIGHBOUR_GAP_M = 0.005 # so two objects sharing a destination do not touch | |
| _IN_CLEARANCE_M = 0.005 # drop height above a container's floor | |
| def to_base(world_to_base: np.ndarray, points: np.ndarray) -> np.ndarray: | |
| """Map world-frame points ``[..., 3]`` into the robot base frame the policy commands in.""" | |
| flat = np.asarray(points, dtype=np.float64).reshape(-1, 3) | |
| homogeneous = np.concatenate([flat, np.ones((len(flat), 1))], axis=1) | |
| return (homogeneous @ np.asarray(world_to_base, dtype=np.float64).T)[:, :3].reshape(points.shape) | |
| def spread_offsets( | |
| half_extents: Sequence[np.ndarray], dest_half: np.ndarray | |
| ) -> Tuple[np.ndarray, np.ndarray]: | |
| """Where to lay N objects on one destination, as XY offsets plus the layer each one sits in. | |
| Piling is a real failure mode: every arrival dropped on the same point knocks the last one off. | |
| The constraint is each object's FOOTPRINT, not its centre. A grid that only keeps centres | |
| inside the destination still aims every object so its own body overhangs the rim, which for a | |
| container means setting it down on the wall, where a delivery is credited and then lost. | |
| When a grid sized for the largest object cannot hold them all, each object's own footprint is | |
| laid in rows along the destination's longer side instead, and what still does not fit piles on | |
| the floor. Stacked layers are avoided because they aim each later arrival higher above the | |
| destination, and an object dropped from that height bounces out of a container. | |
| """ | |
| count = len(half_extents) | |
| offsets = np.zeros((count, 2)) | |
| layers = np.zeros(count, dtype=int) | |
| if count <= 1: | |
| return offsets, layers | |
| source_half = np.array([max(h[0] for h in half_extents), max(h[1] for h in half_extents)]) | |
| step = 2.0 * source_half + _NEIGHBOUR_GAP_M | |
| # The centre of an object at the extreme cell must sit this far in for its edge to clear the rim. | |
| usable = np.maximum(np.asarray(dest_half[:2], dtype=np.float64) - _NEIGHBOUR_GAP_M - source_half, 0.0) | |
| cells = np.maximum(np.floor(2.0 * usable / step).astype(int) + 1, 1) | |
| per_layer = int(cells[0] * cells[1]) | |
| if count > per_layer: | |
| return _row_offsets(half_extents, np.asarray(dest_half[:2], dtype=np.float64)), layers | |
| for index in range(count): | |
| cell, layers[index] = index % per_layer, index // per_layer | |
| grid_xy = np.array([cell % cells[0], cell // cells[0]], dtype=np.float64) | |
| offsets[index] = (grid_xy - (cells - 1) / 2.0) * step | |
| return offsets, layers | |
| def _row_offsets(half_extents: Sequence[np.ndarray], dest_half: np.ndarray) -> np.ndarray: | |
| """XY offsets laying each object's own footprint in rows along ``dest_half``'s longer axis.""" | |
| along = 0 if dest_half[0] >= dest_half[1] else 1 | |
| across = 1 - along | |
| length = 2.0 * float(dest_half[along]) - _NEIGHBOUR_GAP_M | |
| rows: List[List[int]] = [[]] | |
| used = 0.0 | |
| for index, half in enumerate(half_extents): | |
| width = 2.0 * float(half[along]) + _NEIGHBOUR_GAP_M | |
| if rows[-1] and used + width > length: | |
| rows.append([]) | |
| used = 0.0 | |
| rows[-1].append(index) | |
| used += width | |
| depths = [max(2.0 * float(half_extents[i][across]) for i in row) + _NEIGHBOUR_GAP_M for row in rows] | |
| offsets = np.zeros((len(half_extents), 2)) | |
| row_start = -0.5 * sum(depths) | |
| for row, depth in zip(rows, depths, strict=True): | |
| widths = [2.0 * float(half_extents[i][along]) + _NEIGHBOUR_GAP_M for i in row] | |
| start = -0.5 * sum(widths) | |
| for index, width in zip(row, widths, strict=True): | |
| offsets[index, along] = start + 0.5 * width | |
| offsets[index, across] = row_start + 0.5 * depth | |
| start += width | |
| row_start += depth | |
| # Rows past the destination's edge are pulled back in: those objects pile on the others. | |
| limit = np.maximum(dest_half[None, :] - np.asarray(half_extents)[:, :2] - _NEIGHBOUR_GAP_M, 0.0) | |
| return np.clip(offsets, -limit, limit) | |
| def goal_point( | |
| relation: str, | |
| dest_center: np.ndarray, | |
| dest_min: np.ndarray, | |
| dest_max: np.ndarray, | |
| source_half: np.ndarray, | |
| offset_xy: np.ndarray, | |
| layer: int = 0, | |
| ) -> Tuple[np.ndarray, float]: | |
| """Where a source object's CENTRE must end up, and the EEF height to release it at. | |
| ``on`` rests it on the destination's top face; ``in`` drops it just above the floor of the | |
| container, which is what ``is_inside`` scores. Those differ for a walled destination: the object | |
| must END at the floor, but it must be RELEASED above the rim, since carrying it down inside fouls the | |
| gripper on the wall and on anything already delivered. Releasing above the rim lets it fall the | |
| last centimetres, which is how the container gets loaded. | |
| """ | |
| # "on" rests the object on the top face; "in" aims just above the container's AABB floor, | |
| # which ``is_inside`` credits. | |
| rim_z = float(dest_max[2]) + float(source_half[2]) | |
| z = rim_z | |
| if relation == "in": | |
| z = float(dest_min[2]) + float(source_half[2]) + _IN_CLEARANCE_M | |
| # A stacked layer rests on the layer below it. | |
| z += layer * 2.0 * float(source_half[2]) | |
| point = np.array([dest_center[0] + offset_xy[0], dest_center[1] + offset_xy[1], z], dtype=np.float32) | |
| return point, max(rim_z, z) | |
| def build_payloads( | |
| *, | |
| names: Sequence[str], | |
| centers: np.ndarray, | |
| mins: np.ndarray, | |
| maxs: np.ndarray, | |
| grasped: np.ndarray, | |
| goals: Sequence[Dict[str, Any]], | |
| n_envs: int, | |
| long_axis_yaws: Optional[np.ndarray] = None, | |
| ) -> List[Dict[str, Any]]: | |
| """One privileged payload per env. Arrays are ``[n_objects, n_envs, 3]`` in the base frame. | |
| Each entry of ``goals`` is ``{"source": idx, "destination": idx | None, "relation": str, | |
| "satisfied": [n_envs] bool}``, optionally with ``"predicate"`` (default ``"pick_and_place"``; | |
| ``pickup``/``grasp``/``reach`` end the motion early) and ``"point"``, an explicit goal | |
| position, ``[3]`` or ``[n_envs, 3]``, for a goal whose destination is a place rather than an | |
| object, which is how a region or a relative-placement task states where the object must end up. | |
| """ | |
| halves = 0.5 * (maxs - mins) | |
| per_destination: Dict[Optional[int], List[int]] = {} | |
| for index, goal in enumerate(goals): | |
| per_destination.setdefault(goal["destination"], []).append(index) | |
| last_destination = {goal["source"]: goal["destination"] for goal in goals} | |
| payloads = [] | |
| for env in range(n_envs): | |
| offsets = {} | |
| for destination, indices in per_destination.items(): | |
| if destination is None: | |
| continue | |
| # One slot per object, not per goal: a relay or revisit brings the same object back to | |
| # the same destination, and a slot of its own per visit aims it at the rim. Only objects | |
| # left here need slots side by side; when every visitor moves on again (a relay's pot), | |
| # they are there one at a time, so each takes the centre. | |
| sources = list(dict.fromkeys(goals[i]["source"] for i in indices)) | |
| if not any(last_destination[source] == destination for source in sources): | |
| offsets.update({i: (np.zeros(2), 0) for i in indices}) | |
| continue | |
| spread, layers = spread_offsets( | |
| [halves[source, env] for source in sources], halves[destination, env] | |
| ) | |
| slots = dict(zip(sources, zip(spread, layers, strict=True), strict=True)) | |
| offsets.update({i: slots[goals[i]["source"]] for i in indices}) | |
| conditions = [] | |
| for index, goal in enumerate(goals): | |
| source = goal["source"] | |
| target = encode_object( | |
| names[source], | |
| centers[source, env], | |
| mins[source, env], | |
| maxs[source, env], | |
| display_name=names[source], | |
| grasped=bool(grasped[source, env]), | |
| long_axis_yaw_rad=(None if long_axis_yaws is None else float(long_axis_yaws[source, env])), | |
| ) | |
| destination = goal["destination"] | |
| point, release_z = None, None | |
| explicit = goal.get("point") | |
| if explicit is not None: | |
| explicit = np.asarray(explicit, dtype=np.float64) | |
| point = explicit[env] if explicit.ndim == 2 else explicit | |
| release_z = float(point[2]) | |
| elif destination is not None: | |
| point, release_z = goal_point( | |
| goal["relation"], | |
| centers[destination, env], | |
| mins[destination, env], | |
| maxs[destination, env], | |
| halves[source, env], | |
| offsets[index][0], | |
| layer=int(offsets[index][1]), | |
| ) | |
| conditions.append( | |
| encode_condition( | |
| goal.get("predicate", "pick_and_place"), | |
| target, | |
| relation=goal["relation"], | |
| goal_point_m=point, | |
| release_z_m=release_z, | |
| destination=(names[destination] if destination is not None else None), | |
| grasped=bool(grasped[source, env]), | |
| satisfied=bool(goal["satisfied"][env]), | |
| ) | |
| ) | |
| objects = [ | |
| encode_object( | |
| name, | |
| centers[i, env], | |
| mins[i, env], | |
| maxs[i, env], | |
| display_name=name, | |
| grasped=bool(grasped[i, env]), | |
| long_axis_yaw_rad=(None if long_axis_yaws is None else float(long_axis_yaws[i, env])), | |
| ) | |
| for i, name in enumerate(names) | |
| ] | |
| payloads.append(encode_state(conditions, objects)) | |
| return payloads | |