Skip to content

kinematics.machine

toolpath_engine.kinematics.machine.Joint dataclass

Base class for kinematic joints.

Source code in utde_v0.1.0/toolpath_engine/kinematics/machine.py 
22
23
24
25
26
27
28
29
30
31
32
33
@dataclass
class Joint:
    """Base class for kinematic joints."""
    name: str
    axis: Vector3 = field(default_factory=lambda: Vector3(0, 0, 1))
    limits: Optional[Tuple[float, float]] = None  # None = unlimited
    home: float = 0.0
    value: float = 0.0  # current joint value

    def transform_matrix(self) -> np.ndarray:
        """4x4 transform for current joint value."""
        raise NotImplementedError

transform_matrix()

4x4 transform for current joint value.

Source code in utde_v0.1.0/toolpath_engine/kinematics/machine.py 
31
32
33
def transform_matrix(self) -> np.ndarray:
    """4x4 transform for current joint value."""
    raise NotImplementedError

toolpath_engine.kinematics.machine.Linear dataclass

Bases: Joint

A linear (prismatic) joint — translates along its axis.

Source code in utde_v0.1.0/toolpath_engine/kinematics/machine.py 
36
37
38
39
40
41
42
43
44
@dataclass
class Linear(Joint):
    """A linear (prismatic) joint — translates along its axis."""

    def transform_matrix(self) -> np.ndarray:
        m = np.eye(4)
        a = self.axis.to_array()
        m[:3, 3] = a * self.value
        return m

toolpath_engine.kinematics.machine.Rotary dataclass

Bases: Joint

A rotary joint — rotates around its axis. Value in degrees.

Source code in utde_v0.1.0/toolpath_engine/kinematics/machine.py 
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
@dataclass
class Rotary(Joint):
    """A rotary joint — rotates around its axis. Value in degrees."""

    def transform_matrix(self) -> np.ndarray:
        angle = math.radians(self.value)
        a = self.axis.normalized().to_array()
        c, s = math.cos(angle), math.sin(angle)
        t = 1 - c
        x, y, z = a

        m = np.eye(4)
        m[0, 0] = t * x * x + c
        m[0, 1] = t * x * y - s * z
        m[0, 2] = t * x * z + s * y
        m[1, 0] = t * x * y + s * z
        m[1, 1] = t * y * y + c
        m[1, 2] = t * y * z - s * x
        m[2, 0] = t * x * z - s * y
        m[2, 1] = t * y * z + s * x
        m[2, 2] = t * z * z + c
        return m

A rigid link between joints, defined by a fixed transform.

Source code in utde_v0.1.0/toolpath_engine/kinematics/machine.py 
71
72
73
74
75
76
77
78
79
80
81
82
@dataclass
class Link:
    """A rigid link between joints, defined by a fixed transform."""
    offset: Vector3 = field(default_factory=Vector3)
    rotation: Optional[np.ndarray] = None  # 3x3 rotation matrix

    def transform_matrix(self) -> np.ndarray:
        m = np.eye(4)
        if self.rotation is not None:
            m[:3, :3] = self.rotation
        m[:3, 3] = self.offset.to_array()
        return m

toolpath_engine.kinematics.machine.KinematicChain

A chain of joints and links from base to end effector.

Source code in utde_v0.1.0/toolpath_engine/kinematics/machine.py 
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
class KinematicChain:
    """
    A chain of joints and links from base to end effector.
    """

    def __init__(self, name: str = "chain"):
        self.name = name
        self.joints: List[Joint] = []
        self.links: List[Link] = []  # link[i] is the fixed transform BEFORE joint[i]

    def add_joint(self, joint: Joint, link: Optional[Link] = None):
        if link is None:
            link = Link()
        self.links.append(link)
        self.joints.append(joint)

    def forward_kinematics(self, joint_values: Optional[List[float]] = None) -> np.ndarray:
        """
        Compute the 4x4 end-effector transform given joint values.
        If joint_values is None, uses current joint values.
        """
        if joint_values is not None:
            for j, val in zip(self.joints, joint_values):
                j.value = val

        m = np.eye(4)
        for link, joint in zip(self.links, self.joints):
            m = m @ link.transform_matrix() @ joint.transform_matrix()
        return m

    def get_joint_values(self) -> List[float]:
        return [j.value for j in self.joints]

    def set_joint_values(self, values: List[float]):
        for j, v in zip(self.joints, values):
            j.value = v

    def joint_names(self) -> List[str]:
        return [j.name for j in self.joints]

