Closing the Motion Execution Gap: From Semantic Motion Task Constraints to Kinematic Control

1 Introduction

When describing robot motions, semantic constraints provide a natural and expressive approach. Consider "make a salad", as shown in Fig. 1 where a PR2 robot prepares ingredients by cutting a zucchini. Such instructions are represented in cognitive robotics as symbolic tasks specifying what to achieve, not how to move. High-level actions are decomposed into sub-tasks (cutting, grasping, placing) using Generative AI [Gupta et al., 2024] or plan executives [Beetz et al., 2010; Fox and Long, 2003], often leveraging knowledge graphs [K¨umpel et al., 2025]. For example, "cut the zucchini into small slices" yields explicit constraints (slice thickness) and implicit ones (safe knife handling near humans), separating valid from invalid trajectories. A system remains essential to convert these symbolic, constraint-rich descriptions into continuous motion satisfying all constraints.

Complete cognitive robot architectures such as SkiROS2 [Mayr et al., 2023], ArmarX [W¨achter et al., 2016], and CRAM [Beetz et al., 2023] are designed to achieve transferability across robots by hierarchically resolving symbolic motion tasks down to primitive Cartesian pose or joint-space goals. Those primitive poses are typically further divided into mobile base and manipulator tasks [Sandakalum and Ang Jr, 2022] and they assume that tasks can be grounded at planning time [Pan et al., 2024]. This design inherently limits the class of symbolic motion tasks that can be defined. A task such as "open the fridge" cannot easily be reduced to these primitives, as it requires a discrete arch trajectory for the hand and the robot's mobile base to be positioned appropriately [Bozcuo˘glu et al., 2018]. Motions requiring degrees of freedom, like pouring from a jug at an arbitrary angle, are more naturally expressed with geometric constraints [Borghesan et al., 2015]. This limitation constitutes the "Motion Execution Gap".

Addressing the motion execution gap through generative AI remains infeasible. Due to their probabilistic nature, such methods cannot provide formal guarantees for constraint adherence [Huang et al., 2025]. Even stateof-the-art Vision-Language-Action models (VLAs) require embodiment-specific post-training for transferability across robotic platforms [Kim et al., 2025].

A popular alternative is to formulate motions as constraint optimization problems. The task function approach expressed as a Quadratic Program (QP), often termed QP-control [Aertbeli¨en and De Schutter, 2014; Bouyarmane et al., 2018;

Stelter et al., 2022; Escande et al., 2014], is a promising vari-

ation. Several works integrate QP-based task-function control with symbolic motion representations: In [Pane et al., 2021], a QP-controller pairs with a finite state machine (FSM) representation, executing motion tasks in parallel while FSM transitions serve as monitors. [Bolotnikova et al., 2021] similarly combine FSMs with QP-control for sequential/parallel multi-limb tasks. [Dom´ınguez et al., 2022] attaches a low-frequency Behavior Tree (BT) to a high-frequency QPcontroller, dynamically adding/removing tasks via implicit end conditions for smoothness. [Haviland et al., 2022] employs BTs for pick-and-place sequencing, focusing on discrete motions rather than continuous task composition. [Rovida et al., 2018] extends BTs with parallel composites for concurrent impedance-controlled "skills", merging commands from parallel nodes. FSMs and BTs are common choices, though FSMs suffer state explosion and BTs face computational scaling issues.

Motion Statecharts (MSCs) are introduced as statecharts specialized for motion composition. Statecharts, first introduced by [Harel, 1987], have become a general term for extensions of FSMs aimed at alleviating the state explosion problem for specific use cases. MSCs enable symbolic reasoning over motion tasks and support online monitoring and adaptation akin to hybrid symbolic/neural planning.

However, task transitions commonly introduce discontinuities, typically mitigated by slowing down during transition [Dom´ınguez et al., 2022] or gradual task weight scaling [Pane et al., 2021], both limiting the motion representation. For instance, single-task switches necessitate systemwide deceleration, impacting parallel tasks. The proposed system instead formulates task functions as short-horizon lMPC with explicit jerk limits, ensuring a smooth velocity motion profile. MSCs are thus allowed to swap motion tasks at any time without concern for the underlying controller.

