remove log for hartleys estimator

jrl-umi3218 · Feb 7, 2024 · 8dcf2e5 · 8dcf2e5
1 parent a5e0709
commit 8dcf2e5
Showing 1 changed file with 0 additions and 73 deletions.
diff --git a/src/TiltObserver.cpp b/src/TiltObserver.cpp
@@ -681,79 +681,6 @@ void TiltObserver::addToLogger(const mc_control::MCController & ctl,
   logger.addLogEntry(category + "_FloatingBase_world_vel", [this]() -> const sva::MotionVecd & { return velW_; });
   logger.addLogEntry(category + "_debug_x1", [this]() -> const so::Vector3 & { return x1_; });
 
-  logger.addLogEntry(category + "_Hartley_contact1_isSet",
-                     [this]() -> int
-                     {
-                       odometry::LoContactWithSensor & contact =
-                           (*odometryManager_.contactsManager().contacts().begin()).second;
-
-                       if(contact.isSet()) { return 1; }
-                       else { return 0; }
-                     });
-  logger.addLogEntry(category + "_Hartley_contact2_isSet",
-                     [this]() -> int
-                     {
-                       odometry::LoContactWithSensor & contact =
-                           (*std::next(odometryManager_.contactsManager().contacts().begin(), 1)).second;
-                       if(contact.isSet()) { return 1; }
-                       else { return 0; }
-                     });
-
-  logger.addLogEntry(category + "_Hartley_contact1_position",
-                     [this, &ctl]() -> so::Vector3
-                     {
-                       auto & realRobot = ctl.realRobot(robot_);
-                       odometry::LoContactWithSensor & contact =
-                           (*odometryManager_.contactsManager().contacts().begin()).second;
-                       sva::PTransformd worldSurfacePose = realRobot.surfacePose(contact.surface());
-                       so::kine::Kinematics imuContactKine =
-                           updatedWorldImuKine_.getInverse()
-                           * conversions::kinematics::fromSva(worldSurfacePose, so::kine::Kinematics::Flags::pose);
-
-                       return imuContactKine.position();
-                     });
-
-  logger.addLogEntry(category + "_Hartley_contact1_orientation",
-                     [this, &ctl]() -> Eigen::Quaterniond
-                     {
-                       auto & realRobot = ctl.realRobot(robot_);
-                       odometry::LoContactWithSensor & contact =
-                           (*odometryManager_.contactsManager().contacts().begin()).second;
-                       sva::PTransformd worldSurfacePose = realRobot.surfacePose(contact.surface());
-                       so::kine::Kinematics imuContactKine =
-                           updatedWorldImuKine_.getInverse()
-                           * conversions::kinematics::fromSva(worldSurfacePose, so::kine::Kinematics::Flags::pose);
-
-                       return imuContactKine.orientation;
-                     });
-  logger.addLogEntry(category + "_Hartley_contact2_position",
-                     [this, &ctl]() -> so::Vector3
-                     {
-                       auto & realRobot = ctl.realRobot(robot_);
-                       odometry::LoContactWithSensor & contact =
-                           (*std::next(odometryManager_.contactsManager().contacts().begin(), 1)).second;
-                       sva::PTransformd worldSurfacePose = realRobot.surfacePose(contact.surface());
-                       so::kine::Kinematics imuContactKine =
-                           updatedWorldImuKine_.getInverse()
-                           * conversions::kinematics::fromSva(worldSurfacePose, so::kine::Kinematics::Flags::pose);
-
-                       return imuContactKine.position();
-                     });
-
-  logger.addLogEntry(category + "_Hartley_contact2_orientation",
-                     [this, &ctl]() -> Eigen::Quaterniond
-                     {
-                       auto & realRobot = ctl.realRobot(robot_);
-                       odometry::LoContactWithSensor & contact =
-                           (*std::next(odometryManager_.contactsManager().contacts().begin(), 1)).second;
-                       sva::PTransformd worldSurfacePose = realRobot.surfacePose(contact.surface());
-                       so::kine::Kinematics imuContactKine =
-                           updatedWorldImuKine_.getInverse()
-                           * conversions::kinematics::fromSva(worldSurfacePose, so::kine::Kinematics::Flags::pose);
-
-                       return imuContactKine.orientation;
-                     });
-
   logger.addLogEntry(category + "_debug_realWorldImuLocAngVel",
                      [this, &ctl]() -> so::Vector3
                      {