forward_kinematics(joint_values=None)

Compute the 4x4 end-effector transform given joint values. If joint_values is None, uses current joint values.

Source code in utde_v0.1.0/toolpath_engine/kinematics/machine.py 
101
102
103
104
105
106
107
108
109
110
111
112
113
def forward_kinematics(self, joint_values: Optional[List[float]] = None) -> np.ndarray:
    """
    Compute the 4x4 end-effector transform given joint values.
    If joint_values is None, uses current joint values.
    """
    if joint_values is not None:
        for j, val in zip(self.joints, joint_values):
            j.value = val

    m = np.eye(4)
    for link, joint in zip(self.links, self.joints):
        m = m @ link.transform_matrix() @ joint.transform_matrix()
    return m

toolpath_engine.kinematics.machine.Machine

Complete machine definition with kinematics, tool offset, and configuration.

Supports two kinematic chains: tool-side and workpiece-side. For simple machines (3-axis gantry), only the tool chain is needed.

Source code in utde_v0.1.0/toolpath_engine/kinematics/machine.py 
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
class Machine:
    """
    Complete machine definition with kinematics, tool offset, and configuration.

    Supports two kinematic chains: tool-side and workpiece-side.
    For simple machines (3-axis gantry), only the tool chain is needed.
    """

    def __init__(self, name: str = "machine"):
        self.name = name
        self.tool_chain = KinematicChain("tool")
        self.workpiece_chain = KinematicChain("workpiece")
        self.tool_offset = Vector3(0, 0, 0)
        self.config: Dict[str, Any] = {}

    def add_joint(self, joint: Joint, link: Optional[Link] = None, chain: str = "tool"):
        """Add a joint to the specified chain."""
        if chain == "tool":
            self.tool_chain.add_joint(joint, link)
        else:
            self.workpiece_chain.add_joint(joint, link)

    def set_tool_offset(self, x: float = 0, y: float = 0, z: float = 0):
        self.tool_offset = Vector3(x, y, z)

    def forward_kinematics(self, joint_values: Optional[Dict[str, float]] = None) -> Tuple[Vector3, Vector3]:
        """
        Compute tool tip position and tool axis direction.
        Returns (position, tool_axis) in world coordinates.
        """
        if joint_values:
            tool_vals = [joint_values.get(j.name, j.value) for j in self.tool_chain.joints]
            wp_vals = [joint_values.get(j.name, j.value) for j in self.workpiece_chain.joints]
        else:
            tool_vals = None
            wp_vals = None

        # Tool chain: base → tool tip
        tool_m = self.tool_chain.forward_kinematics(tool_vals)

        # Apply tool offset
        offset_m = np.eye(4)
        offset_m[:3, 3] = self.tool_offset.to_array()
        tool_m = tool_m @ offset_m

        # Workpiece chain: base → workpiece (inverted, since we want world → workpiece)
        if self.workpiece_chain.joints:
            wp_m = self.workpiece_chain.forward_kinematics(wp_vals)
            # Tool tip in workpiece frame
            wp_inv = np.linalg.inv(wp_m)
            final_m = wp_inv @ tool_m
        else:
            final_m = tool_m

        position = Vector3.from_array(final_m[:3, 3])
        tool_axis = Vector3.from_array(final_m[:3, 2])  # Z axis of tool frame

        return position, tool_axis

    def inverse_kinematics(
        self,
        target_pos: Vector3,
        target_orient: Orientation,
        initial_guess: Optional[Dict[str, float]] = None,
    ) -> Dict[str, float]:
        """
        Numerical IK solver: find joint values that place the tool tip
        at target_pos with tool axis along target_orient.

        Uses scipy.optimize.minimize with joint limit constraints.
        """
        all_joints = self.tool_chain.joints + self.workpiece_chain.joints
        n = len(all_joints)

        if n == 0:
            return {}

        # Initial guess
        x0 = np.zeros(n)
        if initial_guess:
            for i, j in enumerate(all_joints):
                x0[i] = initial_guess.get(j.name, j.home)
        else:
            x0 = np.array([j.home for j in all_joints])

        target_p = target_pos.to_array()
        target_d = target_orient.to_array()

        def objective(x):
            # Set joint values
            jv = {}
            for i, j in enumerate(all_joints):
                jv[j.name] = x[i]
            pos, axis = self.forward_kinematics(jv)
            pos_err = np.linalg.norm(pos.to_array() - target_p)
            dir_err = np.linalg.norm(axis.normalized().to_array() - target_d)
            return pos_err ** 2 + (dir_err * 100) ** 2  # weight orientation

        # Bounds from joint limits
        bounds = []
        for j in all_joints:
            if j.limits:
                bounds.append(j.limits)
            elif isinstance(j, Rotary):
                bounds.append((-360, 360))
            else:
                bounds.append((-10000, 10000))

        result = minimize(objective, x0, method="L-BFGS-B", bounds=bounds,
                          options={"maxiter": 500, "ftol": 1e-12})

        solution = {}
        for i, j in enumerate(all_joints):
            solution[j.name] = result.x[i]
        return solution

    def check_limits(self, joint_values: Dict[str, float]) -> List[str]:
        """Check if joint values are within limits. Returns list of violations."""
        violations = []
        all_joints = self.tool_chain.joints + self.workpiece_chain.joints
        for j in all_joints:
            if j.name in joint_values and j.limits:
                val = joint_values[j.name]
                if val < j.limits[0] or val > j.limits[1]:
                    violations.append(
                        f"{j.name}: {val:.3f} outside [{j.limits[0]}, {j.limits[1]}]"
                    )
        return violations

    # --- serialization -------------------------------------------------------
    def to_dict(self) -> Dict:
        """Serialize machine to a dictionary (for YAML export)."""
        def joint_to_dict(j):
            d = {
                "name": j.name,
                "type": "linear" if isinstance(j, Linear) else "rotary",
                "axis": [j.axis.x, j.axis.y, j.axis.z],
                "home": j.home,
            }
            if j.limits:
                d["limits"] = list(j.limits)
            return d

        return {
            "name": self.name,
            "tool_chain": [joint_to_dict(j) for j in self.tool_chain.joints],
            "workpiece_chain": [joint_to_dict(j) for j in self.workpiece_chain.joints],
            "tool_offset": [self.tool_offset.x, self.tool_offset.y, self.tool_offset.z],
            "config": self.config,
        }

    def to_yaml(self) -> str:
        return yaml.dump(self.to_dict(), default_flow_style=False, sort_keys=False)

    @classmethod
    def from_dict(cls, data: Dict) -> "Machine":
        m = cls(data.get("name", "machine"))

        for jd in data.get("tool_chain", []):
            JointCls = Linear if jd["type"] == "linear" else Rotary
            j = JointCls(
                name=jd["name"],
                axis=Vector3(*jd["axis"]),
                limits=tuple(jd["limits"]) if "limits" in jd else None,
                home=jd.get("home", 0),
            )
            m.add_joint(j, chain="tool")

        for jd in data.get("workpiece_chain", []):
            JointCls = Linear if jd["type"] == "linear" else Rotary
            j = JointCls(
                name=jd["name"],
                axis=Vector3(*jd["axis"]),
                limits=tuple(jd["limits"]) if "limits" in jd else None,
                home=jd.get("home", 0),
            )
            m.add_joint(j, chain="workpiece")

        if "tool_offset" in data:
            m.set_tool_offset(*data["tool_offset"])

        m.config = data.get("config", {})
        return m

    @classmethod
    def from_yaml(cls, yaml_str: str) -> "Machine":
        data = yaml.safe_load(yaml_str)
        return cls.from_dict(data)

    # --- common machine presets ----------------------------------------------
    @classmethod
    def cartesian_3axis(cls, name="3axis_gantry", travel=(500, 500, 400)) -> "Machine":
        """Standard XYZ cartesian gantry."""
        m = cls(name)
        m.add_joint(Linear("X", Vector3(1, 0, 0), limits=(0, travel[0])))
        m.add_joint(Linear("Y", Vector3(0, 1, 0), limits=(0, travel[1])))
        m.add_joint(Linear("Z", Vector3(0, 0, 1), limits=(0, travel[2])))
        return m

    @classmethod
    def gantry_5axis_ac(cls, name="5axis_AC", travel=(500, 500, 400),
                        a_limits=(-120, 120)) -> "Machine":
        """5-axis gantry with AC rotary table."""
        m = cls(name)
        # Gantry (tool side)
        m.add_joint(Linear("X", Vector3(1, 0, 0), limits=(0, travel[0])))
        m.add_joint(Linear("Y", Vector3(0, 1, 0), limits=(0, travel[1])))
        m.add_joint(Linear("Z", Vector3(0, 0, 1), limits=(0, travel[2])))
        # Rotary table (workpiece side)
        m.add_joint(Rotary("A", Vector3(1, 0, 0), limits=a_limits), chain="workpiece")
        m.add_joint(Rotary("C", Vector3(0, 0, 1), limits=None), chain="workpiece")
        return m

    @classmethod
    def gantry_5axis_bc(cls, name="5axis_BC", travel=(500, 500, 400),
                        b_limits=(-120, 120)) -> "Machine":
        """5-axis gantry with BC rotary table."""
        m = cls(name)
        m.add_joint(Linear("X", Vector3(1, 0, 0), limits=(0, travel[0])))
        m.add_joint(Linear("Y", Vector3(0, 1, 0), limits=(0, travel[1])))
        m.add_joint(Linear("Z", Vector3(0, 0, 1), limits=(0, travel[2])))
        m.add_joint(Rotary("B", Vector3(0, 1, 0), limits=b_limits), chain="workpiece")
        m.add_joint(Rotary("C", Vector3(0, 0, 1), limits=None), chain="workpiece")
        return m

    def __repr__(self):
        tool_joints = ", ".join(j.name for j in self.tool_chain.joints)
        wp_joints = ", ".join(j.name for j in self.workpiece_chain.joints)
        return f"Machine('{self.name}', tool=[{tool_joints}], workpiece=[{wp_joints}])"

