Merge of 7326f21

PiperOrigin-RevId: 493010164 Change-Id: I8999d4ff5ac2375832eaa99d5ad444a3e93eb600
ltinphan · Dec 5, 2022 · 9d036f6 · 9d036f6
2 parents d8c9b6e + 7326f21
commit 9d036f6
Show file tree

Hide file tree

Showing 21 changed files with 370 additions and 293 deletions.
diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
@@ -24,7 +24,10 @@ jobs:
               -G Ninja
             tmpdir: "/tmp"
           - os: windows-2022
-            cmake_build_args: "-- -m"
+            cmake_args: >-
+              -G Ninja
+              -DCMAKE_C_COMPILER:STRING=clang
+              -DCMAKE_CXX_COMPILER:STRING=clang++
             tmpdir: "C:/Temp"
 
     name: "MuJoCo MPC on ${{ matrix.os }} ${{ matrix.additional_label }}"

diff --git a/mjpc/agent.cc b/mjpc/agent.cc
@@ -161,6 +161,11 @@ void Agent::Plan(std::atomic<bool>& exitrequest,
       model_->opt.timestep = timestep_;
       model_->opt.integrator = integrator_;
 
+      // make contacts differentiable
+      // for (int i = 0; i < model_->ngeom; i++) {
+      //   model_->geom_solimp[mjNIMP * i] = 0.0;
+      // }
+
       // set planning steps
       steps_ =
           mju_max(mju_min(horizon_ / timestep_ + 1, kMaxTrajectoryHorizon), 1);

diff --git a/mjpc/main.cc b/mjpc/main.cc
@@ -343,7 +343,6 @@ void PhysicsLoop(mj::Simulate& sim) {
 
             // run single step, let next iteration deal with timing
             mj_step(m, d);
-
           } else {  // in-sync: step until ahead of cpu
             bool measured = false;
             mjtNum prevSim = d->time;

diff --git a/mjpc/planners/cost_derivatives.cc b/mjpc/planners/cost_derivatives.cc
@@ -177,39 +177,39 @@ void CostDerivatives::Compute(double* r, double* rx, double* ru,
         mju_scl(DataAt(cd.cxx, t * dim_state_derivative * dim_state_derivative),
                 DataAt(cd.cxx, t * dim_state_derivative * dim_state_derivative),
                 s, dim_state_derivative * dim_state_derivative);
-        mju_mulMatMat(cd.cxx_scratch_.data(),
+        mju_mulMatMat(DataAt(cd.cxx_scratch_, t * dim_state_derivative * dim_state_derivative),
                       DataAt(cd.cx, t * dim_state_derivative),
                       DataAt(cd.cx, t * dim_state_derivative),
                       dim_state_derivative, 1, dim_state_derivative);
-        mju_scl(cd.cxx_scratch_.data(), cd.cxx_scratch_.data(), risk * s,
+        mju_scl(DataAt(cd.cxx_scratch_, t * dim_state_derivative * dim_state_derivative), DataAt(cd.cxx_scratch_, t * dim_state_derivative * dim_state_derivative), risk * s,
                 dim_state_derivative * dim_state_derivative);
         mju_addTo(
             DataAt(cd.cxx, t * dim_state_derivative * dim_state_derivative),
-            cd.cxx_scratch_.data(),
+            DataAt(cd.cxx_scratch_, t * dim_state_derivative * dim_state_derivative),
             dim_state_derivative * dim_state_derivative);
 
         // cxu
         mju_scl(DataAt(cd.cxu, t * dim_state_derivative * dim_action),
                 DataAt(cd.cxu, t * dim_state_derivative * dim_action), s,
                 dim_state_derivative * dim_action);
         mju_mulMatMat(
-            cd.cxu_scratch_.data(), DataAt(cd.cx, t * dim_state_derivative),
+            DataAt(cd.cxu_scratch_, t * dim_state_derivative * dim_action), DataAt(cd.cx, t * dim_state_derivative),
             DataAt(cd.cu, t * dim_action), dim_state_derivative, 1, dim_action);
-        mju_scl(cd.cxu_scratch_.data(), cd.cxu_scratch_.data(), risk * s,
+        mju_scl(DataAt(cd.cxu_scratch_, t * dim_state_derivative * dim_action), DataAt(cd.cxu_scratch_, t * dim_state_derivative * dim_action), risk * s,
                 dim_state_derivative * dim_action);
         mju_addTo(DataAt(cd.cxu, t * dim_state_derivative * dim_action),
-                  cd.cxu_scratch_.data(), dim_state_derivative * dim_action);
+                  DataAt(cd.cxu_scratch_, t * dim_state_derivative * dim_action), dim_state_derivative * dim_action);
 
         // cuu
         mju_scl(DataAt(cd.cuu, t * dim_action * dim_action),
                 DataAt(cd.cuu, t * dim_action * dim_action), s,
                 dim_action * dim_action);
-        mju_mulMatMat(cd.cuu_scratch_.data(), DataAt(cd.cu, t * dim_action),
+        mju_mulMatMat(DataAt(cd.cuu_scratch_, t * dim_action * dim_action), DataAt(cd.cu, t * dim_action),
                       DataAt(cd.cu, t * dim_action), dim_action, 1, dim_action);
-        mju_scl(cd.cuu_scratch_.data(), cd.cuu_scratch_.data(), risk * s,
+        mju_scl(DataAt(cd.cuu_scratch_, t * dim_action * dim_action), DataAt(cd.cuu_scratch_, t * dim_action * dim_action), risk * s,
                 dim_action * dim_action);
         mju_addTo(DataAt(cd.cuu, t * dim_action * dim_action),
-                  cd.cuu_scratch_.data(), dim_action * dim_action);
+                  DataAt(cd.cuu_scratch_, t * dim_action * dim_action), dim_action * dim_action);
       });
     }
     pool.WaitCount(count_before + T);