In most of the aforementioned task function-based systems, only the robot kinematics are modeled. In this paper, it is instead proposed to model the kinematics of the environment as well to enable motions to be defined in a worldcentric manner. This allows the definition of the previous "open the fridge" example with two constraints, hold the handle and move the door according to its articulation model. In [Bouyarmane et al., 2018], a similar idea is explored, although the approach is based entirely on URDF [ROS.org, 2022], which is limited in its descriptive capabilities. The approach proposed in this paper is instead similar to [R¨ofer et al., 2022], by which even complex dependent kinematics can be described. Both are built on top of CasADi [Andersson et al., 2019], a library for computer algebra and automatic differentiation designed specifically for optimal control.

The proposed framework is called Giskard and uniquely combines the following components to close the motion execution gap.

• Motion Statecharts are provided as executable symbolic motion representations, by which online monitoring and automatic semantic annotation of trajectories are enabled.
• A differentiable kinematic world model is provided as a structured semantic digital twin, by which world-centric

task functions are supported.

• lMPC-based task-function control combines active constraints dictated by a MSC with explicit jerk bounds to prevent discontinuities during task transition.

Together these components not only allow motion descriptions via semantic constraints, but also offer real-time semantic feedback during the motion. This lets the high-level planner implement a closed semantic feedback loop with the motion executive. It can understand why motions succeed or fail, and adjust plans, instead of treating execution as a black box.

Transferability was validated on eight diverse platforms, as discussed in Chapter 5.2, ranging from mobile manipulators such as the PR2 to static dual-arm setups. Minimal robotspecific tuning was required, by which out-of-distribution generalization was demonstrated.

2 Differentiable KinematicWorldModel

Giskard requires a kinematic model to describe motions with task functions. An example world model is shown in Fig. 2. The model is defined as follows.

• L: A set of links.
• J : A set of joints.
• S = F ∪V:
• F: Symbols referring to Degrees of Freedom (DoFs). This set includes robot's DoFs, e.g., "wrist roll joint", as well as environmental DoFs, e.g., "hinge". For each such symbol q, its time derivatives up to the third order, namely .q, .. q, and ... q are also included in F.
• V: Symbols representing non-actuated variables, treated as constant by the controller, e.g., robot localization.
• C: A set of constraints limiting the range of symbols in F. Links and joints form a tree structure that includes both the robot and the articulated environment. Joint transformations are defined as functions over S.

They are implemented as symbolic expression using CasADi [Andersson et al., 2019] for automatic differentiation. A distinction is made between joints and DoFs, since a default condition many-to-many relationship exists between them. For example, the "Odometry" joint contains non-actuated variables to encode positional measurements relative to "map" and three DoFs to encode the robot's ability to translate and rotate relative to "Base Link". DoFs of the articulated environment are treated as controllable, for example in the case of a "Hinge", since the robot can exert indirect control over them while in contact. This approach simplifies the definition of environment manipulation tasks, as discussed in the next subsection.

2.1 Differentiable Task Functions

The main purpose of the world model is to provide a means of computing forward kinematics (FK) for chains between any link pairs. FKs are computed by multiplying the transformation matrices of all joints along the chain. Fig. 2 shows a handle-to-gripper chain spanning both the robot and the environment. All FKs are automatically differentiable thanks to CasADi and can be used in the definition of task functions.

The high-level idea of the task function approach is to describe a motion as a set of task functions. Task functions are constraints defined on differentiable task spaces. A task space (f) maps the world state onto an arbitrary space:

where m and n are arbitrary integers. The robot must be capable of influencing the task space. Technically, this means that at least one partial derivative with respect to the robot's DoFs is non-zero. FKs that include parts of the robot satisfy this definition. In Fig. 2, this enables the description of a dooropening task by combining a joint-space task for the "Hinge" with a Cartesian-space task for the gripper to hold the handle. Furthermore, these expressions can be extended with arithmetic and trigonometric operations provided by CasADi to produce arbitrary geometric constructs.

