Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions .gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -9,3 +9,6 @@ __pycache__/
.pytest_cache/
.ruff_cache/
.DS_Store

# mkdocs build output
site/
8 changes: 8 additions & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,16 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0

### Added

* Examples `07_any_6_axis_arm.py` (an ABB IRB 4600 printing a vase, no kinematics solver) and `08_which_ik.py` (which IK each robot gets).

* `NativePlanner.has_closed_form_ik`: whether IK for the current robot cell is solved in closed form or numerically.

### Changed

* Examples are shorter: no timing code, simpler comments.
* The README and the planner docstring show `NativePlanner()` with any robot; the UR solver is an option for UR arms.
* Copyright year is 2026.

### Removed


Expand Down
2 changes: 1 addition & 1 deletion LICENSE
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
MIT License

Copyright (c) 2018-2026 Gramazio Kohler Research
Copyright (c) 2026 Gramazio Kohler Research

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
Expand Down
2 changes: 1 addition & 1 deletion NOTICE
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
compas_fab_native
Copyright (c) 2018-2026 Gramazio Kohler Research
Copyright (c) 2026 Gramazio Kohler Research
Licensed under the MIT License (see LICENSE).

This repository contains code ported from, data derived from, and algorithms that closely
Expand Down
27 changes: 24 additions & 3 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -28,14 +28,35 @@ for Python 3.9 and later. Elsewhere, pip builds from source, which needs a

## Use

It works with any serial robot in a compas_fab `RobotCell`:

```python
from compas_fab.backends import UR5Kinematics
from compas_fab.robots import RobotCell
from compas_fab.robots import RobotCellLibrary
from compas_fab_native import NativePlanner

planner = NativePlanner(UR5Kinematics())
robot_cell, state = RobotCellLibrary.abb_irb4600_40_255()
# or your own robot:
# robot_cell = RobotCell.from_urdf_and_srdf("robot.urdf", "robot.srdf", "path/to/meshes")
# state = robot_cell.default_cell_state()

planner = NativePlanner()
planner.set_robot_cell(robot_cell, state)
solutions = list(planner.iter_inverse_kinematics(target, state))
trajectory = planner.plan_cartesian_motion(waypoints, state)
```

More examples are in [`docs/files/`](docs/files).
The planner picks the inverse kinematics when the cell is set:

- 6-axis arms with a spherical wrist (most ABB, KUKA, Stäubli, Fanuc and Yaskawa arms) get
a closed form, built from the robot model.
- UR arms get a closed form when you pass their compas_fab solver, e.g.
`NativePlanner(UR5Kinematics())`.
- All other robots (7-axis arms, robots on tracks or gantries) get numerical IK.

More examples are in [`docs/files/`](docs/files), among them
[`07_any_6_axis_arm.py`](docs/files/07_any_6_axis_arm.py) (an ABB printing a vase) and
[`08_which_ik.py`](docs/files/08_which_ik.py) (which IK five robots get).

## Repository layout

Expand Down
37 changes: 11 additions & 26 deletions docs/files/01_kinematics.py
Original file line number Diff line number Diff line change
@@ -1,14 +1,11 @@
"""01 - Forward and inverse kinematics with the native planner.
"""01 - Forward and inverse kinematics.

The native planner is a compas_fab backend like any other: you give it a RobotCell, then
ask it for kinematics, collision checks and paths. The difference is underneath: the work
happens in a compiled (Rust) core instead of Python, PyBullet or ROS.
The native planner is a compas_fab backend like any other: give it a robot cell, then ask
for kinematics, collision checks and paths. The work happens in a compiled (Rust) core.

