Skip to content

Commit da3f51a

Browse files
Jepson2kJepson2k
committed
add tcp_offset commands and fix planner tool sync on RESET
- new SET_TCP_OFFSET / TCP_OFFSET wire commands; SetTcpOffsetCommand is a SystemCommand and now lives in SYSTEM_CMD_TYPES so the client waits for ack instead of leaving stale OK in the rx queue - TCP_OFFSET added to QUERY_CMD_TYPES for consistency - controller now syncs tool state to the planner subprocess on RESET (fixes test_cartesian_move_validation when run after tool tests) - joint_path_to_tcp_poses uses pinokin so3_rpy (intrinsic XYZ) to match the convention used elsewhere in robot.py - async_client move_s/move_p Category: Smooth Motion -> Motion to match the ABC; drop incompatible move_j @Overloads - examples reorganised to mirror programs/ (deleted pick_and_place, reachable poses in demo_showcase, use robot.create_sync_client) - example tests gated behind --examples flag (port 5001 collides with the integration server fixture); 240s subprocess + 300s pytest timeout per example
1 parent 3e3eeea commit da3f51a

25 files changed

Lines changed: 809 additions & 327 deletions

‎examples/demo_showcase.py‎

Lines changed: 157 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,157 @@
1+
"""Showcase script demonstrating all motion types.
2+
3+
Exercises move_j, move_l, move_c, move_p, move_s, blended zig-zag,
4+
tool actions, TCP offset, and precision TRF rotations.
5+
6+
Run:
7+
python examples/demo_showcase.py
8+
"""
9+
10+
import math
11+
12+
from parol6 import Robot
13+
14+
HOST = "127.0.0.1"
15+
PORT = 5001
16+
17+
with Robot(host=HOST, port=PORT, normalize_logs=True) as robot:
18+
rbt = robot.create_sync_client(timeout=2.0)
19+
rbt.wait_ready(timeout=5.0)
20+
rbt.simulator(True)
21+
22+
# Select tool and home
23+
rbt.select_tool("SSG-48")
24+
rbt.home(wait=True)
25+
26+
# move_j vs move_l (joint-space then linear-cartesian to nearby pose)
27+
rbt.move_j(pose=[100, 240, 334, 90, 0, 90], speed=0.5, wait=True)
28+
rbt.move_l([-50, 240, 334, 90, 0, 90], speed=0.5, wait=True)
29+
30+
# ── Curved motion: three vertical circles + sine-wave spline ──────────
31+
RADIUS = 30
32+
SPEED = 0.4
33+
CIRCLE_Y = 340
34+
ORIENTATION = [90, 0, 90]
35+
CENTERS = [(0, CIRCLE_Y, 280), (0, CIRCLE_Y, 210), (0, CIRCLE_Y, 140)]
36+
37+
def circle_pt(cx, cz, angle_deg):
38+
"""Circle in the XZ plane (vertical) at fixed Y."""
39+
a = math.radians(angle_deg)
40+
return [
41+
cx + RADIUS * math.cos(a),
42+
CIRCLE_Y,
43+
cz + RADIUS * math.sin(a),
44+
] + ORIENTATION
45+
46+
# Circle 1: full circle with a single move_c (start = end)
47+
cx, _, cz = CENTERS[0]
48+
rbt.move_j(pose=circle_pt(cx, cz, 0), speed=0.5, wait=True)
49+
rbt.move_c(
50+
via=circle_pt(cx, cz, 180), end=circle_pt(cx, cz, 0), speed=SPEED, wait=True
51+
)
52+
53+
# Circle 2: two half-circle move_c arcs
54+
cx, _, cz = CENTERS[1]
55+
rbt.move_l(circle_pt(cx, cz, 0), speed=SPEED, wait=True)
56+
rbt.move_c(
57+
via=circle_pt(cx, cz, 90), end=circle_pt(cx, cz, 180), speed=SPEED, wait=True
58+
)
59+
rbt.move_c(
60+
via=circle_pt(cx, cz, 270), end=circle_pt(cx, cz, 0), speed=SPEED, wait=True
61+
)
62+
63+
# Circle 3: computed waypoints with move_p
64+
cx, _, cz = CENTERS[2]
65+
waypoints = [circle_pt(cx, cz, i * 30) for i in range(12)]
66+
waypoints.append(waypoints[0])
67+
rbt.move_l(waypoints[0], speed=SPEED, wait=True)
68+
rbt.move_p(waypoints, speed=SPEED, wait=True)
69+
70+
# Sine wave through all three circle centers (bottom to top) using move_s
71+
SINE_POINTS = 36
72+
z_min, z_max = CENTERS[2][2], CENTERS[0][2]
73+
spline = []
74+
for i in range(SINE_POINTS + 1):
75+
t = i / SINE_POINTS
76+
z = z_min + t * (z_max - z_min)
77+
x = RADIUS * math.cos(t * 3 * 2 * math.pi)
78+
spline.append([x, CIRCLE_Y, z] + ORIENTATION)
79+
rbt.move_s(spline, speed=SPEED, wait=True)
80+
81+
# ── Zig-zag scan ─────────────────────────────────────────────────────
82+
ZZ_ORI = [-180, -90, -180]
83+
ROWS = 6
84+
Y_MIN, Y_MAX = 0, 160
85+
Z_MIN, Z_MAX = 200, 300
86+
X = 280
87+
BLEND = 15
88+
89+
rbt.move_j(pose=[X, 0, 334] + ZZ_ORI, speed=0.5, wait=True)
90+
rbt.move_l([X, Y_MIN, Z_MAX + 30] + ZZ_ORI, speed=0.5, wait=True)
91+
z_step = (Z_MAX - Z_MIN) / (ROWS - 1)
92+
for row in range(ROWS):
93+
z = Z_MAX - row * z_step
94+
is_last = row == ROWS - 1
95+
y_start, y_end = (Y_MIN, Y_MAX) if row % 2 == 0 else (Y_MAX, Y_MIN)
96+
rbt.move_l([X, y_start, z] + ZZ_ORI, speed=0.5, r=BLEND, wait=False)
97+
rbt.move_l(
98+
[X, y_end, z] + ZZ_ORI, speed=0.5, r=0 if is_last else BLEND, wait=False
99+
)
100+
rbt.wait_motion()
101+
102+
# ── Precision demo: pencil pick-up and TCP-offset rotations ──────────
103+
# Home first — orientation flip from zigzag end requires fresh joint config.
104+
rbt.home(wait=True)
105+
PRECISION_POSE = [0, -250, 350, -90, 0, -90]
106+
rbt.move_j(pose=PRECISION_POSE, speed=0.5, wait=True)
107+
108+
# Test gripper: two quick close/open cycles
109+
rbt.tool.close(speed=1.0)
110+
rbt.tool.open(speed=1.0)
111+
rbt.tool.close(speed=1.0)
112+
rbt.tool.open(speed=1.0)
113+
114+
# Approach pencil: move_j to 100mm above, descend linearly, grab, retract
115+
PENCIL_ABOVE = [-90, -81.6, 161.8, 0, -69.4, 180]
116+
rbt.move_j(angles=PENCIL_ABOVE, speed=0.3, wait=True)
117+
rbt.move_l([0, 0, -100, 0, 0, 0], rel=True, speed=0.2, wait=True)
118+
rbt.tool.close(wait=True)
119+
rbt.move_l([0, 0, 100, 0, 0, 0], rel=True, speed=0.2, wait=True)
120+
rbt.move_j(pose=PRECISION_POSE, speed=0.3, wait=True)
121+
122+
# Offset TCP to pencil tip (~100mm exposed below gripper)
123+
rbt.set_tcp_offset(-100, 0, 0)
124+
125+
# Pencil tip traces straight lines (linear precision demo)
126+
# Forward/back (tool Z = world -Y at this pose)
127+
rbt.move_l([0, 0, 100, 0, 0, 0], speed=0.3, frame="TRF", rel=True, wait=True)
128+
rbt.move_l([0, 0, -200, 0, 0, 0], speed=0.3, frame="TRF", rel=True, wait=True)
129+
rbt.move_l([0, 0, 100, 0, 0, 0], speed=0.3, frame="TRF", rel=True, wait=True)
130+
# Side to side (tool Y = world -X at this pose)
131+
rbt.move_l([0, 60, 0, 0, 0, 0], speed=0.3, frame="TRF", rel=True, wait=True)
132+
rbt.move_l([0, -120, 0, 0, 0, 0], speed=0.3, frame="TRF", rel=True, wait=True)
133+
rbt.move_l([0, 60, 0, 0, 0, 0], speed=0.3, frame="TRF", rel=True, wait=True)
134+
135+
# Precision TRF rotations — pencil tip stays stationary while wrist rotates
136+
SWEEP = 20
137+
for axis in range(3):
138+
delta = [0, 0, 0, 0, 0, 0]
139+
delta[3 + axis] = -SWEEP
140+
rbt.move_l(delta, speed=0.5, frame="TRF", rel=True, wait=True)
141+
delta[3 + axis] = SWEEP
142+
rbt.move_l(delta, speed=0.5, frame="TRF", rel=True, wait=True)
143+
rbt.move_l(delta, speed=0.5, frame="TRF", rel=True, wait=True)
144+
delta[3 + axis] = -SWEEP
145+
rbt.move_l(delta, speed=0.5, frame="TRF", rel=True, wait=True)
146+
147+
# Place pencil back: descend linearly, release, retract
148+
rbt.set_tcp_offset(0, 0, 0)
149+
rbt.move_j(angles=PENCIL_ABOVE, speed=0.3, wait=True)
150+
rbt.move_l([0, 0, -100, 0, 0, 0], rel=True, speed=0.2, wait=True)
151+
rbt.tool.open(wait=True)
152+
rbt.move_l([0, 0, 100, 0, 0, 0], rel=True, speed=0.2, wait=True)
153+
154+
# Return and finish
155+
rbt.move_j(pose=PRECISION_POSE, speed=0.3, wait=True)
156+
rbt.home(wait=True)
157+
print("Done!")

