Fix bug in cube residual, improve formatting.

PiperOrigin-RevId: 603333266
Change-Id: I8eeae82bc46fd3ddd4311f9da38a28b12cd848ca

Author: erez-tom

mjpc/planners/sampling/planner.cc

@@ -433,9 +433,10 @@ void SamplingPlanner::Plots(mjvFigure* fig_planner, mjvFigure* fig_timer,
 
   // ----- timer ----- //
 
-  PlotUpdateData(
-      fig_timer, timer_bounds, fig_timer->linedata[0 + timer_shift][0] + 1,
-      1.0e-3 * noise_compute_time * planning, 100, 0 + timer_shift, 0, 1, -100);
+  PlotUpdateData(fig_timer, timer_bounds,
+                 fig_timer->linedata[0 + timer_shift][0] + 1,
+                 1.0e-3 * noise_compute_time * planning, 100,
+                 0 + timer_shift, 0, 1, -100);
 
   PlotUpdateData(fig_timer, timer_bounds,
                  fig_timer->linedata[1 + timer_shift][0] + 1,

mjpc/tasks/cube/solve.cc

@@ -109,11 +109,17 @@ void CubeSolve::ResidualFn::Residual(const mjModel* model, const mjData* data,
   counter += 6;
 
   // ---------- Residual (4) ----------
+
+  // The unmodified cube model has 20 ball joints: nq=86, nv=66.
+  // The patch adds a free joint: nq=93, nv=72.
+  // The task adds a ball joint: nq=97, nv=75.
+  // The shadow hand has 24 DoFs: nq=121, nv=99.
+  // The following two residuals apply for the last 24 entries of qpos and qvel:
   mju_sub(residual + counter, data->qpos + 97, model->key_qpos + 97, 24);
   counter += 24;
 
   // ---------- Residual (5) ----------
-  mju_copy(residual + counter, data->qvel + 97, 24);
+  mju_copy(residual + counter, data->qvel + 75, 24);
   counter += 24;
 
   // ---------- Residual (6) ----------