Install the core once with ``pip install .`` from a compas_fab checkout.
Install it with ``pip install compas_fab_native``.
"""

import time

from compas.geometry import Frame
from compas_fab.backends import UR5Kinematics
from compas_fab.robots import FrameTarget
Expand All @@ -17,36 +14,24 @@

from compas_fab_native import NativePlanner

# 1. A robot cell: a UR5 on its own. `state` holds everything that moves: the robot's joint
# values, its base frame, tools and obstacles.
# A UR5. `state` holds what moves: joint values, base frame, tools, obstacles.
robot_cell, state = RobotCellLibrary.ur5()

# 2. The planner. A UR has an offset wrist, so we tell it which closed-form solver to use.
# (Spherical-wrist arms like ABB or Staubli need nothing: the solver is derived from the
# robot model.) `set_robot_cell` sends the robot to the core, fitting collision spheres
# to every link mesh. That takes a moment, once.
# URs need their solver for closed-form IK (example 08 explains why).
# `set_robot_cell` sends the robot to the core.
planner = NativePlanner(UR5Kinematics())
t0 = time.perf_counter()
planner.set_robot_cell(robot_cell, state)
print("set_robot_cell: {:.0f} ms (once per cell)".format((time.perf_counter() - t0) * 1e3))
print("closed-form IK:", planner.has_closed_form_ik)

# 3. Forward kinematics: where is the flange (the robot coordinate frame, RCF) for these joints?
# Forward kinematics: where is the flange for these joint values?
state.robot_configuration.joint_values = [0.0, -1.2, 1.4, -1.8, -1.57, 0.0]
flange = planner.forward_kinematics(state, TargetMode.ROBOT)
print("flange at", flange.point)

# 4. Inverse kinematics: every way the robot can reach a frame. A 6-axis arm has up to 8
# (shoulder left/right x elbow up/down x wrist flip). Collision checking is on by default,
# so solutions where the robot hits itself or an obstacle are dropped.
# Inverse kinematics: every way to reach a frame, up to 8 for a 6-axis arm.
# Solutions that collide are dropped.
target = FrameTarget(Frame([0.4, 0.2, 0.5], [1, 0, 0], [0, -1, 0]), TargetMode.ROBOT)
solutions = list(planner.iter_inverse_kinematics(target, state))
print("{} collision-free IK solutions".format(len(solutions)))
for configuration in solutions:
print(" ", ["{:+.2f}".format(v) for v in configuration.joint_values])

# 5. How fast? Time many IK calls, including the collision check of every solution.
n = 200
t0 = time.perf_counter()
for _ in range(n):
list(planner.iter_inverse_kinematics(target, state))
print("IK + collision check: {:.0f} us per target (mostly Python overhead)".format((time.perf_counter() - t0) / n * 1e6))
49 changes: 20 additions & 29 deletions docs/files/02_collision_checking.py
Original file line number Diff line number Diff line change
@@ -1,13 +1,11 @@
"""02 - Collision checking: spheres for speed, meshes for the final word.
"""02 - Collision checking: spheres for speed, meshes to be sure.

The core checks collisions on a sphere model of the robot: each link is filled with spheres
fitted to its mesh. Sphere checks are cheap enough to test thousands of configurations in
a few milliseconds. Spheres are an approximation, so the core also keeps the exact meshes
and can check any configuration on them.
The core fills each link with spheres fitted to its mesh. Sphere checks are cheap, so
thousands of configurations can be checked at once. Spheres are an approximation, so the
core also keeps the exact meshes.
"""

import random
import time

from compas.geometry import Box
from compas.geometry import Frame
Expand All @@ -19,7 +17,7 @@

from compas_fab_native import NativePlanner

# 1. A UR5 next to a table-sized box.
# A UR5 next to a table.
robot_cell, state = RobotCellLibrary.ur5()
robot_cell.rigid_body_models["table"] = RigidBody.from_mesh(Box(0.6, 0.6, 0.3).to_mesh(triangulated=True))
state = robot_cell.default_cell_state()
Expand All @@ -28,47 +26,40 @@
planner = NativePlanner(UR5Kinematics())
planner.set_robot_cell(robot_cell, state)

# 2. What the core sees. Obstacles are boxes (their bounding boxes, by default). The robot is
# spheres, and some link pairs are never checked: neighbours that touch in every
# configuration (found by sampling, like MoveIt's setup assistant).
# What the core sees: spheres for the robot, boxes for obstacles. Neighbouring links that
# always touch are never checked against each other.
core = planner.client.core
n_spheres, n_self_spheres, n_boxes = core.sphere_counts
print("{} spheres for obstacles, {} for self-collision, {} obstacle boxes".format(n_spheres, n_self_spheres, n_boxes))
print("link pairs never checked:", core.disabled_collisions)

# 3. One configuration: arm reaching down into the table.
# One configuration: the arm reaching down into the table.
state.robot_configuration.joint_values = [0.0, -0.6, 1.6, -1.0, -1.57, 0.0]
try:
planner.check_collision(state)
print("down: free")
except CollisionCheckError as e:
print("down:", e)

# 4. Many configurations at once. `clearance` returns, per configuration, the smallest
# distance between the robot spheres and anything else (negative = penetration).
# Many configurations at once. `clearance` gives the smallest distance to anything, per
# configuration. Negative means collision.
random.seed(0)
configurations = [robot_cell.random_configuration(robot_cell.main_group_name).joint_values for _ in range(10000)]
t0 = time.perf_counter()
clearances = core.clearance(configurations)
elapsed = time.perf_counter() - t0
colliding = sum(c <= 0 for c in clearances)
print("10,000 random configurations in {:.1f} ms: {} in collision".format(elapsed * 1e3, colliding))
print("{} of 10,000 random configurations collide".format(sum(c <= 0 for c in clearances)))

