From 8f94f726a96add1516a57abbe41533ee5f0485c7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 4 Sep 2022 01:59:18 -0400 Subject: [PATCH] single leg robot test --- gtsam/hybrid/tests/testHybridEstimation.cpp | 78 ++++++++++++--------- 1 file changed, 46 insertions(+), 32 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 086c7cb7d..c66c8916d 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -15,7 +15,6 @@ * @author Varun Agrawal */ -#include #include #include #include @@ -40,7 +39,6 @@ using symbol_shorthand::M; using symbol_shorthand::X; class Robot { - size_t K_; DiscreteKeys modes_; HybridNonlinearFactorGraph nonlinearFactorGraph_; HybridGaussianFactorGraph linearizedFactorGraph_; @@ -118,12 +116,10 @@ TEST(Estimation, StillRobot) { vector measurements = {0, 0}; Robot robot(K, measurements); - // robot.print(); HybridValues delta = robot.optimize(); delta.continuous().print("delta update:"); - if (delta.discrete()[M(0)] == 0) { std::cout << "The robot is stationary!" << std::endl; } else { @@ -132,17 +128,16 @@ TEST(Estimation, StillRobot) { EXPECT_LONGS_EQUAL(0, delta.discrete()[M(0)]); } +/* ****************************************************************************/ TEST(Estimation, MovingRobot) { size_t K = 2; vector measurements = {0, 2}; Robot robot(K, measurements); - // robot.print(); HybridValues delta = robot.optimize(); delta.continuous().print("delta update:"); - if (delta.discrete()[M(0)] == 0) { std::cout << "The robot is stationary!" << std::endl; } else { @@ -153,7 +148,6 @@ TEST(Estimation, MovingRobot) { /// A robot with a single leg. class SingleLeg { - size_t K_; DiscreteKeys modes_; HybridNonlinearFactorGraph nonlinearFactorGraph_; HybridGaussianFactorGraph linearizedFactorGraph_; @@ -165,10 +159,9 @@ class SingleLeg { * * @param K The number of discrete timesteps * @param pims std::vector of preintegrated IMU measurements. - * @param contacts std::vector denoting whether the leg was in contact at each - * timestep. + * @param fk std::vector of forward kinematic measurements for the leg. */ - SingleLeg(size_t K, std::vector pims, std::vector contacts) { + SingleLeg(size_t K, std::vector pims, std::vector fk) { // Create DiscreteKeys for binary K modes for (size_t k = 0; k < K; k++) { modes_.emplace_back(M(k), 2); @@ -176,10 +169,15 @@ class SingleLeg { ////// Create hybrid factor graph. + auto measurement_noise = noiseModel::Isotropic::Sigma(3, 1.0); + + // Add prior on the first pose + nonlinearFactorGraph_.emplace_nonlinear>( + X(0), Pose2(0, 2, 0), measurement_noise); + // Add measurement factors. // These are the preintegrated IMU measurements of the base. - auto measurement_noise = noiseModel::Isotropic::Sigma(3, 1.0); - for (size_t k = 0; k < K; k++) { + for (size_t k = 0; k < K - 1; k++) { nonlinearFactorGraph_.emplace_nonlinear>( X(k), X(k + 1), pims.at(k), measurement_noise); } @@ -187,35 +185,30 @@ class SingleLeg { // 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++) { - if (contacts.at(k) == 1) { - nonlinearFactorGraph_.emplace_nonlinear>( - X(k), L(k), Pose2(), fk_noise) - } else { - nonlinearFactorGraph_.emplace_nonlinear>( - X(k), L(k), Pose2(), fk_noise) - } + nonlinearFactorGraph_.emplace_nonlinear>( + X(k), L(k), fk.at(k), fk_noise); } // 2 noise models where moving has a higher covariance. - auto stance_model = noiseModel::Isotropic::Sigma(1, 1e-2); - auto swing_model = noiseModel::Isotropic::Sigma(1, 1e2); + auto stance_model = noiseModel::Isotropic::Sigma(3, 1e-2); + auto swing_model = noiseModel::Isotropic::Sigma(3, 1e2); // 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; + using ContactFactor = BetweenFactor; - for (size_t k = 0; k < K; k++) { + for (size_t k = 0; k < K - 1; k++) { KeyVector keys = {L(k), L(k + 1)}; DiscreteKeys dkeys{modes_[k], modes_[k + 1]}; - auto stance = boost::make_shared(keys.at(0), keys.at(1), - 0.0, stance_model), - lift = boost::make_shared(keys.at(0), keys.at(1), 0.0, - swing_model), - land = boost::make_shared(keys.at(0), keys.at(1), 0.0, - swing_model), - swing = boost::make_shared(keys.at(0), keys.at(1), - 0.0, swing_model); + 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), + land = boost::make_shared( + keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model), + swing = boost::make_shared( + keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model); // 00 - swing, 01 - land, 10 - toe-off, 11 - stance std::vector> components = {swing, land, lift, stance}; @@ -225,7 +218,8 @@ class SingleLeg { // Create the linearization point. for (size_t k = 0; k < K; k++) { - linearizationPoint_.insert(X(k), static_cast(k + 1)); + linearizationPoint_.insert(X(k), Pose2(k, 2, 0)); + linearizationPoint_.insert(L(k), Pose2(0, 0, 0)); } linearizedFactorGraph_ = @@ -243,11 +237,31 @@ class SingleLeg { HybridBayesNet::shared_ptr hybridBayesNet = linearizedFactorGraph_.eliminateSequential(hybridOrdering); + hybridBayesNet->print(); HybridValues delta = hybridBayesNet->optimize(); return delta; } + + Values linearizationPoint() const { return linearizationPoint_; } }; +/* ****************************************************************************/ +TEST(Estimation, LeggedRobot) { + std::vector pims = {Pose2(1, 0, 0)}; + // Leg is in stance throughout + std::vector fk = {Pose2(0, -2, 0), Pose2(-1, -2, 0)}; + SingleLeg robot(2, pims, fk); + + std::cout << "\n\n\n" << std::endl; + // robot.print(); + Values initial = robot.linearizationPoint(); + // initial.print(); + + HybridValues delta = robot.optimize(); + delta.print(); + + initial.retract(delta.continuous()).print("\n\n========="); +} /* ************************************************************************* */ int main() { TestResult tr;