Geometric Relations Semantics

Software for semantically checked geometric relations between rigid bodies: relative position, orientation, pose, linear velocity, angular velocity, and twist (plus force, torque, and wrench).

It implements the semantics defined in:

Tinne De Laet, Steven Bellens, Ruben Smits, Erwin Aertbeliën, Herman Bruyninckx, and Joris De Schutter, Geometric Relations between Rigid Bodies: Semantics for Standardization, IEEE Robotics & Automation Magazine, 2012 — download the paper (KU Leuven Lirias)

Every geometric relation carries its full semantic description — which point/orientation frame, fixed to which body, with respect to which reference body, expressed in which coordinate frame. Operations (composition, inversion, change of point/frame, twist integration) check these semantics and derive the result's semantics automatically, catching the classic errors listed in Box 1 of the paper: wrong inverse semantics, illegal composition logic, composing twists at different points, mixing coordinate frames, wrong multiplication order, and integrating non-integrable twists.

The notation at a glance — a robot holding a tool, an object in the environment, and the semantically checked chain that yields the tool pose with respect to the object (Python/JAX API shown; the C++ API follows the same semantics):

Geometric primitives and relations

The geometric primitives (paper Sec. III-A) are the vocabulary every relation is expressed in. Each primitive is fixed to a body, written x|B:

Primitive	Notation	What it represents / is used for
Body	C, D, ...	A rigid body (robot link, tool, object, the world). All other primitives are fixed to a body; relations are always between two bodies.
Point	e, f, ...	A location on a body — no orientation, no extent. Used to express relative position and as the point a linear velocity / twist refers to.
Orientation frame	[a], [b], ...	Three orthonormal axes fixed to a body. Used to express relative orientation, and — as the coordinate frame [r] — to give numerical coordinates a meaning.
Frame	{g}, {h}, ...	A displacement frame: an origin point g plus an orientation frame [g] with the same name. The practical primitive for poses (≙ a KDL/tf frame, a MuJoCo body frame).

The geometric relations built from them (paper Table I; the coordinate form adds the coordinate frame [r], instantaneously fixed to the reference body, in which the numbers are expressed):

Relation	Minimal semantics	What it represents / is used for
Position	Position(e|C, f|D)	Position of point e on body C relative to point f on D. Coordinates: 3-vector.
Orientation	Orientation([a]|C, [b]|D)	Orientation of body C relative to D, via orientation frames on each. Coordinates: rotation matrix / quaternion.
Pose	Pose((e,[a])|C, (f,[b])|D), or Pose({g}|C, {h}|D) with frames	Combined position + orientation — the 6-DOF displacement between two bodies. Coordinates: homogeneous transform / (quaternion, position).
Linear velocity	LinearVelocity(e|C, D)	Linear velocity of point e of body C relative to body D. Independent of any point chosen on D (paper Sec. VII-A.1) — hence no reference point.
Angular velocity	AngularVelocity(C, D)	Angular velocity of body C relative to D. Independent of any point or orientation frame on either body.
Twist	Twist(e|C, D)	Angular velocity of C w.r.t. D and linear velocity of point e — the 6-D velocity. Only integrable when the point and coordinate frame belong to the same frame (body-fixed / screw twist).
Force, Torque, Wrench	Force(C, D), Torque(e|C, D), Wrench(e|C, D)	The dynamic duals of angular velocity, linear velocity, and twist (C++ library only).

---

Plain C++ users

The core library (geometric_semantics) depends only on Boost (uuid); the KDL coordinate bindings (geometric_semantics_kdl) additionally need orocos-kdl. The code compiles with modern compilers (tested g++ 11).

Semantic checking is controlled by preprocessor flags: -DCHECK enables the checks, -DOUTPUT_WRONG / -DOUTPUT_CORRECT print diagnostics for failing / passing checks. Without -DCHECK the operations run unchecked.

There is no maintained build system for the C++ tree (the original CMake files are rosbuild-era); compiling the sources directly works:

g++ -DCHECK -DOUTPUT_WRONG \
    -Igeometric_semantics/src -Igeometric_semantics_kdl/src \
    $(pkg-config --cflags orocos-kdl) \
    your_app.cpp \
    $(find geometric_semantics/src geometric_semantics_kdl/src \
        -name '*.cpp' ! -name '*Check.cpp') \
    $(pkg-config --libs orocos-kdl)

(The *Check.cpp files are textually included by their parent translation units and must not be compiled standalone.)

Typical usage — the one-robot example, semantics only:

using namespace geometric_semantics;
PoseCoordinatesSemantics poseO2_E2("o2","o2","O2","e2","e2","E2","e2");
PoseCoordinatesSemantics poseO2_O1("o2","o2","O2","o1","o1","O1","o1");
PoseCoordinatesSemantics poseE2_O2 = poseO2_E2.inverse2();
PoseCoordinatesSemantics poseE2_O1 = compose(poseO2_O1, poseE2_O2);
// -> Pose((e2,[e2])|E2,(o1,[o1])|O1,[o1]); illegal operations return false /
//    a copy and print the violated constraint when OUTPUT_WRONG is set