# 5. The exact check, on the link meshes. Slower (about 0.2 ms per configuration) but exact.
# Here we compare both on the first 300 configurations.
# The exact check on the meshes is slower but exact. Compare both on 300 configurations.
sample = configurations[:300]
exact = core.exact_check(sample)
spheres_say = [c <= 0 for c in clearances[:300]]
meshes_say = [contact is not None for contact in exact]
missed = [(q, c) for q, s, m, c in zip(sample, spheres_say, meshes_say, exact) if m and not s]
false_alarms = [q for q, s, m in zip(sample, spheres_say, meshes_say) if s and not m]
print("spheres and meshes disagree on {}/300 configurations:".format(len(missed) + len(false_alarms)))
print(" {} false alarms (spheres say collision, meshes say free)".format(len(false_alarms)))
print(" {} missed contacts (spheres say free, meshes collide), for example:".format(len(missed)))
for q, contact in missed[:3]:
print(" {} touches {}".format(*contact))
missed = [contact for s, m, contact in zip(spheres_say, meshes_say, exact) if m and not s]
false_alarms = sum(s and not m for s, m in zip(spheres_say, meshes_say))
print("spheres say collision, meshes say free: {}".format(false_alarms))
print("spheres say free, meshes collide: {}, for example:".format(len(missed)))
for contact in missed[:3]:
print(" {} touches {}".format(*contact))

# The misses are near-contacts between neighbouring links, mostly the forearm and the wrist,
# where the inscribed spheres leave the mesh surface uncovered. This is why the planner
# validates its final paths on the exact meshes (see 03_cartesian_line.py), and why
# The misses are near-touches between neighbouring links, where spheres leave gaps. That is
# why planned paths are checked on the meshes too (example 03), and why
# `check_collision(state, {"exact": True})` exists.
26 changes: 10 additions & 16 deletions docs/files/03_cartesian_line.py
Original file line number Diff line number Diff line change
@@ -1,13 +1,9 @@
"""03 - A Cartesian path: a straight line of frames.

`plan_cartesian_motion` finds joint configurations for every frame such that consecutive
configurations are close (no sudden flips). The native planner solves IK for every frame
in one go, keeps all the solutions, and picks the smoothest collision-free sequence through
them, rather than following one IK branch and hoping it stays valid.
The planner solves IK for every frame, keeps all solutions, and picks the smoothest
collision-free sequence through them. So it does not get stuck on one IK branch.
"""

import time

from compas.geometry import Frame
from compas_fab.backends import UR5Kinematics
from compas_fab.robots import FrameWaypoints
Expand All @@ -20,27 +16,25 @@
planner = NativePlanner(UR5Kinematics())
planner.set_robot_cell(robot_cell, state)

# 1. 100 frames on a 60 cm line in front of the robot, flange pointing down.
# 100 frames on a 60 cm line, flange pointing down.
frames = [Frame([0.45, -0.3 + 0.6 * i / 99, 0.4], [1, 0, 0], [0, -1, 0]) for i in range(100)]
waypoints = FrameWaypoints(frames, TargetMode.ROBOT)

# 2. Plan. By default the result is also validated on the exact meshes.
t0 = time.perf_counter()
# Plan. The result is also checked on the exact meshes.
trajectory = planner.plan_cartesian_motion(waypoints, state)
print("{} points in {:.1f} ms".format(len(trajectory.points), (time.perf_counter() - t0) * 1e3))
print("{} points".format(len(trajectory.points)))

# 3. What the planner did, from the statistics it attaches to the trajectory.
# What the planner did.
stats = trajectory.attributes["native_stats"]
print("IK solutions considered: {:.0f}, of which collision-free: {:.0f}".format(stats["ik_nodes"], stats["free_nodes"]))
print("configurations checked on the exact meshes: {:.0f}".format(stats["exact_configs_checked"]))
print("IK solutions considered: {:.0f}, collision-free: {:.0f}".format(stats["ik_nodes"], stats["free_nodes"]))

# 4. The path is smooth: the largest joint change between two consecutive points.
# The path is smooth: the largest joint change between two points.
points = trajectory.points
largest = max(max(abs(a - b) for a, b in zip(p.joint_values, q.joint_values)) for p, q in zip(points, points[1:]))
print("largest joint step: {:.3f} rad".format(largest))

