Real2Sim kinematic trajectory refinement

Real2Sim kinematic trajectory refinement#

Refinement repairs a scene-instantiated nominal Cartesian path before Atomic Skills freeze their expansion baseline. The source evidence, control period, phase events and gripper semantics remain separately traceable. A bounded anchor warp preserves gravity-aligned translation; sparse fixed-grid fitting retains exact contact/hold constraints and phase-edge derivative stencils. Robot IK, motion budgets, collision evidence and measured task acceptance are separate checks. This API does not estimate robot calibration or contact forces.

Numerical pose fitting#

`PoseRefinementResult`	An owned fixed-grid fit and measured corrections to its input.
`refine_pose_trajectory`(poses, *, anchors[, ...])	Fit a pose reference while retaining exact, caller-owned hard anchors.

Classes:

PoseRefinementResult

An owned fixed-grid fit and measured corrections to its input.

Functions:

refine_pose_trajectory(poses, *, anchors[, ...])

Fit a pose reference while retaining exact, caller-owned hard anchors.

class embodichain.compute.trajectory.refinement.PoseRefinementResult[source]#

An owned fixed-grid fit and measured corrections to its input.

Parameters:

poses (ndarray) – Refined world TCP transforms, shaped (T, 4, 4).
max_translation_m (float) – Largest Euclidean correction from the input.
max_rotation_rad (float) – Largest geodesic rotation correction from the input.
iterations (int) – Maximum QP iteration count across coordinate solves.

Methods:

__init__(poses, max_translation_m, ...)

__init__(poses, max_translation_m, max_rotation_rad, iterations)#

embodichain.compute.trajectory.refinement.refine_pose_trajectory(poses, *, anchors, reference=None, weights=None, max_translation_m=0.02, max_rotation_rad=0.17453292519943295, smoothing_weight=1.0, max_iterations=4000)[source]#

Fit a pose reference while retaining exact, caller-owned hard anchors.

Six sparse, banded quadratic programs fit translation and a local rotation chart. Memory is linear in sample count; no dense sample-pair matrices are formed. reference may contain a geometric anchor warp, but trust bounds are always measured against poses. Rotations spanning the chart cut are rejected. Component trust boxes are followed by exact Euclidean/geodesic checks, so an approximate box never enlarges the declared correction budget. This operation neither changes timestamps nor certifies collisions or IK.

Parameters:

poses (ndarray) – Original proper world TCP poses, shaped (T, 4, 4).
anchors (Mapping[int, ndarray]) – Exact target transforms keyed by source sample index.
reference (ndarray | None) – Optional geometrically corrected initial reference.
weights (ndarray | None) – Positive per-sample observation weights, shaped (T,).
max_translation_m (float) – Maximum total correction relative to original input.
max_rotation_rad (float) – Maximum total orientation correction in radians.
smoothing_weight (float) – Nonnegative penalty on second sample differences.
max_iterations (int) – Bounded iteration budget for each coordinate QP.

Return type:

PoseRefinementResult

Returns:

Owned refined poses and measured correction diagnostics.

Raises:

ValueError – Inputs, chart, constraints, convergence or budgets are invalid.

Grounded interaction constraints#

embodichain.lab.sim.motion.refinement

Interaction-constrained nominal pose repair before the expansion baseline.

`MotionRefinementCfg`	Explicit permission and finite budgets for Cartesian nominal repair.
`InteractionConstraintSet`	One grounded skill's exact anchor and provenance.
`MotionRefinementResult`	Owned pose correction evidence for one scene-specific baseline.
`KinematicMotionRefiner`	Fit a bounded Cartesian path to skill-owned interaction constraints.

Classes:

`InteractionConstraintSet`	One grounded skill's exact anchor and provenance.
`KinematicMotionRefiner`	Fit a bounded Cartesian path to skill-owned interaction constraints.
`MotionRefinementCfg`	Explicit permission and finite budgets for Cartesian nominal repair.
`MotionRefinementResult`	Owned pose correction evidence for one scene-specific baseline.

class embodichain.lab.sim.motion.refinement.InteractionConstraintSet[source]#

One grounded skill’s exact anchor and provenance.

Parameters:

skill_id (str) – Pick, held transport or direct Place.
phases (tuple[PhaseAnnotation, ...]) – Complete, contiguous source phase partition.
anchor_index (int) – Grasp/lift entry, transport endpoint or release sample.
anchor_pose (ndarray) – World TCP target derived from the task and geometry.
geometry_id (str) – Geometry/affordance evidence identity, never a category name.
lift_height_m (float | None) – Explicit requested world-Z lift for opt-in Pick repair.

Methods:

__init__(skill_id, phases, anchor_index, ...)

__init__(skill_id, phases, anchor_index, anchor_pose, geometry_id, lift_height_m=None)#

class embodichain.lab.sim.motion.refinement.KinematicMotionRefiner[source]#

Fit a bounded Cartesian path to skill-owned interaction constraints.

Parameters:: cfg (MotionRefinementCfg) – Explicit repair permissions and numerical budgets.

Methods:

`__init__`(cfg)
`refine`(poses, constraints, *[, ...])	Repair geometry before freezing protected contact and hold samples.

__init__(cfg)[source]#

refine(poses, constraints, *, observation_weights=None)[source]#

Repair geometry before freezing protected contact and hold samples.