With coordinates, use the same classes templated on KDL types (Pose<KDL::Frame>, Twist<KDL::Twist>, ...) — see geometric_semantics_examples/src/ and the unit tests under geometric_semantics{,_kdl}/test/.

Note: two long-standing defects where the C++ deviated from the paper were fixed in this repository (uniform changePoint argument direction — the position of the new point with respect to the old point — and the changePointRefBody coordinate sign). See the git history and the geosem README for the full list.

ROS & Orocos users

The ROS and Orocos integration packages date from the rosbuild / ROS Fuerte era and are kept for reference:

• geometric_semantics_msgs — ROS messages for all semantics and coordinate-semantics types; geometric_semantics_msgs_conversions converts between messages and C++ classes.
• geometric_semantics_tf, geometric_semantics_tf_msgs, geometric_semantics_tf_msgs_conversions — tf integration: stamped, semantically annotated transforms.
• geometric_semantics_orocos_typekit, geometric_semantics_orocos_typekit_kdl — Orocos RTT typekits for the plain and KDL-templated types.
• rtt_geometric_semantics_msgs, rtt_geometric_semantics_tf_msgs — RTT typekits for the ROS messages.

They have not been ported to catkin/ament or modern Orocos toolchains; expect to update the build glue before use. The message definitions (geometric_semantics_msgs/msg/*.msg) remain a useful, build-system-neutral specification of the semantic data model.

JAX / MJX users

geometric_semantics_jax/ contains geosem, a minimal Python/JAX implementation designed for use inside jax.jit / mujoco.mjx loops:

• semantics are static pytree aux_data, coordinates are array leaves — semantic checks run at trace time and compile away; the compiled XLA is pure math with the composition order already derived from the semantics;
• MuJoCo conventions throughout: wxyz quaternions, (q, t) poses matching xquat/xpos, [angular, linear] 6-vector twists matching cvel;
• closed-form SE(3) exponential for twist integration (paper Box 3), batched, differentiable;
• an MJX bridge (geosem.mjx): body_pose, relative_pose, body_twist (with the subtree-COM reference point handled semantically), body_fixed_twist;
• a pure-NumPy fallback for every layer: the same functions run without JAX installed (and geosem.mjx also accepts a host mujoco.MjData), so a quantity computed in the training loop can be re-evaluated in ordinary Python dev tooling.

cd geometric_semantics_jax
pip install -e ".[jax,mjx,test]"
pytest

import jax
from geosem import compose
from geosem.mjx import relative_pose, body_fixed_twist, body_pose, WORLD

@jax.jit
def step(d):
    rel  = relative_pose(model, d, "gun", "object")   # Box 2 pattern
    tw   = body_fixed_twist(model, d, "gun")          # integrable twist
    disp = tw.integrate(0.002)                        # Box 3
    pose = body_pose(model, d, "gun")
    return rel, compose(disp, pose.redeclare(body=WORLD))

See the geosem README for the paper-notation ↔ API mapping and the design.

---

License

Dual-licensed under the GNU LGPL v2.1 or the Modified BSD License, at your discretion (see LICENSE and the per-file headers).

Repository layout

Directory	Contents
geometric_semantics/	Core C++ library: semantics + coordinate-semantics classes and templated coordinate classes, with unit tests
geometric_semantics_kdl/	Coordinate implementations bound to Orocos KDL (KDL::Vector/Rotation/Frame/Twist/Wrench)
geometric_semantics_examples/	Worked examples from the paper (one/two-robot chains, typical errors)
geometric_semantics_jax/	geosem — Python/JAX implementation, MJX-compatible (README)
geometric_semantics_msgs*, geometric_semantics_tf*	ROS messages, conversions, and tf integration
geometric_semantics_orocos_typekit*, rtt_geometric_semantics_*	Orocos RTT typekits
doc/	Figures and documentation assets

Acknowledgements

The geometric semantics project is supported by:

• FWO:
  • Tinne De Laet is a PostDoctoral Fellow of the Research Foundation - Flanders (FWO) in Belgium.
  • Flemish FWO project G040410N Autonomous manipulation tasks with a flying robot.
• European FP7 projects:
  • Rosetta: FP7-230902, Robot control for skilled execution of tasks in natural interaction with humans; based on autonomy, cumulative knowledge and learning
  • BRICS: FP7-231940, Best practices in robotics
  • ROBOHOW: FP7-ICT-288533
• KU Leuven
  • KU Leuven's Concerted Research Action GOA/2005/010 and GOA/2010/011
  • KU Leuven-BOF PFV/10/002 Center-of-Excellence Optimization in Engineering (OPTEC)