LADDER-RoboLab-Long / ladder /components /inference /api /core /privileged_geometry.py
petkopetkov's picture
LADDER RoboLab-Long for LeRobot EnvHub (LADDER v1.0.0)
01171b7 verified
Raw History Blame Contribute Delete
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