add_joint(joint, link=None, chain='tool')

Add a joint to the specified chain.

Source code in utde_v0.1.0/toolpath_engine/kinematics/machine.py 
141
142
143
144
145
146
def add_joint(self, joint: Joint, link: Optional[Link] = None, chain: str = "tool"):
    """Add a joint to the specified chain."""
    if chain == "tool":
        self.tool_chain.add_joint(joint, link)
    else:
        self.workpiece_chain.add_joint(joint, link)

cartesian_3axis(name='3axis_gantry', travel=(500, 500, 400)) classmethod

Standard XYZ cartesian gantry.

Source code in utde_v0.1.0/toolpath_engine/kinematics/machine.py 
316
317
318
319
320
321
322
323
@classmethod
def cartesian_3axis(cls, name="3axis_gantry", travel=(500, 500, 400)) -> "Machine":
    """Standard XYZ cartesian gantry."""
    m = cls(name)
    m.add_joint(Linear("X", Vector3(1, 0, 0), limits=(0, travel[0])))
    m.add_joint(Linear("Y", Vector3(0, 1, 0), limits=(0, travel[1])))
    m.add_joint(Linear("Z", Vector3(0, 0, 1), limits=(0, travel[2])))
    return m

check_limits(joint_values)

Check if joint values are within limits. Returns list of violations.

