Merge branch 'main' into ps_py

Author: erez-tom

mjpc/direct/direct.cc

@@ -64,12 +64,12 @@ void Direct::Initialize(const mjModel* model) {
   }
 
   // timestep
-  this->model->opt.timestep = GetNumberOrDefault(this->model->opt.timestep,
-                                                 model, "estimator_timestep");
+  this->model->opt.timestep =
+      GetNumberOrDefault(this->model->opt.timestep, model, "direct_timestep");
 
   // length of configuration trajectory
   configuration_length_ =
-      GetNumberOrDefault(3, model, "batch_configuration_length");
+      GetNumberOrDefault(3, model, "direct_configuration_length");
 
   // check configuration length
   if (configuration_length_ > max_history_) {

mjpc/estimators/batch.cc

@@ -49,6 +49,16 @@ void Batch::Initialize(const mjModel* model) {
   // base method
   Direct::Initialize(model);
 
+  // set batch size
+  int batch_size = std::min(
+      std::max(GetNumberOrDefault(3, model, "batch_configuration_length"),
+               kMinDirectHistory),
+      kMaxFilterHistory);
+
+  if (batch_size != configuration_length_) {
+    SetConfigurationLength(batch_size);
+  }
+
   // allocation dimension
   int nq = model->nq, nv = model->nv, na = model->na;
   int nvel_max = nv * max_history_;

mjpc/estimators/batch.h

@@ -32,7 +32,7 @@
 namespace mjpc {
 
 // max filter history
-inline constexpr int kMaxFilterHistory = 32;
+inline constexpr int kMaxFilterHistory = 128;
 
 // ----- batch estimator ----- //
 // based on: "Physically-Consistent Sensor Fusion in Contact-Rich Behaviors"
@@ -78,6 +78,11 @@ class Batch : public Direct, public Estimator {
   // get data
   mjData* Data() override { return data_[0].get(); };
 
+  // get qfrc
+  double* Qfrc() override {
+    return force_prediction.Get(configuration_length_ - 2);
+  };
+
   // get process noise
   double* ProcessNoise() override { return noise_process.data(); };
 

mjpc/estimators/estimator.h

@@ -60,6 +60,9 @@ class Estimator {
   // get data
   virtual mjData* Data() = 0;
 
+  // get qfrc
+  virtual double* Qfrc() = 0;
+
   // process noise
   virtual double* ProcessNoise() = 0;
 
@@ -220,6 +223,9 @@ class GroundTruth : public Estimator {
   // get data
   mjData* Data() override { return data_; };
 
+  // get qfrc
+  double* Qfrc() override { return data_->qfrc_actuator; }
+
   // get process noise
   double* ProcessNoise() override { return noise_process.data(); };
 

mjpc/estimators/kalman.h

@@ -80,6 +80,9 @@ class Kalman : public Estimator {
   // get data
   mjData* Data() override { return data_; };
 
+  // get qfrc
+  double* Qfrc() override { return data_->qfrc_actuator; };
+
   // get process noise
   double* ProcessNoise() override { return noise_process.data(); };
 

mjpc/estimators/unscented.cc

@@ -37,6 +37,10 @@ void Unscented::Initialize(const mjModel* model) {
   if (this->data_) mj_deleteData(this->data_);
   data_ = mj_makeData(model);
 
+  // settings
+  settings.alpha = GetNumberOrDefault(1.0, model, "unscented_alpha");
+  settings.beta = GetNumberOrDefault(2.0, model, "unscented_beta");
+
   // timestep
   this->model->opt.timestep = GetNumberOrDefault(this->model->opt.timestep,
                                                  model, "estimator_timestep");

mjpc/estimators/unscented.h

@@ -81,6 +81,9 @@ class Unscented : public Estimator {
   // get data
   mjData* Data() override { return data_; };
 
+  // get qfrc
+  double* Qfrc() override { return data_->qfrc_actuator; };
+
   // get process noise
   double* ProcessNoise() override { return noise_process.data(); };
 

mjpc/grpc/filter.proto

@@ -56,6 +56,7 @@ message UpdateResponse {}
 message State {
   repeated double state = 1 [packed = true];
   optional double time = 2;
+  repeated double qfrc = 3 [packed = true];
 }
 
 message StateRequest {

mjpc/grpc/filter_service.cc

@@ -174,6 +174,12 @@ grpc::Status FilterService::State(grpc::ServerContext* context,
   // get time
   output->set_time(active_filter->Time());
 
+  // get qfrc
+  double* qfrc = active_filter->Qfrc();
+  for (int i = 0; i < model->nv; i++) {
+    output->add_qfrc(qfrc[i]);
+  }
+
   return grpc::Status::OK;
 }