scale prior gui slider

google-deepmind · Aug 20, 2023 · 07e466c · 07e466c
1 parent db2670f
commit 07e466c
Show file tree

Hide file tree

Showing 5 changed files with 20 additions and 12 deletions.
diff --git a/mjpc/estimators/batch.cc b/mjpc/estimators/batch.cc
@@ -543,7 +543,7 @@ void Batch::Update(const double* ctrl, const double* sensor) {
   mjData* d = data_[0].get();
 
   // current time index
-  int t = current_time_index;
+  int t = current_time_index_;
 
   // configurations
   double* q0 = configuration.Get(t - 1);
@@ -585,7 +585,7 @@ void Batch::Update(const double* ctrl, const double* sensor) {
   int configuration_length_cache = configuration_length_;
 
   // set reduced configuration length for optimization
-  configuration_length_ = current_time_index + 2;
+  configuration_length_ = current_time_index_ + 2;
   if (configuration_length_ != configuration_length_cache) {
     ShiftResizeTrajectory(0, configuration_length_);
   }
@@ -642,7 +642,7 @@ void Batch::Update(const double* ctrl, const double* sensor) {
   } else {
     // dimension
     int nvar_new = nvar;
-    if (current_time_index < configuration_length_ - 2) {
+    if (current_time_index_ < configuration_length_ - 2) {
       nvar_new += nv;
     }
 
@@ -674,8 +674,8 @@ void Batch::Update(const double* ctrl, const double* sensor) {
   configuration_length_ = configuration_length_cache;
 
   // check estimation horizon
-  if (current_time_index < configuration_length_ - 2) {
-    current_time_index++;
+  if (current_time_index_ < configuration_length_ - 2) {
+    current_time_index_++;
   } else {
     // shift trajectories once estimation horizon is filled
     Shift(1);
@@ -1940,7 +1940,7 @@ void Batch::InitializeFilter() {
   int nq = model->nq, nv = model->nv;
 
   // filter mode status
-  current_time_index = 1;
+  current_time_index_ = 1;
 
   // filter settings
   settings.gradient_tolerance = 1.0e-6;
@@ -2711,6 +2711,7 @@ void Batch::GUI(mjUI& ui, EstimatorGUIData& data) {
       {mjITEM_SELECT, "Integrator", 2, &data.integrator,
        "Euler\nRK4\nImplicit\nFastImplicit"},
       {mjITEM_SLIDERINT, "Horizon", 2, &data.horizon, "3 3"},
+      {mjITEM_SLIDERNUM, "Prior Scale", 2, &data.scale_prior, "1.0e-8 0.1"},
       {mjITEM_END}};
 
   // set estimation horizon limits
@@ -2932,7 +2933,7 @@ void Batch::SetGUIData(EstimatorGUIData& data) {
 
     // update configuration length and current time index
     configuration_length_ = horizon;
-    current_time_index -= horizon_diff;
+    current_time_index_ -= horizon_diff;
   }
 }
 

diff --git a/mjpc/estimators/batch.h b/mjpc/estimators/batch.h
@@ -159,7 +159,7 @@ class Batch : public Estimator {
     times.Set(&time_copy, 0);
 
     // reset current time index
-    current_time_index = 1;
+    current_time_index_ = 1;
   }
 
   // set covariance
@@ -474,9 +474,6 @@ class Batch : public Estimator {
     bool flg_actuation = 1;
   } finite_difference;
 
-  // filter mode status
-  int current_time_index;
-
  private:
   // convert sequence of configurations to velocities, accelerations
   void ConfigurationToVelocityAcceleration();
@@ -715,6 +712,9 @@ class Batch : public Estimator {
   std::vector<double> scratch0_condmat_;
   std::vector<double> scratch1_condmat_;
 
+  // filter mode status
+  int current_time_index_;
+
   // status (internal)
   int cost_count_;          // number of cost evaluations
   bool cost_skip_ = false;  // flag for only evaluating cost derivatives

diff --git a/mjpc/estimators/gui.cc b/mjpc/estimators/gui.cc
@@ -37,6 +37,9 @@ void EstimatorGUIData::Initialize(const mjModel* model, int nprocess,
   // sensor noise
   sensor_noise.resize(nsensor);
 
+  // scale prior 
+  scale_prior = GetNumberOrDefault(1.0, model, "batch_scale_prior");
+
   // estimation horizon
   horizon = GetNumberOrDefault(3, model, "batch_configuration_length");
 }

diff --git a/mjpc/estimators/gui.h b/mjpc/estimators/gui.h
@@ -45,6 +45,9 @@ class EstimatorGUIData {
   // sensor noise
   std::vector<double> sensor_noise;
 
+  // scale prior 
+  double scale_prior;
+
   // estimation horizon
   int horizon;
 };

diff --git a/mjpc/utilities.h b/mjpc/utilities.h
@@ -401,7 +401,8 @@ void PrincipalEigenVector4(double* res, const double* mat,
 
 // set scaled symmetric block matrix in band matrix
 void SetBlockInBand(double* band, const double* block, double scale, int ntotal,
-                    int nband, int nblock, int shift, int row_skip = 0, bool add = true);
+                    int nband, int nblock, int shift, int row_skip = 0,
+                    bool add = true);
 
 }  // namespace mjpc