diff --git a/.gitignore b/.gitignore index 92f7fd6..c3974dc 100644 --- a/.gitignore +++ b/.gitignore @@ -9,3 +9,6 @@ __pycache__/ .pytest_cache/ .ruff_cache/ .DS_Store + +# mkdocs build output +site/ diff --git a/CHANGELOG.md b/CHANGELOG.md index 40c49cb..6755ddf 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -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 diff --git a/LICENSE b/LICENSE index 1639c38..e179e7d 100644 --- a/LICENSE +++ b/LICENSE @@ -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 diff --git a/NOTICE b/NOTICE index 6e3804e..99b2854 100644 --- a/NOTICE +++ b/NOTICE @@ -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 diff --git a/README.md b/README.md index a133c79..38dbecb 100644 --- a/README.md +++ b/README.md @@ -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 diff --git a/docs/files/01_kinematics.py b/docs/files/01_kinematics.py index 9189960..03d6a07 100644 --- a/docs/files/01_kinematics.py +++ b/docs/files/01_kinematics.py @@ -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 @@ -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)) diff --git a/docs/files/02_collision_checking.py b/docs/files/02_collision_checking.py index 49cce31..eee76cf 100644 --- a/docs/files/02_collision_checking.py +++ b/docs/files/02_collision_checking.py @@ -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 @@ -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() @@ -28,15 +26,14 @@ 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) @@ -44,31 +41,25 @@ 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. diff --git a/docs/files/03_cartesian_line.py b/docs/files/03_cartesian_line.py index 0429c03..b4d3cad 100644 --- a/docs/files/03_cartesian_line.py +++ b/docs/files/03_cartesian_line.py @@ -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 @@ -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))) diff --git a/docs/files/04_tool_and_free_axis.py b/docs/files/04_tool_and_free_axis.py index 21f4e7f..b22fa2c 100644 --- a/docs/files/04_tool_and_free_axis.py +++ b/docs/files/04_tool_and_free_axis.py @@ -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 @@ -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:]): @@ -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)) diff --git a/docs/files/05_mesh_obstacles.py b/docs/files/05_mesh_obstacles.py index 11f56c8..9f2823b 100644 --- a/docs/files/05_mesh_obstacles.py +++ b/docs/files/05_mesh_obstacles.py @@ -1,14 +1,10 @@ """05 - Obstacles with holes: distance fields instead of boxes. -By default every obstacle is reduced to its bounding box: fast, safe, and fine for tables, -walls and pallets. It is wrong for anything with a hole or a concave shape, where the box -fills the very space the robot needs. With ``obstacles="esdf"`` the core builds a distance -field from the obstacle meshes (a grid of distances to the surface, stored only near the -surface) and checks the robot spheres against it. +By default each obstacle becomes its bounding box. That is fine for tables and walls, but +wrong for a ring: the box fills the hole. With ``obstacles="esdf"`` the core uses a +distance field of the obstacle meshes instead. """ -import time - from compas.geometry import Frame from compas.geometry import Torus from compas_fab.backends import UR5Kinematics @@ -21,37 +17,32 @@ from compas_fab_native import NativePlanner -# 1. A ring (50 cm across, tube 5 cm thick) lying flat in front of the robot. (Much -# narrower and the UR5's wrist only just fits: whether it passes then depends on details -# of the sphere fit.) +# A ring, 50 cm across, lying flat in front of the robot. robot_cell, _ = RobotCellLibrary.ur5() robot_cell.rigid_body_models["ring"] = RigidBody.from_mesh(Torus(0.25, 0.025).to_mesh(u=48, v=12, triangulated=True)) state = robot_cell.default_cell_state() state.rigid_body_states["ring"] = RigidBodyState(Frame([0.45, 0.0, 0.45], [1, 0, 0], [0, 1, 0])) -# 2. The task: plunge the flange straight down through the middle of the ring. +# The task: move the flange straight down through the ring. frames = [Frame([0.45, 0.0, 0.65 - 0.23 * i / 29], [1, 0, 0], [0, -1, 0]) for i in range(30)] waypoints = FrameWaypoints(frames, TargetMode.ROBOT) options = {"free_axis_samples": 12} -# 3. With boxes: the ring's bounding box covers its hole, so this cannot work. +# With boxes: the box covers the hole, so there is no path. boxes = NativePlanner(UR5Kinematics(), obstacles="boxes") boxes.set_robot_cell(robot_cell, state) try: boxes.plan_cartesian_motion(waypoints, state, options=options) print("boxes: found a path") except CartesianMotionError: - print("boxes: no path (the bounding box fills the hole)") + print("boxes: no path") -# 4. With a distance field: 2 cm grid, distances stored up to 30 cm from the surface. +# With a distance field (2 cm grid): the hole is free. esdf = NativePlanner(UR5Kinematics(), obstacles="esdf", esdf_options={"voxel": 0.02, "truncation": 0.3}) -t0 = time.perf_counter() esdf.set_robot_cell(robot_cell, state) -print("distance field built in {:.0f} ms (again only when obstacles change)".format((time.perf_counter() - t0) * 1e3)) -t0 = time.perf_counter() trajectory = esdf.plan_cartesian_motion(waypoints, state, options=options) -print("distance field: {} points in {:.1f} ms".format(len(trajectory.points), (time.perf_counter() - t0) * 1e3)) +print("distance field: {} points".format(len(trajectory.points))) -# 5. And the final word, on the exact meshes: is the path really free? +# Double-check the path on the exact meshes. contacts = esdf.client.core.exact_check([p.joint_values for p in trajectory.points]) print("collisions on the exact meshes:", sum(c is not None for c in contacts)) diff --git a/docs/files/06_free_motion.py b/docs/files/06_free_motion.py index ed6f26d..6ea1434 100644 --- a/docs/files/06_free_motion.py +++ b/docs/files/06_free_motion.py @@ -1,13 +1,10 @@ -"""06 - Free motion: from here to there, around obstacles. +"""06 - Free motion around obstacles. -Cartesian paths (examples 03 and 04) prescribe where the tool goes. Free motion only -prescribes where the robot ends up: the planner chooses the way, avoiding obstacles. It -first tries the straight joint-space motion, then grows trees from the start and from the -goal until they meet (RRT-Connect), then shortens and smooths the path. +A Cartesian path says where the tool goes. Free motion only says where the robot ends up, +and the planner finds a way around obstacles. It tries the straight joint motion first, +then searches (RRT-Connect), then shortens and smooths the path. """ -import time - from compas.geometry import Box from compas.geometry import Frame from compas.geometry import Point @@ -23,7 +20,7 @@ from compas_fab_native import NativePlanner -# 1. A UR5 with a wall in front of it. +# A UR5 with a wall in front of it. robot_cell, _ = RobotCellLibrary.ur5() robot_cell.rigid_body_models["wall"] = RigidBody.from_mesh(Box(0.05, 0.8, 0.8).to_mesh(triangulated=True)) state = robot_cell.default_cell_state() @@ -31,50 +28,37 @@ planner = NativePlanner(UR5Kinematics()) planner.set_robot_cell(robot_cell, state) -# 2. From the left of the wall to the right. The straight joint-space motion would sweep the -# arm through the wall, so the planner has to find a way around. +# From left of the wall to right of it. Moving straight would hit the wall. state.robot_configuration.joint_values = [-1.2, -1.2, 1.4, -1.8, -1.57, 0.0] goal = robot_cell.zero_configuration() goal.joint_values = [1.2, -1.2, 1.4, -1.8, -1.57, 0.0] -t0 = time.perf_counter() trajectory = planner.plan_motion(ConfigurationTarget(goal), state) stats = trajectory.attributes["native_stats"] -print("around the wall in {:.1f} ms".format((time.perf_counter() - t0) * 1e3)) -print( - " planning {:.0f} us ({:.0f} iterations), smoothing {:.0f} us, exact-mesh check {:.0f} us".format( - stats["planning_us"], stats["iterations"], stats["simplification_us"], stats["exact_us"] - ) -) -print(" joint-space length: {:.2f} before smoothing, {:.2f} after".format(stats["initial_cost"], stats["cost"])) +print("around the wall: {:.0f} iterations".format(stats["iterations"])) +print("joint-space length: {:.2f} before smoothing, {:.2f} after".format(stats["initial_cost"], stats["cost"])) -# 3. To a frame instead of a configuration. A 6-axis arm reaches a frame in up to 8 ways; -# the planner grows its goal tree from all of them at once and takes whichever it -# reaches first. +# To a frame. The planner aims at all IK solutions of the frame at once. target = FrameTarget(Frame([0.3, 0.35, 0.3], [1, 0, 0], [0, -1, 0]), TargetMode.ROBOT) trajectory = planner.plan_motion(target, state) print("to a frame: {:.2f} s".format(trajectory.attributes["native_stats"]["duration"])) -# 4. To a point and an axis, e.g. a drill that may spin about its own axis: every rotation -# about the axis is acceptable, so even more goals. +# To a point and an axis, like a drill: any rotation about the axis is fine. target = PointAxisTarget(Point(0.3, -0.35, 0.3), Vector(0, 0, -1), TargetMode.ROBOT) trajectory = planner.plan_motion(target, state, options={"axis_samples": 16}) print("to a point and axis: {:.2f} s".format(trajectory.attributes["native_stats"]["duration"])) -# 5. Timing. The trajectory is timed under the joints' velocity limits (from the URDF) and -# acceleration limits, starting and ending at rest. Corners are blended (rounded off by -# at most 0.05 rad, if the rounded corner is collision-free) so the robot does not stop -# at every waypoint. Each point carries its time, velocities and accelerations. -print("timed: {} points over {:.2f} s".format(len(trajectory.points), stats["duration"])) +# Trajectories are timed: within the joint speed and acceleration limits, starting and +# ending at rest. Each point has its time, velocities and accelerations. +print("timed: {} points over {:.2f} s".format(len(trajectory.points), trajectory.attributes["native_stats"]["duration"])) slow = planner.plan_motion(ConfigurationTarget(goal), state, options={"velocity_scaling": 0.25, "max_acceleration": 1.0}) print("at a quarter of the speed: {:.2f} s".format(slow.attributes["native_stats"]["duration"])) middle = trajectory.points[len(trajectory.points) // 2] print("halfway: t = {:.2f} s, joint speeds {}".format(middle.time_from_start.seconds, ["{:+.2f}".format(v) for v in middle.velocities])) untimed = planner.plan_motion(ConfigurationTarget(goal), state, options={"time_parameterization": False}) -print("untimed, the path's corners only: {} points".format(len(untimed.points))) +print("untimed, corners only: {} points".format(len(untimed.points))) -# 6. Free motion does not need closed-form IK, so any serial robot works, like this 7-axis -# Panda (no kinematics solver given). -panda_cell, panda_state = RobotCellLibrary.panda() +# Free motion works for any robot, like this 7-axis Panda (no solver given). +panda_cell, _ = RobotCellLibrary.panda() panda_cell.rigid_body_models["post"] = RigidBody.from_mesh(Box(0.1, 0.1, 1.2).to_mesh(triangulated=True)) panda_state = panda_cell.default_cell_state() panda_state.rigid_body_states["post"] = RigidBodyState(Frame([0.45, 0.0, 0.6], [1, 0, 0], [0, 1, 0])) @@ -85,6 +69,5 @@ panda_state.robot_configuration = panda_state.robot_configuration.merged(start) goal = panda_cell.zero_configuration() goal.joint_values = [1.2, 0.3, 0.0, -2.0, 0.0, 2.3, 0.8] -t0 = time.perf_counter() trajectory = panda.plan_motion(ConfigurationTarget(goal), panda_state) -print("panda around a post in {:.1f} ms, {:.2f} s long".format((time.perf_counter() - t0) * 1e3, trajectory.attributes["native_stats"]["duration"])) +print("panda around a post: {:.2f} s".format(trajectory.attributes["native_stats"]["duration"])) diff --git a/docs/files/07_any_6_axis_arm.py b/docs/files/07_any_6_axis_arm.py new file mode 100644 index 0000000..0437d48 --- /dev/null +++ b/docs/files/07_any_6_axis_arm.py @@ -0,0 +1,49 @@ +"""07 - Any 6-axis arm, no solver: 3D printing with an ABB IRB 4600. + +Most industrial arms (ABB, KUKA, Staubli, Fanuc, Yaskawa) have a spherical wrist: the last +three axes meet in one point. The planner recognises this from the robot model and solves +IK in closed form. No solver needed. Other robots get numerical IK (example 08). + +Your own robot loads like the library ones: + + robot_cell = RobotCell.from_urdf_and_srdf("robot.urdf", "robot.srdf", "path/to/meshes") +""" + +import math + +from compas.geometry import Frame +from compas_fab.robots import FrameTarget +from compas_fab.robots import PointAxisWaypoints +from compas_fab.robots import RobotCellLibrary +from compas_fab.robots import TargetMode + +from compas_fab_native import NativePlanner + +# An ABB IRB 4600 with a printing nozzle, on a floor. No solver given. +robot_cell, state = RobotCellLibrary.abb_irb4600_40_255_printing_tool() +planner = NativePlanner() +planner.set_robot_cell(robot_cell, state) +print("closed-form IK:", planner.has_closed_form_ik) + +# IK for the nozzle tip (TargetMode.TOOL), pointing down. Up to 8 solutions in theory; +# this arm's joint limits leave 2 (wrist flipped or not). +target = FrameTarget(Frame([1.6, 0.0, 0.4], [1, 0, 0], [0, -1, 0]), TargetMode.TOOL) +solutions = list(planner.iter_inverse_kinematics(target, state)) +print("{} collision-free IK solutions".format(len(solutions))) + +# A vase: 10 layers of 2 cm, printed as one spiral. A nozzle only needs a point and a +# direction, so the waypoints are points with an axis, not frames. The planner chooses +# how the nozzle turns about its axis. +center, radius, layer_height = [1.6, 0.0, 0.1], 0.15, 0.02 +points = [] +for i in range(72 * 10 + 1): + t = 2 * math.pi * i / 72 + points.append([center[0] + radius * math.cos(t), center[1] + radius * math.sin(t), center[2] + layer_height * i / 72]) +waypoints = PointAxisWaypoints([(point, [0, 0, -1]) for point in points], TargetMode.TOOL) + +trajectory = planner.plan_cartesian_motion(waypoints, state) +print("vase: {} waypoints, {} trajectory points".format(len(points), len(trajectory.points))) + +# The same path with numerical IK, as a robot without a closed form would plan it. +trajectory = planner.plan_cartesian_motion(waypoints, state, options={"ik": "numeric"}) +print("vase, numerical IK: {} trajectory points".format(len(trajectory.points))) diff --git a/docs/files/08_which_ik.py b/docs/files/08_which_ik.py new file mode 100644 index 0000000..2d6a97a --- /dev/null +++ b/docs/files/08_which_ik.py @@ -0,0 +1,41 @@ +"""08 - Which IK each robot gets. + +The planner picks the IK when the robot cell is set: + +- a compas_fab solver given, like ``UR5Kinematics()``: its closed form. +- a 6-axis arm with a spherical wrist: a closed form built from the robot model. The + planner checks that joints 2 and 3 are parallel and perpendicular to joint 1, and that + axes 4 to 6 meet in one point. URs fail this check, so they need their solver. +- anything else: numerical IK. + +Closed form returns every solution within the joint limits. Numerical IK returns the +solutions it found from random starting points (up to `max_results`, default 8). +""" + +from compas_fab.backends import UR5Kinematics +from compas_fab.robots import FrameTarget +from compas_fab.robots import RobotCellLibrary +from compas_fab.robots import TargetMode + +from compas_fab_native import NativePlanner + +robots = [ + ("ABB IRB 4600", RobotCellLibrary.abb_irb4600_40_255, None), + ("Staubli TX2-60L", RobotCellLibrary.staubli_tx2_60l, None), + ("UR5 with UR5Kinematics()", RobotCellLibrary.ur5, UR5Kinematics()), + ("UR5 without a solver", RobotCellLibrary.ur5, None), + ("Panda (7 axes)", RobotCellLibrary.panda, None), +] + +for name, load, solver in robots: + robot_cell, state = load() + planner = NativePlanner(solver) + planner.set_robot_cell(robot_cell, state) + kind = "closed form" if planner.has_closed_form_ik else "numerical" + + # A pose the robot can reach: the forward kinematics of a random configuration. + check = state.copy() + check.robot_configuration = robot_cell.random_configuration(robot_cell.main_group_name) + target = FrameTarget(planner.forward_kinematics(check, TargetMode.ROBOT), TargetMode.ROBOT) + solutions = list(planner.iter_inverse_kinematics(target, state, options={"check_collision": False})) + print("{:26} {:12} {} solutions".format(name, kind, len(solutions))) diff --git a/docs/index.md b/docs/index.md index 5c199f2..af27035 100644 --- a/docs/index.md +++ b/docs/index.md @@ -53,6 +53,8 @@ Each example builds on the previous one and explains what the planner does as it - [`04_tool_and_free_axis.py`](files/04_tool_and_free_axis.py){: download="04_tool_and_free_axis.py" }: a spiral toolpath that only works when the tool may turn about its own axis - [`05_mesh_obstacles.py`](files/05_mesh_obstacles.py){: download="05_mesh_obstacles.py" }: plunging through a ring, impossible with bounding boxes, possible with a distance field - [`06_free_motion.py`](files/06_free_motion.py){: download="06_free_motion.py" }: free motion around obstacles, to configurations, frames and point-axis targets, timed under the joint limits, on a UR5 and a 7-axis Panda +- [`07_any_6_axis_arm.py`](files/07_any_6_axis_arm.py){: download="07_any_6_axis_arm.py" }: an ABB IRB 4600 printing a spiral vase with no kinematics solver given, in closed form and with numerical IK +- [`08_which_ik.py`](files/08_which_ik.py){: download="08_which_ik.py" }: which IK five robots get (ABB, Stäubli, UR5 with and without a solver, Panda), and how many solutions each finds ## How it works, in short diff --git a/mkdocs.yml b/mkdocs.yml index e549d69..07f87cd 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -4,7 +4,7 @@ repo_url: https://github.com/compas-dev/compas_fab_native repo_name: compas-dev/compas_fab_native edit_uri: blob/main/docs/ -copyright: Copyright © 2018 - 2026, Gramazio Kohler Research +copyright: Copyright © 2026, Gramazio Kohler Research exclude_docs: | diagrams/build.py diff --git a/src/compas_fab_native/planner.py b/src/compas_fab_native/planner.py index 33d9401..3a61ae8 100644 --- a/src/compas_fab_native/planner.py +++ b/src/compas_fab_native/planner.py @@ -4,13 +4,14 @@ free-space planning in the Rust core (``compas_fab_native``). User code switches to it by changing the planner: - planner = NativePlanner(UR5Kinematics()) + planner = NativePlanner() planner.set_robot_cell(robot_cell, robot_cell_state) trajectory = planner.plan_cartesian_motion(waypoints, start_state) trajectory = planner.plan_motion(target, start_state) -IK is closed-form where the robot has one (UR arms, spherical-wrist arms) and numerical -otherwise. Obstacles are boxes or distance fields. What the robot carries (attached tools, +IK is closed-form where the robot has one and numerical otherwise. Spherical-wrist arms +get a closed form built from the robot model; UR arms get one when their compas_fab solver +is passed (``NativePlanner(UR5Kinematics())``). Obstacles are boxes or distance fields. What the robot carries (attached tools, the workpieces they hold, bodies attached to its links) collides like its links do. """ @@ -93,7 +94,8 @@ class NativePlanner( ---------- kinematics_solver compas_fab analytical solver of the robot (e.g. ``UR5Kinematics()``); its parameters - are handed to the core's closed-form IK. + are handed to the core's closed-form IK. Optional: spherical-wrist arms get a closed + form without it, other robots get numerical IK (see ``has_closed_form_ik``). sphere_options Sphere fitting options (see ``model.fit_sphere_sets``). exact @@ -142,6 +144,17 @@ def __init__( self._static_cache = None self._moving = None + @property + def has_closed_form_ik(self) -> bool: + """True if IK for the current robot cell is solved in closed form, False if numerically. + + Known once the robot cell is set. Closed form: the ``kinematics_solver`` given, or a + spherical-wrist arm recognised from the robot model. + """ + if self.client.core is None: + raise ValueError("Set a robot cell first.") + return self.client.core.has_analytic_ik + # -- SetRobotCell / SetRobotCellState --------------------------------------------- def set_robot_cell(self, robot_cell, robot_cell_state=None, options: Optional[dict] = None): diff --git a/tests/test_native_planner.py b/tests/test_native_planner.py index 0578cc7..bafa4f8 100644 --- a/tests/test_native_planner.py +++ b/tests/test_native_planner.py @@ -126,6 +126,19 @@ def test_exact_final_pass_and_exact_check(ur5): planner.check_collision(folded, {"exact": True}) +@pytest.mark.parametrize( + "factory, solver, closed_form", + [("abb_irb4600_40_255", None, True), ("ur5", UR5Kinematics(), True), ("ur5", None, False), ("panda", None, False)], +) +def test_has_closed_form_ik(factory, solver, closed_form): + cell, state = getattr(RobotCellLibrary, factory)(load_geometry=False) + planner = NativePlanner(solver) + with pytest.raises(ValueError): + planner.has_closed_form_ik + planner.set_robot_cell(cell, state) + assert planner.has_closed_form_ik is closed_form + + @pytest.mark.parametrize("factory", ["abb_irb4600_40_255", "staubli_tx2_60l"]) def test_spherical_wrist_robots_without_a_solver(factory): """The closed form for spherical-wrist arms is derived from the robot model."""