Motions are described by constraining task spaces using task functions. A Task Function is an (in-)equality constraint on a scalar-valued task space: fϵ = bϵ, or lξ ≤ fξ ≤ uξ.

For example, a 6D Cartesian-space motion goal is defined using six equality constraints, three for position and three for rotation, with fϵ derived from the FK and bϵ being the position/rotation error. Grasping a handle can be implemented with a task space describing the distance between the robot's gripper and a line representing the handle. A task function can constrain that distance to be equal to zero. The resulting motion moves the gripper to its closest point on the handle.

3 Motion Statecharts

Consider a high-level planner that provides a symbolic action description such as "open the fridge". It may produce a sequence such as "grasp the handle" → "move the door open" → "release the handle", or more granular steps. These actions must be grounded into continuous control commands. Furthermore, to execute this sequence as a single motion, a single set of task functions is insufficient because the constraints for opening the door must not be active during the handle-grasping phase.

Previous work proposes the use of hierarchical FSMs [Pane et al., 2021; Bolotnikova et al., 2021] or BTs [Dom´ınguez et al., 2022; Haviland et al., 2022; Rovida et al., 2018] to group task functions into different phases. The structure of BTs makes them difficult to integrate directly into the control loop, as all three cited works run them at lower frequency (2-10 Hz) in parallel with high-frequency controllers. FSMs do not share this limitation and support real-time execution, but suffer from the state explosion problem. As system complexity increases, the number of states grows combinatorially, though hierarchical definitions mitigate this to some extent.

The proposed MSCs build on statecharts to retain FSM real-time capability while alleviating state explosion by organizing motion components into a network of dual-FSMs. This yields a reusable representation that bridges the motion execution gap by composing motion tasks and monitors into reactive graphs portable across robots and environments.

3.1 Structure and Elements

MSCs are defined as a variation of statecharts with explicit motion-related constructs tailored to robot control. They are represented as graphs whose nodes can contain nested statecharts, by which reusability and structural decomposition are promoted. All nodes may, but do not have to, be associated with motion task functions (see Sec. 2.1), which remain active for the duration of the node's activation. Nodes that are associated with task functions are referred to as Motion Tasks, those without are called Monitors. An example node is depicted in Fig. 3. Each node contains two internal FSMs.

• An observation FSM (Fig. 3, top-center) is used to represent whether an observed condition is currently True, False, or "?". For Motion Tasks, this state represents the constraint satisfaction status. An explicit "?" state is required for situations in which the state is unknown, for example when the node has never been active.
• A life cycle FSM (Fig. 3, bottom) is used to determine whether the node is active.

Conditions are depicted in the lower half of a node (see Fig. 3, top right). They govern transitions between life cycle states, for example transitioning a node from to ▶ when the start condition is satisfied. These transitions are expressed as ternary logical expressions that combine the three-valued observation states of any node on the same hierarchy level. In addition, all nodes may implement callbacks that are triggered during every control cycle in which the node is ▶ , or when a transition between life cycle states occurs. A nested statechart is referred to as a MSC Template. MSC Templates are themselves nodes with the same internal FSMs structure and contain isolated statecharts. The life cycle states of nested MSC nodes are linked to the life cycle state of the parent. If the parent node is in ▶ state, the child nodes can change state freely. In all other cases the child nodes mirror the parent's state, if the diagram in Fig. 3 allows the transition. This means a parent cannot force a child transition from to , but it can make a child transition from ▶ to .

In addition to enabling reuse of frequently occurring MSC structures, templates are used to impose structure on motions. Common examples include the following. Sequential: Child nodes are activated upon the predecessor reaching the True state, and the template succeeds when the final child succeeds. Parallel: All child nodes are activated simultaneously, and the template succeeds when n children reach the True state.