# 5. Starting from where the robot is. By default the planner may start on any IK branch;
# `start_from_state` forces the path to begin at the state's configuration.
# By default the path may start on any IK branch. `start_from_state` starts it where the
# robot is.
state.robot_configuration = trajectory.points[0].copy()
trajectory = planner.plan_cartesian_motion(waypoints, state, options={"start_from_state": True})
print("from the current configuration: {} points".format(len(trajectory.points)))
38 changes: 14 additions & 24 deletions docs/files/04_tool_and_free_axis.py
Original file line number Diff line number Diff line change
@@ -1,13 +1,11 @@
"""04 - A tool, and letting it turn about its own axis.
"""04 - A tool that may turn about its own axis.

For many processes (milling, gluing, drilling, 3D printing) the rotation of the tool about
its own axis does not matter. Fixing it anyway throws away solutions: the robot may be
unable to follow a path with the tool turned one way, but fine with it turned another.
`free_axis_samples` lets the planner choose that rotation at every waypoint.
For milling, gluing, drilling or printing, how the tool is turned about its axis does not
matter. Fixing it anyway can make a path impossible. `free_axis_samples` lets the planner
choose that rotation at every waypoint.
"""

import math
import time

from compas.geometry import Frame
from compas_fab.backends import UR5Kinematics
Expand All @@ -18,42 +16,35 @@

from compas_fab_native import NativePlanner

# 1. A UR5 with a cone-shaped tool. The tool is part of the collision model.
# A UR5 with a cone-shaped tool. The tool collides like a link.
robot_cell, state = RobotCellLibrary.ur5_cone_tool()
planner = NativePlanner(UR5Kinematics())
planner.set_robot_cell(robot_cell, state)

# 2. A spiral traced by the tool tip, 2.5 turns going down, like a print or a milling pass.
# Each frame's x-axis points along the path (TargetMode.TOOL: frames are for the tool's TCF),
# so taken literally the tool must spin 2.5 times about its axis. The last wrist joint
# cannot turn that far.
# A spiral of 2.5 turns, going down. Each frame's x-axis follows the path and z points
# down, so the tool would spin 2.5 times: more than the last joint can turn.
frames = []
for i in range(181):
t = 2.5 * 2 * math.pi * i / 180
point = [0.4 + 0.12 * math.cos(t), 0.12 * math.sin(t), 0.3 - 0.1 * i / 180]
tangent = [-math.sin(t), math.cos(t), 0]
# x along the path, z (the tool axis) pointing down.
frames.append(Frame(point, tangent, [math.cos(t), math.sin(t), 0]))
waypoints = FrameWaypoints(frames, TargetMode.TOOL)
waypoints = FrameWaypoints(frames, TargetMode.TOOL) # frames for the tool tip

# 3. With the tool rotation fixed as given in the frames.
# Rotation fixed as in the frames: no path.
try:
planner.plan_cartesian_motion(waypoints, state)
print("fixed rotation: found a path")
except CartesianMotionError:
print("fixed rotation: no path")

# 4. With 12 rotations about the tool axis to choose from at every waypoint, the planner
# can keep the tool from winding up.
t0 = time.perf_counter()
# 12 rotations to choose from at every waypoint: the tool no longer winds up.
trajectory = planner.plan_cartesian_motion(waypoints, state, options={"free_axis_samples": 12})
print("free rotation: {} points in {:.1f} ms".format(len(trajectory.points), (time.perf_counter() - t0) * 1e3))
print("free rotation: {} points".format(len(trajectory.points)))


# 5. The tip follows the path exactly; only the rotation about the tool axis changed.
# (The planner adds frames between waypoints more than 1 cm apart, the `max_step`
# option, so there are more points than waypoints: each lies on the straight segment
# between two waypoints.)
# The tip still follows the path: compare it with the nearest waypoint segment.
# (There are more points than waypoints: the planner adds one every 1 cm, `max_step`.)
def distance_to_path(p):
best = float("inf")
for a, b in zip(frames, frames[1:]):
Expand All @@ -68,6 +59,5 @@ def distance_to_path(p):
worst = 0.0
for point in trajectory.points:
check.robot_configuration = robot_cell.zero_configuration().merged(point)
tcf = planner.forward_kinematics(check, TargetMode.TOOL)
worst = max(worst, distance_to_path(tcf.point))
worst = max(worst, distance_to_path(planner.forward_kinematics(check, TargetMode.TOOL).point))
print("largest tip distance from the path: {:.1e} m".format(worst))
Loading
Loading