‎examples/draw_circle.py‎

Lines changed: 43 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,11 @@
1-
"""Draw circles with different curved motion commands.
1+
"""Curved motion commands: move_c, move_p, and move_s.
22
3-
Draws two circles side by side, each using a different curve method
4-
to show the range of options:
5-
1. Two move_c arcs (half-circles joined)
6-
2. move_p through computed waypoints (constant TCP speed)
7-
Then a move_s spline threads through both circles.
8-
Runs in the built-in simulator.
3+
Draws three circles using progressively more flexible curve commands,
4+
then connects their centers with a sine-wave spline:
5+
1. Full circle via single move_c (start = end)
6+
2. Two half-circle move_c arcs
7+
3. Computed 12-point waypoints via move_p
8+
4. Sine-wave spline (move_s) through all three centers
99
1010
Run:
1111
python examples/draw_circle.py
@@ -18,51 +18,63 @@
1818
HOST = "127.0.0.1"
1919
PORT = 5001
2020

21-
ORIENTATION = [90, 0, 90]
22-
RADIUS = 20
21+
RADIUS = 30
2322
SPEED = 0.4
24-
25-
# Two circles near center of workspace (close to home position)
26-
CENTERS = [(-25, 240, 240), (25, 240, 240)]
23+
CIRCLE_Y = 340
24+
ORIENTATION = [90, 0, 90]
25+
CENTERS = [(0, CIRCLE_Y, 280), (0, CIRCLE_Y, 210), (0, CIRCLE_Y, 140)]
2726