The terminal states of MSCs are referred to as EndMotion and CancelMotion. Both states trigger termination of the control loop and stop motion execution. The distinction between the two states is purely semantic feedback.

3.2 Motion Statecharts in the Control Loop

At runtime, the state of the MSC is evaluated at every control cycle in the following order:

1. The on tick callbacks of all ▶ nodes are executed given the current system state, including joint states, transforms, and sensor inputs, in order to update their observation states.
2. All life cycle state machines are updated simultaneously based on the current observation states of all nodes.
3. If a final state exhibits a True observation state, execution terminates and zero commands are sent to the robot.
4. Otherwise, the task functions associated with all ▶ nodes are collected and applied as constraints to the lMPC.
5. The lMPC computes the next jerk commands for all DoFs of the system, which are integrated to velocity commands and sent to the robot.

3.3 Example: MSC of a CuttingMotion

Figure 4 illustrates an MSC executing a cutting motion on the HSR platform. The motion starts with the "Pre-Position" task, the only node whose start condition is True. The "Cut" MSC template then coordinates downward, upward, and leftward knife motions through three self-terminating tasks. The embedded monitor "Contact?" aborts the sequence if it fails to activate following "Down" completion. Upon termination of "Move Left", the template advances to a True observation state. The "Done?" monitor subsequently determines whether to reset the "Cut" template or end execution.

In parallel, the "Human Close?" monitor remains active, pausing the "Cut" template on human proximity detection, as depicted in the figure snapshot. At this time, "Down" and "Contact?" have already reached . These nodes remain in because no transition to exists (cf. Fig. 3). "Up" transitions from ▶ to , because such a transition is possible. Unstarted child nodes remain in .

Node observation states serve dual purposes: triggering life cycle transitions and enabling real-time motion annotation. Figure 5 presents a Gantt chart of the cutting motion execution, with each node depicted by dual bars representing life cycle and observation states, respectively. Human proximity is evident between t ≈ 3.4 and t ≈ 4.1 via the monitor's True observation state.

4 lMPC-Based Implementation of the Task Function Approach

MSCs allow task functions to be exchanged at every control cycle. In standard QP-controllers, this can introduce discontinuities. Alternatives defer switching until motion phase end [Dom´ınguez et al., 2022] or use weight scaling [Pane et al., 2021]. The former limits motions to separable phases with pauses, the latter is unable to deactivate constraints immediately and adds tuning parameters for the weight-scaling strategies.

To address this, a lMPC-based task function implementation was developed that prevents discontinuities in the joint velocity commands using jerk limits. It predicts system evolution over a short horizon to enforce jerk limits, with task functions as constraints.

4.1 Formulation as a Quadratic Program

The lMPC is formulated as a QP:

The first term of the objective function minimizes the joint velocities .qk, weighted by w. q, over a prediction horizon N. They are represented as vectors containing one variable per DoF of the system. N is chosen to be sufficient to enforce jerk limits, with N∆t typically below ≤ 0.25s. The weights start at 0.001 and increase linearly to 0.01 at the end of the horizon, thereby encouraging the use of immediate velocities.

The remaining objective function minimizes vectors of slack variables ϵ and ξ, associated with equality and inequality constraints, respectively. They prevent infeasibility in the presence of conflicting constraints through relaxation. By default, their weight is set to 1, normalized by the square of the expected maximum velocity of the corresponding task. As a consequence, using joint velocity to fulfill constraints is approximately two orders of magnitude cheaper than violating them using slack variables.

The jerk decision variables ... q of the objective function influence it only indirectly. Instead, the system model links velocity and jerk through constraints derived from a semiimplicit Euler integration scheme,

Consequently, the weights applied to velocity variables implicitly influence the jerk variables. The system model in Eq. 4 is obtained by eliminating acceleration variables, enforcing a terminal velocity of zero for stability, and linking the current states to the initial system state. Explicit acceleration variables are omitted because their bounds had negligible impact in practice. This reduces the number of decision variables by roughly 1 3 . Introducing explicit jerk penalties to the objective function can destabilize the system by making continued motion cheaper than deceleration.