Source code in utde_v0.1.0/toolpath_engine/kinematics/machine.py 
242
243
244
245
246
247
248
249
250
251
252
253
def check_limits(self, joint_values: Dict[str, float]) -> List[str]:
    """Check if joint values are within limits. Returns list of violations."""
    violations = []
    all_joints = self.tool_chain.joints + self.workpiece_chain.joints
    for j in all_joints:
        if j.name in joint_values and j.limits:
            val = joint_values[j.name]
            if val < j.limits[0] or val > j.limits[1]:
                violations.append(
                    f"{j.name}: {val:.3f} outside [{j.limits[0]}, {j.limits[1]}]"
                )
    return violations

forward_kinematics(joint_values=None)

Compute tool tip position and tool axis direction. Returns (position, tool_axis) in world coordinates.

Source code in utde_v0.1.0/toolpath_engine/kinematics/machine.py 
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
def forward_kinematics(self, joint_values: Optional[Dict[str, float]] = None) -> Tuple[Vector3, Vector3]:
    """
    Compute tool tip position and tool axis direction.
    Returns (position, tool_axis) in world coordinates.
    """
    if joint_values:
        tool_vals = [joint_values.get(j.name, j.value) for j in self.tool_chain.joints]
        wp_vals = [joint_values.get(j.name, j.value) for j in self.workpiece_chain.joints]
    else:
        tool_vals = None
        wp_vals = None

    # Tool chain: base → tool tip
    tool_m = self.tool_chain.forward_kinematics(tool_vals)

    # Apply tool offset
    offset_m = np.eye(4)
    offset_m[:3, 3] = self.tool_offset.to_array()
    tool_m = tool_m @ offset_m

    # Workpiece chain: base → workpiece (inverted, since we want world → workpiece)
    if self.workpiece_chain.joints:
        wp_m = self.workpiece_chain.forward_kinematics(wp_vals)
        # Tool tip in workpiece frame
        wp_inv = np.linalg.inv(wp_m)
        final_m = wp_inv @ tool_m
    else:
        final_m = tool_m

    position = Vector3.from_array(final_m[:3, 3])
    tool_axis = Vector3.from_array(final_m[:3, 2])  # Z axis of tool frame

    return position, tool_axis

