# Debugging Guide

**Warning:** You should only use these tools actively while debugging and preferably only in simulation, because it slows down the control loop.

When a motion does the wrong thing — or the controller fails to produce a command at all — you
need to see what Giskard is computing internally. This guide covers three tools, each answering
a different question:

- **Debug expressions** let you track any symbolic expression (a scalar or a spatial type) that
  a node computes, so you can watch it change over time.
  - **Visualize them live in RViz** to answer *"where is this point/vector/pose right now,
    and where does the goal want it?"*
  - **Plot them to a PDF** to answer *"how did this value evolve over the whole motion?"*
- The **`QuadraticProgramDebugger`** answers *"why did the QP behave this way?"* — it labels the
  raw solver arrays with constraint and degree-of-freedom names so you can read bounds, weights,
  and constraint violations by name.

## Debug expressions

A `DebugExpression`
(`giskardpy.motion_statechart.graph_node.DebugExpression`) wraps three things:

- a `name`, used as the label in every debugging tool,
- an `expression` — either a `Scalar` or a spatial type (`Point3`, `Vector3`, `Quaternion`,
  `RotationMatrix`, `HomogeneousTransformationMatrix`, or `Pose`), and
- a `color`, used when the expression is rendered.

A node registers a debug expression by appending it to `artifacts.debug_expressions` inside its
`build_artifacts()` method:

```python
from semantic_digital_twin.world_description.geometry import Color
from giskardpy.motion_statechart.graph_node import DebugExpression

artifacts.debug_expressions.append(
    DebugExpression(
        name=f"{self.name}/current_normal",
        expression=root_V_tip_normal,
        color=Color.from_rgba(red=1, green=0, blue=0, opacity=1),
    )
)
```

See `tasks/align_planes.py` for a worked example. For Cartesian tasks there is a convenience
helper, `CartesianTask.add_goal_and_current_debug_expressions(artifacts, goal, current)`
(`tasks/cartesian_tasks.py`), which registers `<task name>/goal` (green) and
`<task name>/current` (red) so the two can be told apart in RViz.

Every node's debug expressions are gathered through
`DebugExpression.collect_from(statechart)`; the visualizer and plotter below consume that
list, so you only have to register the expression once.

```{note}
Plain `Scalar` expressions are plotted but **not** drawn in RViz — only spatial types become
markers. Use the plotter to inspect scalars.
```

## Visualizing debug expressions in RViz

The `DebugExpressionPublisher`
(`giskardpy.motion_statechart.debug_expression_publisher`) turns every spatial-typed debug
expression into an RViz marker and republishes it whenever the world state changes.

You do not construct it directly. Instead, add the `DebugExpressionPublishing` extension to
the executor:

```python
from cramph.context import StatechartContext
from cramph.executor import StatechartExecutor
from giskardpy.motion_control import MotionControl
from giskardpy.motion_statechart.debug_expression_publisher import (
    DebugExpressionPublishing,
)
from giskardpy.motion_statechart.ros_context import RosNodeAccess

executor = StatechartExecutor(
    context=StatechartContext(world=world),
    extensions=[RosNodeAccess(node), MotionControl(), DebugExpressionPublishing(node)],
)
executor.compile(motion_statechart)
```

On `compile()`, the extension builds the publisher and attaches it to the statechart. Each
expression's `name` becomes the marker namespace and label, so individual expressions can be
toggled on and off in RViz's display tree. `Vector3` expressions are anchored to their
`visualisation_frame` so the arrow is drawn at the right body.

This is the right tool for spatial reasoning: comparing a goal pose against the current pose,
checking which way a normal points, or confirming a point is where you expect it in the world.

Each spatial type renders as a different marker, and the `<task name>/goal` (green) /
`<task name>/current` (red) convention makes the comparison easy to read:

```{figure} img/pose_marker.png
---
width: 600px
---
A `Pose` is drawn as a coordinate frame. Here `CartesianPose/goal` and `CartesianPose/current`
show the target and actual end-effector pose side by side.
```

```{figure} img/cartesian_position_marker.png
---
width: 600px
---
A `Point3` is drawn as a sphere — `CartesianPosition/goal` (green) and
`CartesianPosition/current` (red).
```

