comment out batch filter until inverse dynamics fix

ltinphan · Aug 25, 2023 · e7d57c3 · e7d57c3
1 parent 341e9d4
commit e7d57c3
Show file tree

Hide file tree

Showing 4 changed files with 55 additions and 10 deletions.
diff --git a/mjpc/estimators/include.cc b/mjpc/estimators/include.cc
@@ -23,8 +23,8 @@ namespace mjpc {
 const char kEstimatorNames[] =
     "Ground Truth\n"
     "Kalman\n"
-    "Unscented\n"
-    "Batch";
+    "Unscented";
+    // "Batch";
 
 // load all available estimators
 std::vector<std::unique_ptr<mjpc::Estimator>> LoadEstimators() {
@@ -35,7 +35,7 @@ std::vector<std::unique_ptr<mjpc::Estimator>> LoadEstimators() {
   estimators.emplace_back(new mjpc::GroundTruth());  // ground truth state
   estimators.emplace_back(new mjpc::Kalman());       // extended Kalman filter
   estimators.emplace_back(new mjpc::Unscented());    // unscented Kalman filter
-  estimators.emplace_back(new mjpc::Batch(1));       // recursive batch filter
+  // estimators.emplace_back(new mjpc::Batch(1));       // recursive batch filter
 
   return estimators;
 }

diff --git a/mjpc/tasks/cartpole/task.xml b/mjpc/tasks/cartpole/task.xml
@@ -19,9 +19,9 @@
     <!-- estimator -->
     <numeric name="estimator" data="0" />
     <numeric name="estimator_sensor_start" data="4" />
-    <numeric name="estimator_number_sensor" data="3" />
+    <numeric name="estimator_number_sensor" data="2" />
     <numeric name="estimator_timestep" data="0.0025" />
-    <numeric name="batch_configuration_length" data="32" />
+    <numeric name="batch_configuration_length" data="5" />
     <numeric name="batch_scale_prior" data="1.0e-1" />
   </custom>
 
@@ -35,7 +35,8 @@
     <!-- estimator -->
     <jointpos name="slider_pos" joint="slider"/>
     <jointpos name="hinge_pos" joint="hinge_1"/>
-    <accelerometer name="tip_acc" site="tip"/>
+    <!-- <jointvel name="vslider_pos" joint="slider"/>
+    <jointvel name="vhinge_pos" joint="hinge_1"/> -->
 
     <!-- trace -->
     <framepos name="trace0" objtype="site" objname="tip"/>

diff --git a/mjpc/tasks/swimmer/task.xml b/mjpc/tasks/swimmer/task.xml
@@ -14,12 +14,12 @@
     <numeric name="gradient_spline_points" data="10" />
 
     <!-- estimator -->
-    <!-- <numeric name="estimator" data="0" /> -->
+    <numeric name="estimator" data="0" />
     <numeric name="estimator_sensor_start" data="2" />
     <numeric name="estimator_number_sensor" data="8" />
     <numeric name="estimator_timestep" data="0.0075" />
-    <numeric name="batch_configuration_length" data="5" />
-    <numeric name="batch_scale_prior" data="1.0e-8" />
+    <numeric name="batch_configuration_length" data="3" />
+    <numeric name="batch_scale_prior" data="1.0e-3" />
   </custom>
 
   <worldbody>
@@ -44,6 +44,15 @@
     <jointpos name="joint3" joint="joint_3" />
     <jointpos name="joint4" joint="joint_4" />
 
+    <!-- <jointvel name="vrootx" joint="rootx" />
+    <jointvel name="vrooty" joint="rooty" />
+    <jointvel name="vrootz" joint="rootz" />
+    <jointvel name="vjoint0" joint="joint_0" />
+    <jointvel name="vjoint1" joint="joint_1" />
+    <jointvel name="vjoint2" joint="joint_2" />
+    <jointvel name="vjoint3" joint="joint_3" />
+    <jointvel name="vjoint4" joint="joint_4" /> -->
+
     <!-- trace -->
     <framepos name="trace0" objtype="geom" objname="nose"/>
 

diff --git a/mjpc/tasks/walker/task.xml b/mjpc/tasks/walker/task.xml
@@ -5,22 +5,57 @@
   <size memory="100K"/>
 
   <custom>
-    <numeric name="agent_planner" data="0" />
+    <!-- agent -->
+    <numeric name="agent_planner" data="2" />
     <numeric name="agent_horizon" data="0.8" />
     <numeric name="agent_timestep" data="0.01" />
     <numeric name="sampling_spline_points" data="3" />
     <numeric name="sampling_exploration" data="0.5" />
     <numeric name="gradient_spline_points" data="5" />
     <numeric name="residual_Height Goal" data="1.2 0.5 1.2" />
     <numeric name="residual_Speed Goal" data="0 -5.0 5.0" />
+
+    <!-- estimator -->
+    <numeric name="estimator" data="0" />
+    <numeric name="estimator_sensor_start" data="4" />
+    <numeric name="estimator_number_sensor" data="9" />
+    <numeric name="estimator_timestep" data="0.005" />
+    <numeric name="batch_configuration_length" data="7" />
+    <numeric name="batch_scale_prior" data="1.0e-8" />
   </custom>
 
   <sensor>
+    <!-- cost -->
     <user name="Control" dim="6" user="0 0.1 0.0 1.0" />
     <user name="Height" dim="1" user="0 10.0 0.0 10.0" />
     <user name="Rotation" dim="1" user="0 3.0 0.0 5.0" />
     <user name="Speed" dim="1" user="0 1.0 0.0 1.0" />
+
+    <!-- estimator -->
+    <jointpos name="rootz" joint="rootz" />
+    <jointpos name="rootx" joint="rootx" />
+    <jointpos name="rooty" joint="rooty" />
+    <jointpos name="right_hip" joint="right_hip" />
+    <jointpos name="right_knee" joint="right_knee" />
+    <jointpos name="right_ankle" joint="right_ankle" />
+    <jointpos name="left_hip" joint="left_hip" />
+    <jointpos name="left_knee" joint="left_knee" />
+    <jointpos name="left_ankle" joint="left_ankle" />
+
+    <!-- <jointvel name="vrootz" joint="rootz" />
+    <jointvel name="vrootx" joint="rootx" />
+    <jointvel name="vrooty" joint="rooty" />
+    <jointvel name="vright_hip" joint="right_hip" />
+    <jointvel name="vright_knee" joint="right_knee" />
+    <jointvel name="vright_ankle" joint="right_ankle" />
+    <jointvel name="vleft_hip" joint="left_hip" />
+    <jointvel name="vleft_knee" joint="left_knee" />
+    <jointvel name="vleft_ankle" joint="left_ankle" /> -->
+
+    <!-- trace -->
     <framepos      name="trace0" objtype="site" objname="torso_site"/>
+
+    <!-- residual -->
     <framepos      name="torso_position" objtype="xbody" objname="torso"/>
     <subtreecom    name="torso_subtreecom" body="torso"/>
     <subtreelinvel name="torso_subtreelinvel" body="torso"/>