gantry_5axis_ac(name='5axis_AC', travel=(500, 500, 400), a_limits=(-120, 120)) classmethod

5-axis gantry with AC rotary table.

Source code in utde_v0.1.0/toolpath_engine/kinematics/machine.py 
325
326
327
328
329
330
331
332
333
334
335
336
337
@classmethod
def gantry_5axis_ac(cls, name="5axis_AC", travel=(500, 500, 400),
                    a_limits=(-120, 120)) -> "Machine":
    """5-axis gantry with AC rotary table."""
    m = cls(name)
    # Gantry (tool side)
    m.add_joint(Linear("X", Vector3(1, 0, 0), limits=(0, travel[0])))
    m.add_joint(Linear("Y", Vector3(0, 1, 0), limits=(0, travel[1])))
    m.add_joint(Linear("Z", Vector3(0, 0, 1), limits=(0, travel[2])))
    # Rotary table (workpiece side)
    m.add_joint(Rotary("A", Vector3(1, 0, 0), limits=a_limits), chain="workpiece")
    m.add_joint(Rotary("C", Vector3(0, 0, 1), limits=None), chain="workpiece")
    return m

gantry_5axis_bc(name='5axis_BC', travel=(500, 500, 400), b_limits=(-120, 120)) classmethod

5-axis gantry with BC rotary table.

