Lamapi commited on
Commit
81c59cd
·
verified ·
1 Parent(s): 5cbb67a

Publish Next Servo with evaluation and replay

Browse files
Files changed (9) hide show
  1. .gitattributes +2 -0
  2. README.md +61 -0
  3. evaluate.py +51 -0
  4. evaluation.json +0 -0
  5. preview.png +3 -0
  6. render.py +89 -0
  7. replay.mp4 +3 -0
  8. requirements.txt +1 -0
  9. servo.py +79 -0
.gitattributes CHANGED
@@ -33,3 +33,5 @@ saved_model/**/* filter=lfs diff=lfs merge=lfs -text
33
  *.zip filter=lfs diff=lfs merge=lfs -text
34
  *.zst filter=lfs diff=lfs merge=lfs -text
35
  *tfevents* filter=lfs diff=lfs merge=lfs -text
 
 
 
33
  *.zip filter=lfs diff=lfs merge=lfs -text
34
  *.zst filter=lfs diff=lfs merge=lfs -text
35
  *tfevents* filter=lfs diff=lfs merge=lfs -text
36
+ preview.png filter=lfs diff=lfs merge=lfs -text
37
+ replay.mp4 filter=lfs diff=lfs merge=lfs -text
README.md ADDED
@@ -0,0 +1,61 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ ---
2
+ language: en
3
+ tags:
4
+ - robotics
5
+ - motion-planning
6
+ ---
7
+
8
+
9
+ # Next Servo
10
+
11
+ Next Servo plans how a two-joint robot arm can carry a circular payload around a stationary fixture. Select one of four scenarios and scrub through the motion to see how the elbow and payload move. The replay shows the actual planner output.
12
+
13
+ [![Next Servo motion replay preview](preview.png)](replay.mp4)
14
+
15
+ [Watch the 14-second replay](replay.mp4)
16
+
17
+ ## How it plans
18
+
19
+ The planner computes both inverse-kinematics branches at the start and destination. It searches a 5-degree joint grid with breadth-first search, then connects the grid route to the exact endpoints. Each edge is checked for link and payload clearance. A bound on motion between samples gives a conservative clearance check for the full interpolated edge.
20
+
21
+ The baseline tries a single straight interpolation in joint space for each pair of inverse-kinematics branches, with the same edge check. It can complete easy transfers with a single move. The grid planner can use intermediate poses when the direct moves fail.
22
+
23
+ ## Results
24
+
25
+ | Simulated workload | Grid planner accepted | Direct interpolation accepted |
26
+ |---|---:|---:|
27
+ | Four selected detour examples | 4/4 | 0/4 |
28
+ | Random workload, seed 19 | 63/80 | 51/80 |
29
+ | Random workload, seed 73 | 59/80 | 46/80 |
30
+ | Random workload, seed 149 | 63/80 | 54/80 |
31
+ | **Random workload total** | **185/240** | **151/240** |
32
+
33
+ The four replay examples were selected to show cases where direct interpolation was rejected. The random workload includes instances that may be infeasible. Four additional checks passed for a clear transfer, blocked pickup, blocked destination and blocked base. A rejection means the algorithm did not certify a route under its constraints. Every accepted grid route passed the conservative edge check. Maximum numerical endpoint error was 7.03e-16 length units across accepted runs.
34
+
35
+ ## Reproduce
36
+
37
+ Install the requirements and run the scripts from this directory.
38
+
39
+ ~~~bash
40
+ pip install -r requirements.txt
41
+ python evaluate.py
42
+ python render.py
43
+ ~~~
44
+
45
+ Rendering requires FFmpeg on PATH. The evaluator exports the sampled inputs and results to evaluation.json. The notebook runs evaluation and replay generation in Colab.
46
+
47
+ ## Scope
48
+
49
+ A planar geometric simulation with rigid, unit-length, zero-width links, a circular payload and one static circular obstacle. The payload starts attached at the pickup location. Motion is kinematic, and playback time is illustrative. A physical arm needs its own geometry, collision model, dynamics, speed limits, grasping and hardware validation.
50
+
51
+
52
+ [Open the live planner](https://huggingface.co/spaces/thelamapi/Next-Servo)
53
+
54
+ ## Python quickstart
55
+
56
+ ~~~python
57
+ from servo import DEMOS, plan
58
+
59
+ route = plan(*DEMOS[0])
60
+ print(len(route) - 1, 'edges')
61
+ ~~~
evaluate.py ADDED
@@ -0,0 +1,51 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Reproduce the four curated cases and a seeded random comparison."""
2
+ import json
3
+ import random
4
+ from pathlib import Path
5
+ from math import hypot
6
+ from servo import DEMOS, clearance, direct, forward, plan, safe_edge
7
+
8
+ def evaluate(case):
9
+ start, goal, obstacle = case
10
+ route = plan(*case)
11
+ result = {"direct_accepted": direct(*case), "planner_accepted": bool(route)}
12
+ if route:
13
+ error = hypot(*(a-b for a,b in zip(forward(route[-1])[1],goal)))
14
+ assert error < 1e-9
15
+ assert all(safe_edge(a,b,obstacle) for a,b in zip(route,route[1:]))
16
+ result.update(endpoint_error=error, edges=len(route)-1,
17
+ minimum_waypoint_clearance=min(clearance(q,obstacle) for q in route))
18
+ return result
19
+
20
+ if __name__ == "__main__":
21
+ suites = {}
22
+ for seed in (19, 73, 149):
23
+ rng = random.Random(seed)
24
+ cases = []
25
+ while len(cases) < 80:
26
+ start, goal = [(rng.uniform(-1.55,1.55),rng.uniform(.2,1.6)) for _ in range(2)]
27
+ if max(hypot(*p) for p in (start,goal)) >= 1.9:
28
+ continue
29
+ obstacle = (rng.uniform(-1,1), rng.uniform(.25,1.3), rng.uniform(.10,.22))
30
+ cases.append((start,goal,obstacle))
31
+ suites[str(seed)] = {"cases": cases, "results": [evaluate(c) for c in cases]}
32
+ edge_cases = {
33
+ "clear_transfer": (((1,.5),(.8,.8),(-1,-1,.15)), True),
34
+ "blocked_pickup": (((1,.5),(.8,.8),(1,.5,.15)), False),
35
+ "blocked_drop": (((1,.5),(.8,.8),(.8,.8,.15)), False),
36
+ "blocked_base": (((1,.5),(.8,.8),(0,0,.15)), False),
37
+ }
38
+ checks = {}
39
+ for name, (case, expected) in edge_cases.items():
40
+ result = evaluate(case)
41
+ assert result["planner_accepted"] == expected, name
42
+ assert result["direct_accepted"] == expected, name
43
+ checks[name] = result
44
+ data = {"curated": [evaluate(c) for c in DEMOS], "suites": suites, "edge_cases": checks}
45
+ Path("evaluation.json").write_text(json.dumps(data, indent=2), encoding="utf-8")
46
+ for name, rows in [("curated", data["curated"]), *[
47
+ (seed, suite["results"]) for seed, suite in suites.items()
48
+ ]]:
49
+ print(name, len(rows), sum(x["planner_accepted"] for x in rows),
50
+ sum(x["direct_accepted"] for x in rows),
51
+ max(x["endpoint_error"] for x in rows if x["planner_accepted"]))
evaluation.json ADDED
The diff for this file is too large to render. See raw diff
 
preview.png ADDED

Git LFS Details

  • SHA256: 49b4f6e7c7c5814ba25a1d351445e0ec93ff58a01e9393b8adc28c4b2437fb80
  • Pointer size: 131 Bytes
  • Size of remote file: 108 kB
render.py ADDED
@@ -0,0 +1,89 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Render four 3.5-second demonstrations as an H.264 video."""
2
+ import subprocess
3
+ from pathlib import Path
4
+ from PIL import Image, ImageDraw, ImageFont
5
+ from servo import DEMOS, clearance, forward, interpolate, plan
6
+
7
+ SIZE = (960, 540)
8
+ FPS = 24
9
+ INK, GREEN, BG = "#102923", "#087b64", "#f3f5ef"
10
+ TITLES = ["Carry across the fixture", "Reach the high shelf",
11
+ "Transfer between stations", "Exit a narrow pocket"]
12
+
13
+ def font(size, bold=False):
14
+ for directory, regular, heavy in [
15
+ (Path("C:/Windows/Fonts"), "segoeui.ttf", "segoeuib.ttf"),
16
+ (Path("/usr/share/fonts/truetype/dejavu"), "DejaVuSans.ttf", "DejaVuSans-Bold.ttf"),
17
+ ]:
18
+ path = directory / (heavy if bold else regular)
19
+ if path.exists():
20
+ return ImageFont.truetype(str(path), size)
21
+ return ImageFont.load_default(size=size)
22
+
23
+ title, label, body, hero = font(36,True), font(15,True), font(19), font(48,True)
24
+
25
+ def xy(p):
26
+ return 335+100*p[0], 293-100*p[1]
27
+
28
+ def disk(draw, p, r, **style):
29
+ x,y = p
30
+ draw.ellipse((x-r,y-r,x+r,y+r), **style)
31
+
32
+ def frame(case_id, case, route, progress):
33
+ start, target, obstacle = case
34
+ image = Image.new("RGB", SIZE, BG)
35
+ d = ImageDraw.Draw(image)
36
+ d.text((36, 23), "NEXT SERVO / MOTION PLANNING", font=label, fill=GREEN)
37
+ d.text((36, 49), TITLES[case_id], font=title, fill=INK)
38
+ d.rounded_rectangle((36, 105, 634, 510), radius=18, fill="#eaf0e9", outline="#ccd9cf", width=2)
39
+ for x in range(85, 620, 25):
40
+ d.line((x, 127, x, 488), fill="#d9e3d9")
41
+ for y in range(127, 499, 25):
42
+ d.line((60, y, 610, y), fill="#d9e3d9")
43
+ x,y,r = obstacle
44
+ disk(d, xy((x,y)), 100*r, fill="#dea291", outline="#a74436", width=2)
45
+ disk(d, xy(start), 8, outline="#59726b", width=2)
46
+ disk(d, xy(target), 12, outline=GREEN, width=2)
47
+ index = int(progress*(len(route)-1))
48
+ q = interpolate(route[index], route[min(index+1,len(route)-1)],
49
+ progress*(len(route)-1)-index)
50
+ elbow, tip = forward(q)
51
+ trail = [xy(forward(a)[1]) for a in route[:index+1]]+[xy(tip)]
52
+ d.line(trail, fill="#29947d", width=4)
53
+ joints = [xy((0,0)), xy(elbow), xy(tip)]
54
+ d.line(joints, fill=INK, width=8, joint="curve")
55
+ for joint in joints[:2]:
56
+ disk(d, joint, 9, fill=INK, outline="white", width=2)
57
+ disk(d, joints[-1], 7, fill="#087b64", outline="white", width=2)
58
+ d.text((667, 125), f"CASE {case_id+1:02d} / 04", font=label, fill=GREEN)
59
+ d.text((667, 173), "Delivered" if progress == 1 else "Carrying", font=title, fill=INK)
60
+ d.text((667, 253), "STRAIGHT JOINT MOVE", font=label, fill="#5e716a")
61
+ d.text((667, 276), "Rejected by clearance check", font=body, fill=INK)
62
+ d.text((667, 337), "PLANNER EDGE", font=label, fill="#5e716a")
63
+ d.text((667, 360), f"{index} / {len(route)-1}", font=hero, fill=GREEN)
64
+ d.line((667, 429, 924, 429), fill="#cdd9d0", width=2)
65
+ for x, color, name in [(674, "#a74436", "Fixture"), (793, GREEN, "Payload")]:
66
+ disk(d, (x, 462), 5, fill=color)
67
+ d.text((x+12, 450), name, font=body, fill=INK)
68
+ d.rectangle((36, 525, 924, 529), fill="#cad9cd")
69
+ d.rectangle((36, 525, 36+888*progress, 529), fill=GREEN)
70
+ return image
71
+
72
+ if __name__ == "__main__":
73
+ output = Path("replay.mp4")
74
+ partial = output.with_suffix(".partial.mp4")
75
+ args = ["ffmpeg","-y","-loglevel","error","-f","rawvideo","-pix_fmt","rgb24",
76
+ "-s","960x540","-r",str(FPS),"-i","pipe:0","-an","-c:v","libx264",
77
+ "-preset","fast","-crf","20","-pix_fmt","yuv420p","-movflags","+faststart",str(partial)]
78
+ with subprocess.Popen(args, stdin=subprocess.PIPE) as proc:
79
+ for i, case in enumerate(DEMOS):
80
+ route = plan(*case)
81
+ if not route:
82
+ raise RuntimeError(f"Unsolved demo {i+1}")
83
+ for n in range(84):
84
+ p = min(1, n/60)
85
+ proc.stdin.write(frame(i,case,route,p).tobytes())
86
+ proc.stdin.close()
87
+ if proc.wait():
88
+ raise RuntimeError("FFmpeg encoding failed")
89
+ partial.replace(output)
replay.mp4 ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ version https://git-lfs.github.com/spec/v1
2
+ oid sha256:1f6a3cea839bec8cea718bf10326d6c17a4500b65b8bfcc3845778535dd112a2
3
+ size 253473
requirements.txt ADDED
@@ -0,0 +1 @@
 
 
1
+ Pillow>=10.1
servo.py ADDED
@@ -0,0 +1,79 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Collision-aware planning for a planar two-link arm carrying a disk."""
2
+ from collections import deque
3
+ from math import acos, atan2, cos, hypot, pi, sin
4
+
5
+ STEP = pi / 36
6
+ RADIUS = 0.065
7
+ OFFSETS = ((1, 0), (-1, 0), (0, 1), (0, -1))
8
+ DEMOS = [
9
+ ((.85, .58), (-.63, .27), (.52, .50, .15)),
10
+ ((-1.03, .40), (-.11, 1.27), (-.03, .99, .15)),
11
+ ((1.39, .49), (-1.02, .53), (.64, 1.28, .15)),
12
+ ((-.37, .30), (.27, .89), (.59, .91, .15)),
13
+ ]
14
+
15
+ def forward(q):
16
+ a, b = q
17
+ elbow = cos(a), sin(a)
18
+ return elbow, (elbow[0] + cos(a+b), elbow[1] + sin(a+b))
19
+
20
+ def inverse(point):
21
+ x, y = point
22
+ b = acos((x*x+y*y)/2-1)
23
+ for bend in (b, -b):
24
+ yield atan2(y, x)-bend/2, bend
25
+
26
+ def clearance(q, obstacle):
27
+ elbow, tip = forward(q)
28
+ x, y, radius = obstacle
29
+ distances = []
30
+ for (ax, ay), (bx, by) in [((0, 0), elbow), (elbow, tip)]:
31
+ dx, dy = bx-ax, by-ay
32
+ t = max(0, min(1, (x-ax)*dx+(y-ay)*dy))
33
+ distances.append(hypot(x-ax-t*dx, y-ay-t*dy)-radius)
34
+ return min(*distances, hypot(x-tip[0], y-tip[1])-radius-RADIUS)
35
+
36
+ def interpolate(a, b, t):
37
+ return tuple(x+(y-x)*t for x, y in zip(a, b))
38
+
39
+ def safe_edge(a, b, obstacle):
40
+ # Every point lies within this motion bound of its nearest sample.
41
+ margin = (2*abs(b[0]-a[0])+abs(b[1]-a[1])) / 24
42
+ return all(clearance(interpolate(a,b,i/12), obstacle) > margin for i in range(13))
43
+
44
+ def plan(start, target, obstacle):
45
+ def angles(node):
46
+ return tuple(i*STEP for i in node)
47
+
48
+ def anchors(point):
49
+ for q in inverse(point):
50
+ i, j = (round(a/STEP) for a in q)
51
+ for di in range(-1, 2):
52
+ for dj in range(-1, 2):
53
+ node = i+di, j+dj
54
+ if max(map(abs, node)) <= 36 and safe_edge(q, angles(node), obstacle):
55
+ yield node, q
56
+
57
+ roots, goals = dict(anchors(start)), dict(anchors(target))
58
+ parents = dict.fromkeys(roots)
59
+ queue = deque(roots)
60
+ while queue:
61
+ node = queue.popleft()
62
+ if node in goals:
63
+ tail = goals[node]
64
+ route = []
65
+ while parents[node] is not None:
66
+ route.append(angles(node))
67
+ node = parents[node]
68
+ return [roots[node], angles(node), *reversed(route), tail]
69
+ for di, dj in OFFSETS:
70
+ nxt = node[0]+di, node[1]+dj
71
+ if (max(map(abs, nxt)) <= 36 and nxt not in parents
72
+ and safe_edge(angles(node), angles(nxt), obstacle)):
73
+ parents[nxt] = node
74
+ queue.append(nxt)
75
+ return []
76
+
77
+ def direct(start, target, obstacle):
78
+ return any(safe_edge(a,b,obstacle) for a in inverse(start) for b in inverse(target))
79
+