diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index c66c8916d..683617a19 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -21,6 +21,8 @@ #include #include #include +#include +#include #include #include #include @@ -204,11 +206,11 @@ class SingleLeg { auto stance = boost::make_shared( keys.at(0), keys.at(1), Pose2(0, 0, 0), stance_model), lift = boost::make_shared( - keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model), + keys.at(0), keys.at(1), Pose2(0, -1, 0), swing_model), land = boost::make_shared( - keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model), + keys.at(0), keys.at(1), Pose2(0, 1, 0), swing_model), swing = boost::make_shared( - keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model); + keys.at(0), keys.at(1), Pose2(1, 0, 0), swing_model); // 00 - swing, 01 - land, 10 - toe-off, 11 - stance std::vector> components = {swing, land, lift, stance}; @@ -237,7 +239,6 @@ class SingleLeg { HybridBayesNet::shared_ptr hybridBayesNet = linearizedFactorGraph_.eliminateSequential(hybridOrdering); - hybridBayesNet->print(); HybridValues delta = hybridBayesNet->optimize(); return delta; } @@ -258,10 +259,172 @@ TEST(Estimation, LeggedRobot) { // initial.print(); HybridValues delta = robot.optimize(); - delta.print(); + // delta.print(); initial.retract(delta.continuous()).print("\n\n========="); + std::cout << "\n\n\n" << std::endl; } + +/// A robot with a single leg - non-hybrid version. +class SL { + NonlinearFactorGraph nonlinearFactorGraph_; + GaussianFactorGraph linearizedFactorGraph_; + GaussianBayesNet bayesNet_; + Values linearizationPoint_; + + public: + /** + * @brief Construct a new Single Leg object. + * + * @param K The number of discrete timesteps + * @param pims std::vector of preintegrated IMU measurements. + * @param fk std::vector of forward kinematic measurements for the leg. + */ + SL(size_t K, const std::vector& pims, const std::vector& fk, + const std::vector& contacts) { + ////// Create hybrid factor graph. + + auto measurement_noise = noiseModel::Isotropic::Sigma(3, 1.0); + + // Add prior on the first pose + nonlinearFactorGraph_.emplace_shared>( + X(0), Pose2(0, 2, 0), measurement_noise); + + // Add measurement factors. + // These are the preintegrated IMU measurements of the base. + for (size_t k = 0; k < K - 1; k++) { + nonlinearFactorGraph_.emplace_shared>( + X(k), X(k + 1), pims.at(k), measurement_noise); + } + + // Forward kinematics from base X to foot L + auto fk_noise = noiseModel::Isotropic::Sigma(3, 1.0); + for (size_t k = 0; k < K; k++) { + nonlinearFactorGraph_.emplace_shared>( + X(k), L(k), fk.at(k), fk_noise); + } + + // 2 noise models where moving has a higher covariance. + auto stance_model = noiseModel::Isotropic::Sigma(3, 1e-4); + auto swing_model = noiseModel::Isotropic::Sigma(3, 1e8); + + // Add "contact models" for the foot. + // The idea is that the robot's leg has a tight covariance for stance and + // loose covariance for swing. + using ContactFactor = BetweenFactor; + + for (size_t k = 0; k < K - 1; k++) { + KeyVector keys = {L(k), L(k + 1)}; + ContactFactor::shared_ptr factor; + if (contacts[k] && contacts[k + 1]) { + // stance + std::cout << "stance 11" << std::endl; + factor = boost::make_shared( + keys.at(0), keys.at(1), Pose2(0, 0, 0), stance_model); + } else if (contacts[k] && !contacts[k + 1]) { + // toe-off + std::cout << "toe-off 10" << std::endl; + factor = boost::make_shared(keys.at(0), keys.at(1), + Pose2(0, 0, 0), swing_model); + } else if (!contacts[k] && contacts[k + 1]) { + // land + std::cout << "land 01" << std::endl; + factor = boost::make_shared(keys.at(0), keys.at(1), + Pose2(0, 0, 0), swing_model); + } else if (!contacts[k] && !contacts[k + 1]) { + // swing + std::cout << "swing 00" << std::endl; + factor = boost::make_shared(keys.at(0), keys.at(1), + Pose2(0, 0, 0), swing_model); + } + + nonlinearFactorGraph_.push_back(factor); + } + + // Create the linearization point. + for (size_t k = 0; k < K; k++) { + linearizationPoint_.insert(X(k), Pose2(k, 2, 0)); + linearizationPoint_.insert(L(k), Pose2(0, 0, 0)); + } + + linearizedFactorGraph_ = + *nonlinearFactorGraph_.linearize(linearizationPoint_); + } + + void print() const { + nonlinearFactorGraph_.print(); + linearizationPoint_.print(); + linearizedFactorGraph_.print(); + } + + VectorValues optimize() { + bayesNet_ = *linearizedFactorGraph_.eliminateSequential(); + + // bayesNet->print(); + VectorValues delta = bayesNet_.optimize(); + return delta; + } + + Values linearizationPoint() const { return linearizationPoint_; } + NonlinearFactorGraph nonlinearFactorGraph() const { + return nonlinearFactorGraph_; + } + GaussianFactorGraph linearizedFactorGraph() const { + return linearizedFactorGraph_; + } + GaussianBayesNet bayesNet() const { return bayesNet_; } +}; + +/* ****************************************************************************/ +TEST(Estimation, LR) { + std::vector pims = {Pose2(1, 0, 0)}; + // Leg is in stance throughout + // std::vector fk = {Pose2(0, -2, 0), Pose2(-1, -2, 0)}; + // Leg is in swing + // std::vector fk = {Pose2(0, -1, 0), Pose2(0, -1, 0)}; + // Leg is in toe-off + // std::vector fk = {Pose2(0, -2, 0), Pose2(0, -1, 0)}; + // Leg is in land + std::vector fk = {Pose2(0, -1, 0), Pose2(0, -2, 0)}; + + vector contacts; + contacts = {1, 1}; + SL robot11(2, pims, fk, contacts); + VectorValues delta = robot11.optimize(); + // robot11.nonlinearFactorGraph().print(); + std::cout << "Error with optimized delta: " << robot11.bayesNet().error(delta) + << std::endl; + robot11.linearizationPoint().retract(delta).print(); + std::cout << "\n===========================\n\n" << std::endl; + + contacts = {1, 0}; + SL robot10(2, pims, fk, contacts); + delta = robot10.optimize(); + // robot10.nonlinearFactorGraph().print(); + std::cout << "Error with optimized delta: " << robot10.bayesNet().error(delta) + << std::endl; + robot10.linearizationPoint().retract(delta).print(); + std::cout << "\n===========================\n\n" << std::endl; + + contacts = {0, 1}; + SL robot01(2, pims, fk, contacts); + delta = robot01.optimize(); + // robot01.nonlinearFactorGraph().print(); + std::cout << "Error with optimized delta: " << robot01.bayesNet().error(delta) + << std::endl; + robot01.linearizationPoint().retract(delta).print(); + std::cout << "\n===========================\n\n" << std::endl; + + contacts = {0, 0}; + SL robot00(2, pims, fk, contacts); + delta = robot00.optimize(); + // robot00.nonlinearFactorGraph().print(); + std::cout << "Error with optimized delta: " << robot00.bayesNet().error(delta) + << std::endl; + robot00.linearizationPoint().retract(delta).print(); + std::cout << "\n===========================\n\n" << std::endl; +} + /* ************************************************************************* */ int main() { TestResult tr;