Source code in utde_v0.1.0/toolpath_engine/kinematics/machine.py 
339
340
341
342
343
344
345
346
347
348
349
@classmethod
def gantry_5axis_bc(cls, name="5axis_BC", travel=(500, 500, 400),
                    b_limits=(-120, 120)) -> "Machine":
    """5-axis gantry with BC rotary table."""
    m = cls(name)
    m.add_joint(Linear("X", Vector3(1, 0, 0), limits=(0, travel[0])))
    m.add_joint(Linear("Y", Vector3(0, 1, 0), limits=(0, travel[1])))
    m.add_joint(Linear("Z", Vector3(0, 0, 1), limits=(0, travel[2])))
    m.add_joint(Rotary("B", Vector3(0, 1, 0), limits=b_limits), chain="workpiece")
    m.add_joint(Rotary("C", Vector3(0, 0, 1), limits=None), chain="workpiece")
    return m

inverse_kinematics(target_pos, target_orient, initial_guess=None)

Numerical IK solver: find joint values that place the tool tip at target_pos with tool axis along target_orient.

Uses scipy.optimize.minimize with joint limit constraints.

Source code in utde_v0.1.0/toolpath_engine/kinematics/machine.py 
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
def inverse_kinematics(
    self,
    target_pos: Vector3,
    target_orient: Orientation,
    initial_guess: Optional[Dict[str, float]] = None,
) -> Dict[str, float]:
    """
    Numerical IK solver: find joint values that place the tool tip
    at target_pos with tool axis along target_orient.

    Uses scipy.optimize.minimize with joint limit constraints.
    """
    all_joints = self.tool_chain.joints + self.workpiece_chain.joints
    n = len(all_joints)

    if n == 0:
        return {}

    # Initial guess
    x0 = np.zeros(n)
    if initial_guess:
        for i, j in enumerate(all_joints):
            x0[i] = initial_guess.get(j.name, j.home)
    else:
        x0 = np.array([j.home for j in all_joints])

    target_p = target_pos.to_array()
    target_d = target_orient.to_array()

    def objective(x):
        # Set joint values
        jv = {}
        for i, j in enumerate(all_joints):
            jv[j.name] = x[i]
        pos, axis = self.forward_kinematics(jv)
        pos_err = np.linalg.norm(pos.to_array() - target_p)
        dir_err = np.linalg.norm(axis.normalized().to_array() - target_d)
        return pos_err ** 2 + (dir_err * 100) ** 2  # weight orientation

    # Bounds from joint limits
    bounds = []
    for j in all_joints:
        if j.limits:
            bounds.append(j.limits)
        elif isinstance(j, Rotary):
            bounds.append((-360, 360))
        else:
            bounds.append((-10000, 10000))

    result = minimize(objective, x0, method="L-BFGS-B", bounds=bounds,
                      options={"maxiter": 500, "ftol": 1e-12})

    solution = {}
    for i, j in enumerate(all_joints):
        solution[j.name] = result.x[i]
    return solution

to_dict()

Serialize machine to a dictionary (for YAML export).

Source code in utde_v0.1.0/toolpath_engine/kinematics/machine.py 
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
def to_dict(self) -> Dict:
    """Serialize machine to a dictionary (for YAML export)."""
    def joint_to_dict(j):
        d = {
            "name": j.name,
            "type": "linear" if isinstance(j, Linear) else "rotary",
            "axis": [j.axis.x, j.axis.y, j.axis.z],
            "home": j.home,
        }
        if j.limits:
            d["limits"] = list(j.limits)
        return d

    return {
        "name": self.name,
        "tool_chain": [joint_to_dict(j) for j in self.tool_chain.joints],
        "workpiece_chain": [joint_to_dict(j) for j in self.workpiece_chain.joints],
        "tool_offset": [self.tool_offset.x, self.tool_offset.y, self.tool_offset.z],
        "config": self.config,
    }