A gravity-preserving anchor warp supplies the initial reference. Contact phases are exact stationary TCP constraints; hold phases retain the warped reference, with world-up displacement preserved. Explicit Pick endpoint projection repairs a lift’s entry/exit before freezing that reference; all resulting changes consume the same correction budget. Free interiors are fitted by a sparse QP. Two samples at each phase edge remain at the corrected reference so later expansion can protect derivative stencils.

Parameters:

poses (ndarray) – Scene-retargeted source TCP path, before anchor repair.
constraints (InteractionConstraintSet) – Grounded interaction anchor and complete phase contract.
observation_weights (ndarray | None) – Optional positive source-confidence weights.

Return type:

MotionRefinementResult

Returns:

Refined path and auditable, scene-specific correction report.

Raises:

ValueError – Source permissions, timing partition or repair is invalid.

class embodichain.lab.sim.motion.refinement.MotionRefinementCfg[source]#

Explicit permission and finite budgets for Cartesian nominal repair.

Translation and rotation budgets cover the whole source-to-refined change, including anchor alignment. Contact events, gripper commands and control time are unchanged. Geometry compatibility, IK and collision acceptance remain independent host/skill checks.

Methods:

`__init__`([max_translation_m, ...])
`copy`(**kwargs)	Return a new object replacing specified fields with new values.
`replace`(**kwargs)	Return a new object replacing specified fields with new values.
`to_dict`()	Convert an object into dictionary recursively.
`validate`([prefix])	Check the validity of configclass object.

Attributes:

`fingerprint`	Return the content identity of all permissions and solver settings.
`pick_entry_projection_samples`	Explicit free-entry transition width; zero preserves an already paced entry.
`project_free_boundaries`	Stop at free-phase boundaries and join adjacent contact anchors exactly.
`project_pick_approach`	Repair the Pick approach endpoint locally while preserving its free entry.
`project_pick_lift`	Explicitly repair a Pick lift endpoint before protecting its baseline.

__init__(max_translation_m=<factory>, max_rotation_rad=<factory>, smoothing_weight=<factory>, max_candidates=<factory>, seed=<factory>, support_margin_m=<factory>, max_release_gap_m=<factory>, max_iterations=<factory>, project_pick_lift=<factory>, project_pick_approach=<factory>, project_free_boundaries=<factory>, pick_entry_projection_samples=<factory>)#

copy(**kwargs)#

Return a new object replacing specified fields with new values.

This is especially useful for frozen classes. Example usage:

@configclass(frozen=True)
class C:
    x: int
    y: int

c = C(1, 2)
c1 = c.replace(x=3)
assert c1.x == 3 and c1.y == 2

Parameters:

obj (object) – The object to replace.
**kwargs – The fields to replace and their new values.

Return type:

object

Returns:

The new object.

property fingerprint: str#: Return the content identity of all permissions and solver settings.

pick_entry_projection_samples: int#: Explicit free-entry transition width; zero preserves an already paced entry.

project_free_boundaries: bool#: Stop at free-phase boundaries and join adjacent contact anchors exactly.

project_pick_approach: bool#: Repair the Pick approach endpoint locally while preserving its free entry.

project_pick_lift: bool#: Explicitly repair a Pick lift endpoint before protecting its baseline.

replace(**kwargs)#

Return a new object replacing specified fields with new values.

This is especially useful for frozen classes. Example usage:

@configclass(frozen=True)
class C:
    x: int
    y: int

c = C(1, 2)
c1 = c.replace(x=3)
assert c1.x == 3 and c1.y == 2

Parameters:

obj (object) – The object to replace.
**kwargs – The fields to replace and their new values.

Return type:

object

Returns:

The new object.

to_dict()#

Convert an object into dictionary recursively.

Note

Ignores all names starting with “__” (i.e. built-in methods).

Parameters:: obj (object) – An instance of a class to convert.
Raises:: ValueError – When input argument is not an object.
Return type:: dict[str, Any]
Returns:: Converted dictionary mapping.

validate(prefix='')#

Check the validity of configclass object.

This function checks if the object is a valid configclass object. A valid configclass object contains no MISSING entries.

Parameters:

obj (object) – The object to check.
prefix (str) – The prefix to add to the missing fields. Defaults to ‘’.

Return type:

list[str]

Returns:

A list of missing fields.

Raises:

TypeError – When the object is not a valid configuration object.

class embodichain.lab.sim.motion.refinement.MotionRefinementResult[source]#

Owned pose correction evidence for one scene-specific baseline.

Parameters:

original_poses (ndarray) – Retargeted source TCP path before any repair.
fit (PoseRefinementResult) – Fixed-grid numerical result.
report (Mapping[str, object]) – JSON-compatible grounded constraints and correction diagnostics.

Methods:

__init__(original_poses, fit, report)

__init__(original_poses, fit, report)#

Configured integration#

Each runtime_services.nominal_motion_sources entry may declare a closed refinement mapping containing MotionRefinementCfg fields. The source must explicitly use eef_pose authority. Pick requires an antipodal object mesh and a calibrated parallel-jaw generator, including for a configured grasp. Place requires the tabletop host’s physical support validator. A failed repair does not fall back to source replay or native planning.

The source provider returns the existing InstantiatedSkillMotion. Its refinement_report and original/refined TCP evidence are copied into typed episode trace attachments. The synchronous sink checks their digest, sample count and measured correction budget before confirming persistence.

Real2Sim kinematic trajectory refinement

Contents

Real2Sim kinematic trajectory refinement#

Numerical pose fitting#

Grounded interaction constraints#

Configured integration#