The bounds defined in Eq. 3 impose direct limits on velocities and jerks. Given the prediction horizon and desired velocity limits, a theoretical maximum jerk exists that allows the system to decelerate from the velocity limit within the horizon. This value is computed once by solving a modified version of Eq. 2 without jerk bounds. Empirically, no benefit was observed from further manual tuning of jerk limits. As a result, N and ∆t are the only parameters requiring robotspecific tuning. Joint position limits are encoded into the velocity bounds by progressively restricting allowed values as joints approach their limits.

Finally, task functions are incorporated into the final two constraint sets in Eq. 5 and 6 by constraining the Jacobian integral over the horizon and relaxing it with slack variables. In Eq. 5 bϵ corresponds to the task space error. In Eq. 6 lξ and uξ define lower and upper bounds on the task space error, respectively. To prevent excessively large error terms, all three bounds are clamped to values achievable within the horizon. The slack variables ϵ and ξ relax these constraints as described previously. They represent the residual task space velocity integral over the horizon and are scaled by ∆t such that their magnitudes are comparable to the integral term. This formulation requires only a single constraint and a single slack variable per task function in the QP. The majority of motions encountered in mobile manipulation can be expressed using such positional task functions, which supports scalability of the overall system. Velocity task constraints can also be formulated, but they require one constraint per discretization step of the prediction horizon. Further details on this formulation are provided in [Stelter, 2025].

For this application, the fastest available QP solver is qpSWIFT [Pandala et al., 2019], which enables execution of the complete control loop at 100 hz on a high-performance CPU.

level by evaluated logical conditions. For example "CutInProgress" is defined as both monitors having a True observation state: C(t) ∧ S(t) ⇒ CutInProgress. Under this model, cutting inside the high-level planner is no longer described using a specific method or perception routines. Instead the high-level decisions are made based on semantic feedback. This clear mapping of physical signals to semantic states lets us reason, for example, that "the knife did not touch the object" if no contact is detected.

5.2 Transferability

To showcase the transferability of the framework, Table 1 list all implemented MSC nodes and the real robots they have been deployed on. Since our approach uses a kinematic world model, all implementations are, in principle, robot-agnostic and can be reused on any platform that offers the required capabilities (e.g., a differential drive for the "Diff-drive navigation" node). To verify this, the world model presented in Section 2 was used to kinematically simulate sensible combinations.

"Feature Functions" are defined as algebraic expressions that combine geometric primitives, such as points, lines, and

5 Evaluation

The presented motion executive aims to close the motion execution gap, by offering a method to describe motions semantically with MSC and translating it into a kinematic lMPC for a given robot. The framework was deployed on eight robot platforms and several different environments, shown in Fig. 6.

5.1 Closing the Motion Execution Gap withMSC

To test if the presented framework is able to close the motion execution gap, it was integrated as the motion executive into the CRAM cognitive architecture [Beetz et al., 2023]. This architecture uses a knowledge base to specialize its plans and parameterize motion designators, which serve as the symbolic counterpart to MSCs in our framework. To integrate our approach, the high-level planner was modified to resolve plans to MSCs instead.

Returning to the cutting example (Fig. 1), a task such as "cut the zucchini" does not yet specify how the cutting should be carried out. Queries to a knowledge base provide answers, for instance that the zucchini should be sliced instead of cubed, or what the preceding and following motions are. The answers allow the decomposition of the plan into sub-actions and finally MSCs, linking them to their semantic meaning. This setup allows the high-level planner to reason about the current system state during execution based on the observation and life cycle states of nodes. After the execution, it can use the final MSC state to reason about failure causes.