```{figure} img/pointing_marker.png
---
width: 500px
---
A `Vector3` is drawn as an arrow anchored to its `visualisation_frame`. Here the green
`Pointing/goal` and red `Pointing/current` arrows show where the robot should point versus
where it currently points.
```

## Plotting debug expressions

To see how debug expressions evolve over an entire motion, add a `DebugExpressionRecording`
(`giskardpy.motion_control`) to the executor, after `MotionControl`. It holds a
`DebugExpressionTrajectoryPlotter`
(`giskardpy.motion_statechart.plotters.debug_expression_trajectory_plotter`):

```python
from cramph.context import StatechartContext
from cramph.executor import StatechartExecutor
from giskardpy.motion_control import DebugExpressionRecording, MotionControl

executor = StatechartExecutor(
    context=StatechartContext(world=world),
    extensions=[MotionControl(), DebugExpressionRecording()],
)
executor.compile(motion_statechart)
executor.tick_until_end()
executor.require_extension(DebugExpressionRecording).plotter.plot(
    "./debug_expressions.pdf"
)
```

The lifecycle is:

- `compile()` calls the plotter's `reset(...)`, which builds a fresh trajectory from
  `DebugExpression.collect_from(statechart)` and discards any previous recording.
- Every `tick()` appends the value of each expression after the commands of that tick were
  applied.
- `plotter.plot(file_name)` writes the PDF. `require_extension` raises
  `MissingExecutorExtensionError` if the executor has no `DebugExpressionRecording`.

The plot has one subplot per expression and one line per scalar component, sharing a common time
axis — for example a `Point3` produces four lines (its homogeneous `x, y, z, w` components). The
plotter exposes a few layout knobs: `subplot_height_in_cm`, `second_width_in_cm`, and `legend`.

```{figure} img/test_pointing_debug_expressions.png
---
width: 600px
---
An example plot for a pointing task: the `Pointing/goal` and `Pointing/current` `Vector3`
expressions, each split into their four homogeneous components, show the goal vector and the
current vector converging over the motion.
```

For a runnable end-to-end example, see
`test/giskardpy_test/test_motion_statechart/test_debug_expression_trajectory.py`.

## Inspecting the QP with `QuadraticProgramDebugger`

When implementing new motion statechart nodes, the controller may produce an unintended command, runs into limits, or the solve fails.
To debug this, it may help to look directly into the quadratic program (QP) itself. 
The QP is just large, anonymous numeric arrays;
the `QuadraticProgramDebugger` (`giskardpy.qp.qp_debugger`) re-labels those arrays with the corresponding
constraint and degree-of-freedom names and exposes them as `pandas.DataFrame`s, so you can read
the problem by name.

Every `QPController` builds one automatically as `controller.debugger` (with no solution yet).
To inspect it:

 - you should run the program in debug mode 
 - place a break point in the last line of `QPController.compute_command(...)`
 - add a watch saying `self.debugger.update(solution)` (when the solve fails, use `update(None)`)
 - look at `self.debugger` in the debugger

### Reading the output

The debugger produces five labeled frames:

- **`direct_limits`** — one row per free variable, with its lower/upper box bounds, the solution
  value, and its quadratic and linear weight. Use it to spot a variable pinned at a bound, a
  zero or oversized weight, or a solution that landed outside its box. Constraints with a weight of `0` are inactive and will have `nan` values.
- **`equality_constraints`** — columns for the constraint value (matrix × decision variables,
  excluding slack), the `slack` (`bounds − value`, i.e. how much the constraint is violated),
  and the `bounds`. A large `slack` pinpoints the violated equality constraint by name.
- **`inequality_constraints`** — same as the previous, but for inequality constraints.
- **`equality_matrix` / `inequality_matrix`** — the labeled constraint matrices (rows are
  constraint names, columns are degree-of-freedom names). It's essentially a Jacobian matrix, so look at how degrees of freedom influence your constraints. `0` means no influence, a positive value means increasing the value will also increase constraints' expression, a negative value means decreasing the value will increase the constraints' expression.