Merge remote-tracking branch 'upstream/main' into sample_gradient_v2

Author: thowell

.github/workflows/build.yml

@@ -68,7 +68,7 @@ jobs:
         $cmake_extra_args
     - name: Build MuJoCo MPC
       working-directory: build
-      run: cmake --build . --config=Release ${{ matrix.cmake_build_args }} --target mjpc agent_test cost_derivatives_test norm_test rollout_test threadpool_test trajectory_test utilities_test direct_force_test direct_optimize_test direct_parameter_test direct_sensor_test direct_trajectory_test direct_utilities_test batch_filter_test batch_prior_test kalman_test unscented_test ${{ matrix.additional_targets }}
+      run: cmake --build . --config=Release ${{ matrix.cmake_build_args }} --target mjpc agent_test agent_utilities_test cost_derivatives_test norm_test rollout_test threadpool_test trajectory_test direct_force_test direct_optimize_test direct_parameter_test direct_sensor_test direct_trajectory_test direct_utilities_test batch_filter_test batch_prior_test kalman_test unscented_test cubic_test gradient_planner_test gradient_test linear_test zero_test backward_pass_test ilqg_test robust_planner_test sampling_planner_test state_test task_test utilities_test ${{ matrix.additional_targets }}
     - name: Test MuJoCo MPC
       working-directory: build
       run: ctest -C Release --output-on-failure .

mjpc/agent.cc

@@ -73,6 +73,19 @@ void Agent::Initialize(const mjModel* model) {
   if (model_) mj_deleteModel(model_);
   model_ = mj_copyModel(nullptr, model);  // agent's copy of model
 
+  // check for limits on all actuators
+  int num_missing = 0;
+  for (int i = 0; i < model_->nu; i++) {
+    if (!model_->actuator_ctrllimited[i]) {
+      num_missing++;
+      printf("%s (actuator %i) missing limits\n",
+             model_->names + model_->name_actuatoradr[i], i);
+    }
+  }
+  if (num_missing > 0) {
+    mju_error("Ctrl limits required for all actuators.\n");
+  }
+
   // planner
   planner_ = GetNumberOrDefault(0, model, "agent_planner");
 

mjpc/planners/sampling/planner.cc

@@ -432,9 +432,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) ----------

mjpc/test/agent/agent_utilities_test.cc

@@ -234,25 +234,6 @@ TEST(AgentUtilitiesTest, Clamp) {
   EXPECT_NEAR(x[2], 0.0, 1.0e-5);
 }
 
-TEST(AgentUtilitiesTest, PowerSequence) {
-  // sequence
-  double sequence[4] = {0.2, 0.3, 0.4, 0.5};
-  int length = 4;
-  double step = 0.1;
-
-  // power
-  double power = 2.0;
-
-  // power sequence
-  PowerSequence(sequence, step, sequence[0], sequence[3], power, length);
-
-  // test
-  EXPECT_NEAR(sequence[0], 0.2, 1.0e-5);
-  EXPECT_NEAR(sequence[1], 0.27142857, 1.0e-5);
-  EXPECT_NEAR(sequence[2], 0.37142857, 1.0e-5);
-  EXPECT_NEAR(sequence[3], 0.5, 1.0e-5);
-}
-
 TEST(AgentUtilitiesTest, FindInterval) {
   // sequence
   std::vector<double> sequence{-1.0, 0.0, 1.0, 2.0};