Direct Collocation Oscillating Reconstructed State Trajectory

[louietouie]

I am working with the DirectCollocation class and an often getting trajectory solutions with large oscillations (sometimes in the position, almost always in the velocity). These oscillations seem to happen between the sample points, so I was thinking it may be due to the dynamics constraints applied at the intermediate collocation points, but I've had trouble figuring out where to go from here.

These oscillations seem to appear mainly when the joints have dampening applied via , but without dampening, other problems and spastic behaviors occur.

Here is the code: https://github.com/louietouie/oscillating_ping_pong/tree/main. I've cut down on the constraints I plan to use (I usually also constrain end-effector cartesian velocity, joint position and velocity limits, u limits, but cut out these to create this minimum example). The constraints in the sample force the robot to start at joint position (0,0,0) and move to joint position (1,1,1), starting and ending at 0 velocity.

Things I've tried:

1. New costs penalizing the squared difference between x_dot at adjacent sample points. I believe this failed because the problem isn't with velocity changes between sample points, but between them.
2. Constraints around joint limits and velocity limits, which sometimes partially hide the problem
3. Increasing the mass / length of the final link has helped, but then it no longer reflects the true robot.
4. FirstOrderHold initial guesses similar to the swing up example
5. More sample points (just makes the oscillations faster)

Does anyone have advice on things worth trying out next to remove these oscillations from the reconstructed trajectory? Thank you!

from pydrake.all import (
    Rgba,
    StartMeshcat,
    Sphere,
    DiagramBuilder,
    AddMultibodyPlantSceneGraph,
    Parser,
    DirectCollocation,
    Solve,
    RigidTransform,
    MeshcatVisualizer,
    MeshcatVisualizerParams,
    Role,
    PiecewisePolynomial,
    SnoptSolver,
    ApplyVisualizationConfig, VisualizationConfig, ContactModel
)
import xacro
import numpy as np

meshcat = StartMeshcat()
meshcat.Delete()

print("BUILDING SCENE")
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
parser = Parser(plant, scene_graph)
urdf = xacro.process_file("models/robot.urdf.xacro").toprettyxml(indent='  ')
(robot,) = parser.AddModelsFromString(urdf, ".urdf")   
basebody = plant.GetBodyByName("base_link")
plant.WeldFrames(plant.world_frame(), basebody.body_frame(), RigidTransform([.1,.7,0]))
plant.set_contact_model(ContactModel.kPoint)
plant.Finalize()

print("CREATING VISUALIZER")
visualizer_config = VisualizationConfig()
visualizer_config.publish_illustration = True
visualizer_config.publish_inertia = True
ApplyVisualizationConfig(visualizer_config, builder, plant=plant, scene_graph=scene_graph, meshcat=meshcat)

print("CREATING CONTEXTS")
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context_clone = plant.GetMyContextFromRoot(context)
plant_context = plant.CreateDefaultContext()

print("PLANT FINALIZED")
NUM_JOINTS = plant.get_actuation_input_port().size()

print("DIRECT COLLOCATION")
num_samples = 11
time_constraint = 1

direct_collocation = DirectCollocation(
    plant,
    plant_context,
    num_time_samples=num_samples,
    input_port_index=plant.get_actuation_input_port().get_index(),
    minimum_time_step=time_constraint/num_samples * 0.9,
    maximum_time_step=time_constraint/num_samples * 1.1,
)
prog = direct_collocation.prog()

print("DEFINING COST")
R = np.diag([1, 2, 5])
u = direct_collocation.input()
direct_collocation.AddRunningCost(u.T @ R @ u)

print("DEFINE CONSTRAINTS")
direct_collocation.AddEqualTimeIntervalsConstraints()
direct_collocation.AddDurationBounds(time_constraint, time_constraint)

start_state = np.zeros(NUM_JOINTS*2)
end_state = np.concatenate([np.ones(NUM_JOINTS), np.zeros(NUM_JOINTS)])
prog.AddBoundingBoxConstraint(
    start_state, start_state, direct_collocation.initial_state())
prog.AddBoundingBoxConstraint(
    end_state, end_state, direct_collocation.final_state())

