Tutorial 5: Kinematic Trees

In this tutorial you will learn how to lock and unlock coordinate systems to cache transformations in a kinematic tree.

The Setup

When mapCS() builds a coord_tf it traverses the kinematic tree from each coordinate system up to WCS. This traversal is repeated every time a transformation is computed. In deep trees, or when backends make expensive calls (e.g. querying a physics engine), this traversal can be a performance bottleneck.

coordsys provides lock() and unlock() to cache the result of tf_to_WCS(). While locked, the coordinate system acts as a fixed root: traversal stops at that node and returns the cached value instead of continuing up the tree.

The Code

#include <print>
import ninbot;

int main()
{
    using namespace nin;
    using namespace nin::R3;

    position_coordsys_child CS_A {WCS, { {2_m, 0_m, 0_m}, {} }};
    position_coordsys_child CS_B {CS_A, { {1_m, 0_m, 0_m}, {} }};
    position_coordsys_child CS_D {CS_B, { {0_m, 0_m, 1_m}, {} }};

    point P = {CS_D, {1_m, 0_m, 0_m}};

    std::println("P in WCS (unlocked):          {}", P.map_to(WCS));

    CS_B.lock();
    std::println("P in WCS (CS_B locked):       {}", P.map_to(WCS));

    position_coordsys_child CS_E {WCS, { {10_m, 0_m, 0_m}, {} }};
    CS_B->set_parent(CS_E);
    std::println("P in WCS (parent changed,\n"
                 "          CS_B still locked): {}", P.map_to(WCS));

    CS_B.unlock();
    std::println("P in WCS (CS_B unlocked):     {}", P.map_to(WCS));

    return 0;
}

Output

P in WCS (unlocked):          (4, 0, 1) (m)
P in WCS (CS_B locked):       (4, 0, 1) (m)
P in WCS (parent changed,
          CS_B still locked): (4, 0, 1) (m)
P in WCS (CS_B unlocked):     (12, 0, 1) (m)

Building the code

Save the example as kinematic-trees.c++ alongside this CMakeLists.txt:

cmake_minimum_required(VERSION 4.0)
project(kinematic-trees LANGUAGES CXX)

find_package(Ninbot REQUIRED)

add_executable(kinematic-trees kinematic-trees.c++)
target_link_libraries(kinematic-trees PRIVATE ninbot::ninbot)

Then configure and build:

cmake -B build -G Ninja -DCMAKE_PREFIX_PATH=/path/to/ninbot/build
cmake --build build
./build/kinematic-trees

Walkthrough

Building the tree

position_coordsys_child CS_A {WCS, { {2_m, 0_m, 0_m}, {} }};
position_coordsys_child CS_B {CS_A, { {1_m, 0_m, 0_m}, {} }};
position_coordsys_child CS_D {CS_B, { {0_m, 0_m, 1_m}, {} }};

point P = {CS_D, {1_m, 0_m, 0_m}};

CS_A is 2 m along x from WCS; CS_B is 1 m along x from CS_A (3 m from WCS); CS_D is 1 m along z from CS_B. P is 1 m along x inside CS_D, placing it at (4, 0, 1) in WCS.

Locking a coordinate system

CS_B.lock();
std::println("P in WCS (CS_B locked): {}", P.map_to(WCS));

lock() computes and caches the current tf_to_WCS() of CS_B. Subsequent calls to tf_to_WCS() on CS_B return the cached value without traversing the parent chain. The value of P in WCS is unchanged: locking does not move any coordinate system.

Effect of locking while the tree changes

position_coordsys_child CS_E {WCS, { {10_m, 0_m, 0_m}, {} }};
CS_B->set_parent(CS_E);
std::println("P in WCS (parent changed, CS_B still locked): {}", P.map_to(WCS));

set_parent() re-attaches CS_B to CS_E (10 m along x from WCS). Because CS_B is locked, its tf_to_WCS() still returns the cached value from before the re-attachment, and P maps to the same location as before.

Unlocking

CS_B.unlock();
std::println("P in WCS (CS_B unlocked): {}", P.map_to(WCS));

After unlock(), tf_to_WCS() traverses the parent chain again. CS_B is now under CS_E, so its WCS position is (11, 0, 0), placing P at (12, 0, 1).

Using `std::unique_lock` for exception safety

coordsys satisfies the Lockable named requirement, so a lock guard pattern can be used to ensure unlock() is called even if an exception is thrown:

{
    std::unique_lock lock {CS_B};
    coord_tf tf = mapCS(CS_D, WCS);  // traversal stops at CS_B
}  // CS_B unlocked here