2827

29-
def pt(cx: float, cz: float, angle_deg: float) -> list[float]:
28+
def circle_pt(cx, cz, angle_deg):
29+
"""Circle in the XZ plane (vertical) at fixed Y."""
3030
a = math.radians(angle_deg)
31-
return [cx + RADIUS * math.cos(a), 240, cz + RADIUS * math.sin(a)] + ORIENTATION
31+
return [
32+
cx + RADIUS * math.cos(a),
33+
CIRCLE_Y,
34+
cz + RADIUS * math.sin(a),
35+
] + ORIENTATION
3236

3337

3438
with Robot(host=HOST, port=PORT, normalize_logs=True):
3539
rbt = RobotClient(host=HOST, port=PORT, timeout=2.0)
3640
rbt.wait_ready(timeout=5.0)
3741
rbt.simulator(True)
3842

39-
print("Homing...")
4043
rbt.home(wait=True)
4144

42-
# -- Circle 1: two move_c arcs (half-circles) --
45+
# Circle 1: full circle with a single move_c (start = end)
4346
cx, _, cz = CENTERS[0]
44-
print(f"\nCircle 1 (two move_c arcs) at X={cx}mm")
45-
rbt.move_l(pt(cx, cz, 0), speed=SPEED, wait=True)
46-
rbt.move_c(via=pt(cx, cz, 90), end=pt(cx, cz, 180), speed=SPEED, wait=True)
47-
rbt.move_c(via=pt(cx, cz, 270), end=pt(cx, cz, 0), speed=SPEED, wait=True)
47+
rbt.move_j(pose=circle_pt(cx, cz, 0), speed=0.5, wait=True)
48+
rbt.move_c(
49+
via=circle_pt(cx, cz, 180), end=circle_pt(cx, cz, 0), speed=SPEED, wait=True
50+
)
4851

