Learning Keypoint Constraints from Visual Demonstrations for Safe Motion Planning

Zheng Qiu; Zhouyu Zhang; Ninghan Zhong; Glen Chou

cs.ROOct 13, 2026

Learning Keypoint Constraints from Visual Demonstrations for Safe Motion Planning

Authors: Zheng Qiu, Zhouyu Zhang, Ninghan Zhong, Glen Chou

Abstract

We present Keypoint Constraint Learning (KCL), a framework for learning task-relevant motion constraints from visual demonstrations and for planning safely with the extracted constraints. To make constraint learning tractable from RGB-D observations, we represent demonstrations using sparse 3D object-centric keypoints and semantic mask centroids extracted through segmentation, visual feature clustering, and point tracking. In this reduced geometric space, we learn keypoint dynamics and infer distance- and orientation-based constraints that make the expert trajectories feasible and approximately locally optimal by enforcing KKT optimality conditions in the reduced state space. We then plan with the learned constraints under uncertainty, using robustness margins derived from accumulated prediction error, and provide a theoretical analysis of how keypoint perception error and learned dynamics error affect the recovered constraints. We evaluate KCL on visual demonstrations from navigation to rigid/deformable manipulation, showing that it recovers multi-stage geometric constraints and outperforms baselines in downstream safe planning.

Figures & tables

Fig. 1: (a) Keypoint constraints learned from a video of a human reorienting and inserting a hammer, a multi-stage task where low-level distance/orientation constraints between objects are learned together with the task segmentation (Stages 1-3). (b) KCL plans a safe trajectory conforming with the constraints.

Experiment	Setting	Method	Demo pts.	Val num.	Dyn. eval loss	KCL runtime (s)	Final stat.	Clearance err. (m)	Const. Safe rate	obs. safe rate	Goal reach
Dubins-car obstacle avoidance
Dubins	Simulation	KCL	1640	1000	0.0028	2.3	2.09	$0.011$	$1.0\pm 0.000$	$1.0\pm 0.000$	$1.0\pm 0.000$
Dubins	Simulation	K-VIL	1640	1000	N/A	N/A	N/A	N/A	$0.184\pm 0.017$	$0.997\pm 0.005$	$1.0\pm 0.000$
Dubins	Simulation	MBIRL	1640	1000	0.0752	N/A	N/A	N/A	$0.0\pm 0.000$	$0.07\pm 0.021$	$1.0\pm 0.000$
Dubins	Simulation	ReKep	1640	1000	N/A	N/A	N/A	N/A	$0.0\pm 0.000$	$0.608\pm 0.063$	$1.0\pm 0.000$
Dubins	Real	KCL	815	5	0.0038259	4.1	0.2429	$0.021$	$1.0\pm 0.000$	$1.0\pm 0.000$	$1.0\pm 0.000$

TABLE I: Full numerical summary of keypoint constraint learning and planning results. “Demo pts.” are trajectory points used for inverse-KKT constraint learning. “Val num.” are the number of planning validation trials. “Clearance error” is the absolute error in the recovered distance-clearance radius; for tasks also with orientation constraints, this reports the distance-clearance error. “Const. safe rate” is satisfaction rate of the demonstration-inferred constraint during planning, “Obs. safe rate” is satisfaction of the ground-truth obstacle-safety check, and “Goal reach” is terminal task completion. Values reported as mean

\pm

standard deviation are computed across evaluation batches.

Fig. 2: Dubins-car task: (a) Simulated demo and (b) plan (green trajectory is plan from KCL). (c) Rollouts of plans computed via baselines and KCL (only KCL achieves $100\%$ satisfaction of both the demonstration-inferred clearance and the ground-truth obstacle-safety check; see Tab. I ). (d) Real demo and (e) planning (red circle is constraint learned by KCL).

Fig. 3: KUKA manipulation: Simulated (a) demo and (b) plan. Hardware (c) demo and (d) plan. In (b), (d), learned constraint (red circle) is overlaid with the true obstacle. In (b), the green trajectory is the planned motion of the bottom mug keypoint, which satisfies the learned obstacle constraint. The mug also satisfies the learned tilting threshold over the full trajectory.

Fig. 4: Rope obstacle avoidance: Simulated (a) demo and (b) plan computed by KCL. Hardware (c) demo and (d) plan computed by KCL. In (b), (d), learned constraint (light red circle) is overlaid with the ground-truth obstacle.

Fig. 5: (a) Keypoint constraints learned from a video of a human lifting, pouring, and placing a mug, where low-level constraints between objects are learned together with the task segmentation (Stages 1-3). We also have occlusions in the demonstration. (b) KCL plans a safe trajectory using the constraints.

Fig. 6: Knot tying: KCL jointly recovers (a) stage boundaries $t_{1}{=}18$ , $t_{2}{=}48$ (black dashed) and (b) constraint thresholds $\theta_{\rm loop}{=}0.042$ m (orange solid), $\theta_{\rm far}{=}0.111$ m (orange dotted) by solving a single MILP, matching the ground-truth decomposition without manual phase annotation. (c) Multi-stage decomposition of demonstration via learned constraints.

