PAROL6-python-API/examples/draw_circle.py at main · PCrnjak/PAROL6-python-API · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
"""Curved motion commands: move_c, move_p, and move_s.

Draws three circles using progressively more flexible curve commands,
then connects their centers with a sine-wave spline:
  1. Full circle via single move_c (start = end)
  2. Two half-circle move_c arcs
  3. Computed 12-point waypoints via move_p
  4. Sine-wave spline (move_s) through all three centers

Run:
    python examples/draw_circle.py
"""

import math

from parol6 import Robot, RobotClient

HOST = "127.0.0.1"
PORT = 5001

RADIUS = 30
SPEED = 0.4
CIRCLE_Y = 340
ORIENTATION = [90, 0, 90]
CENTERS = [(0, CIRCLE_Y, 280), (0, CIRCLE_Y, 210), (0, CIRCLE_Y, 140)]


def circle_pt(cx, cz, angle_deg):
    """Circle in the XZ plane (vertical) at fixed Y."""
    a = math.radians(angle_deg)
    return [
        cx + RADIUS * math.cos(a),
        CIRCLE_Y,
        cz + RADIUS * math.sin(a),
    ] + ORIENTATION


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

    rbt.home(wait=True)

    # Circle 1: full circle with a single move_c (start = end)
    cx, _, cz = CENTERS[0]
    rbt.move_j(pose=circle_pt(cx, cz, 0), speed=0.5, wait=True)
    rbt.move_c(
        via=circle_pt(cx, cz, 180), end=circle_pt(cx, cz, 0), speed=SPEED, wait=True
    )

    # Circle 2: two half-circle move_c arcs
    cx, _, cz = CENTERS[1]
    rbt.move_l(circle_pt(cx, cz, 0), speed=SPEED, wait=True)
    rbt.move_c(
        via=circle_pt(cx, cz, 90), end=circle_pt(cx, cz, 180), speed=SPEED, wait=True
    )
    rbt.move_c(
        via=circle_pt(cx, cz, 270), end=circle_pt(cx, cz, 0), speed=SPEED, wait=True
    )

    # Circle 3: computed 12-point waypoints with move_p
    cx, _, cz = CENTERS[2]
    waypoints = [circle_pt(cx, cz, i * 30) for i in range(12)]
    waypoints.append(waypoints[0])
    rbt.move_l(waypoints[0], speed=SPEED, wait=True)
    rbt.move_p(waypoints, speed=SPEED, wait=True)

    # Sine-wave spline through all three circle centers (bottom to top)
    SINE_POINTS = 36
    z_min, z_max = CENTERS[2][2], CENTERS[0][2]
    spline = []
    for i in range(SINE_POINTS + 1):
        t = i / SINE_POINTS
        z = z_min + t * (z_max - z_min)
        x = RADIUS * math.cos(t * 3 * 2 * math.pi)
        spline.append([x, CIRCLE_Y, z] + ORIENTATION)
    rbt.move_s(spline, speed=SPEED, wait=True)

    rbt.home(wait=True)
    print("Done!")