The high-level planner decides the next step based on this semantic feedback. It may stop the plan, repeat a failed cut, switch tools, or change settings like the target position or direction of movement. The cutting process is described by a discrete separation state S(t) ∈ {False, True}, where S(t) = True means that material separation has taken place at time t. A MSC monitor tracks this semantic state using geometric and contact observations. Another monitor tracks whether the contact force at the tool frame is increasing: C(t) = . f(t) > 0. This feedback is interpreted on the highplanes, between tool and object frames to form differentiable scalar constraints [Borghesan et al., 2015]. They enable the specification of complex motions through composition within MSC. "Diff-drive navigation" addresses the limitation of platforms such as TIAGo++ and Stretch2, which are unable to move sideways. Corresponding templates sequence orientation, forward driving, and reorientation actions to replace Cartesian goals for driving.

The tuning effort required to achieve this transferability was minimal. MSCs themselves do not require any robotspecific tuning. The lMPC formulation depends on only two parameters, ∆t and N (see Sec. 4). The former, ∆t, is matched to the joint feedback rate and typically lies between 0.01s and 0.02s. Increasing N improves motion smoothness at the cost of increased computational effort. This parameter was selected as the minimum value required to accommodate robot-specific noise levels, typically between 15 and 30. For kinematic simulation, identical parameters were used across all setups, namely ∆t = 0.02s and N = 7, in order to minimize computation time. The resulting level of transferability exceeds what can be achieved by single generative models without platform-specific fine-tuning.

5.3 SmoothMotion Task Transition

The dual UR10 setup was deployed in a sterility testing use case. One of the required motions was the insertion of a cylindrical object into a slot, corresponding to a classical peg-in-hole task. Using feature functions, an MSC template was defined in which the motion is decomposed into multiple phases, as illustrated in Fig. 8.

• Always: "Stay on Line", by which the distance between the cylinder bottom and the hole axis is minimized.
• Approach: "Reach Top", minimizing the distance between the cylinder bottom and a point above the hole, combined with "Slightly Tilted", enforcing that the angle between the cylinder and the horizontal world axis remains above a specified threshold.
• Insert: The tilt constraint is replaced by "Insert", formulated as a point-to-point goal.
• Finish: "Tilt Straight", aligning the cylinder axis with the hole axis.

The resulting execution demonstrates that task functions can be exchanged seamlessly during runtime. The measured trajectory shown in Fig. 7 exhibits smooth velocity profiles despite frequent task switches and the presence of sensor noise. This behavior empirically validates the proposed lMPC-based formulation for smooth motion task transitions.

6 Conclusion

This paper closes the motion execution gap by introducing MSC for symbolic motion composition, differentiable kinematic world models as semantic digital twins, and jerkbounded lMPC for smooth task-function control. Validated on eight diverse robots like PR2 or Stretch2, it enables worldcentric specifications, online monitoring, and robust transferability with guarantees unattainable by generative AI, which fails to enforce constraints or long-horizon consistency.

7 Limitations &FutureWork

The framework currently lacks dedicated force control support. Basic force-aware behaviors can nonetheless be realized via monitors that react to sensor feedback and trigger MSC transitions, as demonstrated in the cutting example. The reason is that the kinematic world model does not yet represent robot dynamics, though QP-based controllers that handle such effects are demonstrated in related work [Bolotnikova et al., 2021].

Finally, the system focuses on motion execution rather than planning, leaving integration with sampling based planners or trajectory optimizers as future work.

Acknowledgments

This work was partially supported by: (1) the German Research Foundation DFG, as part of Collaborative Research Center (Sonderforschungsbereich) 1320 Project-ID 329551904 "EASE - Everyday Activity Science and Engineering", University of Bremen (http://www.ease-crc.org/); (2) the German Federal Ministry of Research, Technology and Space (BMFTR) under the Robotics Institute Germany (RIG); (3) the 'FAME' project, supported by the European Research Council (ERC) via the HORIZON ERC Advanced Grant no. 101098006. While partly funded by the European Union, views and opinions expressed are however those of the authors only and do not necessarily reflect those of the European Union. Neither the European Union nor the granting authority can be held responsible for them.