$\sigma_{\rm noise}$	# Trials	Clearance RMSE (m)
0.00	10	$0.0000\pm 0.0000$
0.01	10	$0.0146\pm 0.0022$
0.05	10	$0.0738\pm 0.0078$
0.10	10	$0.1220\pm 0.0205$
0.15	10	$0.1704\pm 0.0423$
0.20	10	$0.1912\pm 0.0250$

TABLE II: Constraint recovery noise robustness.

Task	$N$	$M$	Solve time(s)
Nav.	10	5	$0.56\!\pm\!.01$
Nav.	20	10	$0.75\!\pm\!.00$
Nav.	50	50	$2.05\!\pm\!.05$
Rigid	8	4	$0.07\!\pm\!.00$
Rigid	16	8	$0.11\!\pm\!.00$
Rigid	50	50	$0.37\!\pm\!.04$

TABLE III: Solve times (10 trials).

Appendix figures & tables4 assets

Supplementary material from the paper’s appendix.

Appendix

Fig. 7: Ground-truth CIOC demonstration.

Fig. 8: Peanut constraint with Dubins car dynamics.

Fig. 9: Manipulation baseline comparison. Planning results. For navigation and manipulation planning, blue trajectories satisfy the demonstration-inferred clearance constraint, yellow trajectories avoid the ground-truth obstacle but violate the demonstration-inferred clearance, and dashed orange trajectories collide with the ground-truth obstacle. (a) Navigation baseline comparison. (b) Manipulation baseline comparison.

Fig. 10: Rope planning with the learned keypoint constraint. Three trajectories denote rope keypoint trajectories; the light-blue tube is the SLS safety tube around the constrained keypoint.

Robot Motion Planning

Robot motion planning finds safe, feasible paths and trajectories for robots moving through constrained, changing environments.

See all 271 papers

Learning Unknown Constraints without Unsafe Data via Optimality and Counterfactual Regularization
Oct 7, 2026Zhouyu Zhang, Chih-Yuan Chiu, Glen ChouConstrained RL Learning from Demonstration
School of Electrical and Computer Engineering Georgia Institute of Technology Atlanta, GA 30309 · School of Cybersecurity and Privacy School of Aerospace Engineering Georgia Institute of Technology
KeyGen: Unsupervised Keypoint based Object-Centric Representations for Category-Level Policy Generalization
Sep 23, 2026Shuxin Cao, Liquan Wang, Masoud Moghani +2Point Cloud Learning Generalization in Robotic Manipulation
Georgia Institute of Technology · University of Toronto
AFFORD2ACT: How Far Can Sparse Keypoints Go for Robot Manipulation?
Oct 1, 2025Anukriti Singh, Kasra Torshizi, Khuzema Habib +3Robotic Manipulation Data-Efficient Robot Learning
Department of Computer Science, University of Maryland, College Park, MD, USA.

Oct 7, 2026J/K move · Enter open · S save

Learning Unknown Constraints without Unsafe Data via Optimality and Counterfactual Regularization

Zhouyu Zhang, Chih-Yuan Chiu, Glen Chou

School of Electrical and Computer Engineering Georgia Institute of Technology Atlanta, GA 30309 · School of Cybersecurity and Privacy School of Aerospace Engineering Georgia Institute of Technology

Constrained RL Learning from Demonstration

Open paper AskPDF arXiv

Learning from demonstrations (LfD) provides a framework for inferring unknown constraints from locally optimal, constraint-satisfying expert behavior. Existing approaches largely fall into two paradigms, constrained inverse optimal control (CIOC) and inverse constrained reinforcement learning (ICRL). CIOC exploits optimality conditions such as the Karush--Kuhn--Tucker (KKT) conditions but typically assumes known dynamics and structured constraint representations. Meanwhile, ICRL accommodates complex unknown constraints and unknown transition dynamics but often requires extensive online exploration, during which unsafe constraint violations may occur. In this work, we introduce Counterfactual KKT (CF-KKT), a constraint learning framework that leverages learned dynamics and locally optimal demonstrations to recover unknown constraints without requiring known dynamics or additional risky exploration, thereby combining the data efficiency and safety advantages of CIOC with the flexibility of ICRL. First, we use a locally learned differentiable dynamics model to impose KKT-inspired optimality conditions directly on the demonstrations. Second, we use the learned dynamics to generate reward-improving counterfactual behaviors near the demonstrations, revealing behaviors that would be preferable in the absence of the unknown constraint and thus providing synthetic infeasible data. When the constraint parameterization is known, the same learned-dynamics framework enables direct CIOC-based parameter recovery, and we characterize its sensitivity to dynamics misspecification. Across high-dimensional robotic control tasks, our approach learns neural constraint representations with improved safety and data efficiency relative to state-of-the-art offline ICRL baselines.

Abstract

Robot Motion Planning

Explore similar work

Learning Unknown Constraints without Unsafe Data via Optimality and Counterfactual Regularization

KeyGen: Unsupervised Keypoint based Object-Centric Representations for Category-Level Policy Generalization

AFFORD2ACT: How Far Can Sparse Keypoints Go for Robot Manipulation?