print("SOLVE")
solver = SnoptSolver()
solver_id = solver.solver_id()
major_tol = 1e-3
minor_tol = 1e-3
prog.SetSolverOption(solver_id, "Feasibility tolerance", major_tol)
prog.SetSolverOption(solver_id, "Major feasibility tolerance", major_tol)
prog.SetSolverOption(solver_id, "Major optimality tolerance", major_tol)
prog.SetSolverOption(solver_id, "Minor feasibility tolerance", minor_tol)
prog.SetSolverOption(solver_id, "Minor optimality tolerance", minor_tol)

result = Solve(prog)
print(f'success: {result.is_success()}')

[vincekurtz]

Just based on that third plot, it looks like the green line is fitting all the points (from x_d1, x_d2, and x_d3), not just the x_d3 points. Seems like maybe an issue with the reconstruction of the trajectory rather than the problem setup or solve.

[louietouie]

Oh, good point, I had not caught that. I’ll spend some time tonight trying to diagnose this and update with my findings here. Thank you for the help

x_trajectory = direct_collocation.ReconstructStateTrajectory(result)
x_times = np.linspace(x_trajectory.start_time(), x_trajectory.end_time(), 100)
x_values = x_trajectory.vector_values(x_times)

[louietouie]

Thank you again for the suggestion yesterday. I don't think the reconstruction is trying to fit to all points. When I make the link longer and heavier, this becomes more clear as the oscillations are not as extreme (see image below). My plan is to find a way to better understand / inspect the constraints at the mid collocation points, to see if that may be causing problems. Open to ideas!

With longer + heavier third link

With longer + heavier third link and without damping

[louietouie]

I added the system dynamics to the graph using EvalTimeDerivatives, since from my understanding, this is used to recreate the cubic hermite spline for the state. It was pretty revealing and I believe this is probably contributing to the oscillations. I realize this is somewhat beyond the scope of Drake-specifics/direct collocation now... but if anyone has any experience/advice on how to adjust the URDF / system dynamics for a smoother, more predictable velocity curve, I'm all ears. Thank you!

def get_system_dynamics(position, velocity, u):
    plant.SetPositions(plant_context, position)
    plant.SetVelocities(plant_context, velocity)
    plant.get_actuation_input_port().FixValue(plant_context, u)
    return(plant.EvalTimeDerivatives(plant_context).CopyToVector())

[ZC502]

The oscillations observed by @louietouie @vincekurtz are a textbook example of what I call Non-Associative Residuals (NAR).

Our research shows that in high-frequency dynamics, the causal chain between Sample Point A and B is NOT associative in 4D spacetime. When Drake's DirectCollocation forces a cubic spline to fit derivatives computed under traditional discrete-time assumptions, it inadvertently amplifies the algebraic errors into these 'spastic behaviors.'

I have used SIPA(Simulation Integrity & Physics Auditor) to audit similar trajectories in KUKA iiwa simulations. The tool consistently identifies these oscillations as 'numerical toxins' that real-world hardware will reject. Is the Drake community interested in exploring a Non-Associative Algebraic framework (e.g., Octonions) to resolve these sub-sample artifacts?

https://github.com/ZC502/SIPA.git

Observation: J2 axis experiences a massive acceleration spike ($>600\text{rad/s}^2$) near frame 60.

To understand why these sub-sample oscillations occur, we must look at the Discrete Associator of the solver pipeline. Here is the formal setting of the Non-Associative Residual Hypothesis (NARH) that SIPA uses to audit these trajectories:

Non-Associative Residual Hypothesis (NARH)

1. Setting

Consider a rigid-body simulation system defined by:

• a state space $S \subset \mathbb{R}^n$
• a nominal associative update operator $\Phi \Delta t : S \to S$
• a parallel constraint resolution pipeline composed of sub-operators $\{\Psi_i\}_{i=1}^k$
  ​
  The simulator advances the system state through a discrete update step:

$$ s_{t+1} = \Psi_{\sigma(k)} \circ \cdots \circ \Psi_{\sigma(1)} (s_t) $$

where the permutation 𝜎 represents an execution order determined by internal solver mechanisms such as:

• constraint partitioning
• thread scheduling
• contact batching
• solver splitting

Each operator $\Psi_i$ represents a well-defined physical constraint update (e.g., contact resolution, joint projection, or velocity correction).

However, the order in which these updates are applied may vary between solver iterations or execution contexts.

2. Order Sensitivity in Discrete Solvers

In continuous rigid-body mechanics, many transformations belong to associative algebraic structures (e.g., matrix multiplication or quaternion composition).

However, in practical simulation systems, constraint updates are implemented through finite-precision numerical approximations. Under such conditions the composed operators may exhibit order sensitivity:

$$(\Psi_a \circ \Psi_b) \circ \Psi_c \neq \Psi_a \circ (\Psi_b \circ \Psi_c)$$

This deviation may arise from:

• finite-precision arithmetic
• iterative solver truncation
• projection steps
• asynchronous or parallel execution

To quantify this effect, define the discrete associator:

$$ A(a,b,c;s) = \bigl( (\Psi_a \circ \Psi_b) \circ \Psi_c \bigr)(s) - \bigl( \Psi_a \circ (\Psi_b \circ \Psi_c) \bigr)(s) $$

3. Non-Associative Residual

The Non-Associative Residual (NAR) at state $s_t$ is defined as

$R_t = \lVert A(a,b,c; s_t) \rVert$

for a selected triple of constraint operators representative of the solver pipeline.

This residual does not represent algebraic non-associativity of the physical state space itself.
Instead, it measures order-dependent deviations introduced by the discrete numerical solver.

In other words, $R_t$ quantifies the degree to which the simulation outcome depends on the ordering of constraint updates.

4. Hypothesis (NARH)

The Non-Associative Residual Hypothesis (NARH) states that:

In high interaction density regimes — such as contact-rich robotic manipulation or high-speed trajectory execution — the non-associative residual can become non-negligible relative to trajectory stability metrics.

Over extended simulation horizons, this residual may accumulate as a structured drift component:

$\sum_{t=0}^{T} R_t \not\approx 0$

even when the state increments remain bounded:

$\| s_{t+1} - s_t \| < \epsilon$

This implies that solver order sensitivity may introduce small but measurable deviations in simulated trajectories.

5. Interpretation for Robot Simulation

NARH does not claim that simulators are mathematically incorrect or physically invalid.

Instead, it highlights a practical property of discrete rigid-body solvers:

Parallel constraint resolution may introduce small order-dependent residuals that are not explicitly represented in the simulation state.

For industrial robot simulations, such residuals may manifest as:

• micro-scale TCP jitter
• discontinuous pose jumps
• trajectory instability under small timestep changes
• sensitivity to solver parameterization

These effects are typically subtle but may become visible when analyzing robot trajectories in Cartesian space.

Tools such as SIPA (Simulation Integrity & Physics Auditor) analyze trajectory residuals to detect such phenomena in a diagnostic context.

6. Falsifiability

The NARH framework is empirically testable and may be falsified if:

1. The measured residual $R_t$ remains indistinguishable from numerical noise across interaction densities.
2. Reordering constraint application produces statistically identical trajectories.
3. Classical scalar stability metrics (e.g., kinetic energy norms or velocity bounds) detect instability earlier than any associator-derived signal.

In such cases, the solver behavior may be considered effectively order-invariant for the examined regime.

7. Practical Implication

If validated empirically, NARH suggests that:

• Order sensitivity is an inherent property of discrete constraint solvers.
• Residual-based diagnostics can serve as early indicators of trajectory instability.
• Trajectory analysis tools may benefit from monitoring solver-induced residual signals in addition to traditional stability metrics.

For industrial robot simulation pipelines, such diagnostics can assist in verifying trajectory physical consistency before deployment to real hardware.

References:
Continuous_Physics_Solver_for_AI_Wang_Liu(2025)
https://github.com/ZC502/TinyOEKF/blob/master/docs/Continuous_Physics_Solver_for_AI_Wang_Liu.pdf

[ZC502]

To @louietouie @vincekurtz ,Please look here: an octonion solution:#24061 (comment)