49-
# -- Circle 2: move_p through 12 waypoints (constant TCP speed) --
52+
# Circle 2: two half-circle move_c arcs
5053
cx, _, cz = CENTERS[1]
51-
print(f"Circle 2 (move_p, 12 waypoints) at X={cx}mm")
52-
waypoints = [pt(cx, cz, i * 30) for i in range(12)]
54+
rbt.move_l(circle_pt(cx, cz, 0), speed=SPEED, wait=True)
55+
rbt.move_c(
56+
via=circle_pt(cx, cz, 90), end=circle_pt(cx, cz, 180), speed=SPEED, wait=True
57+
)
58+
rbt.move_c(
59+
via=circle_pt(cx, cz, 270), end=circle_pt(cx, cz, 0), speed=SPEED, wait=True
60+
)
61+
62+
# Circle 3: computed 12-point waypoints with move_p
63+
cx, _, cz = CENTERS[2]
64+
waypoints = [circle_pt(cx, cz, i * 30) for i in range(12)]
5365
waypoints.append(waypoints[0])
5466
rbt.move_l(waypoints[0], speed=SPEED, wait=True)
5567
rbt.move_p(waypoints, speed=SPEED, wait=True)
5668

57-
# -- Finale: move_s spline threading through both circles --
58-
print("Spline (move_s) threading through both circles...")
69+
# Sine-wave spline through all three circle centers (bottom to top)
70+
SINE_POINTS = 36
71+
z_min, z_max = CENTERS[2][2], CENTERS[0][2]
5972
spline = []
60-
for cx, _, cz in CENTERS:
61-
for angle in range(0, 360, 45):
62-
spline.append(pt(cx, cz, angle))
63-
spline.append(spline[0])
64-
65-
rbt.move_l(spline[0], speed=SPEED, wait=True)
73+
for i in range(SINE_POINTS + 1):
74+
t = i / SINE_POINTS
75+
z = z_min + t * (z_max - z_min)
76+
x = RADIUS * math.cos(t * 3 * 2 * math.pi)
77+
spline.append([x, CIRCLE_Y, z] + ORIENTATION)
6678
rbt.move_s(spline, speed=SPEED, wait=True)
6779

6880
rbt.home(wait=True)

‎examples/pick_and_place.py‎

Lines changed: 0 additions & 62 deletions
This file was deleted.

0 commit comments

Comments
 (0)