add more tests to show scheme doesn't work
Varun Agrawal
parent
8f94f726a9
commit
b5eaaabcb5
|
|
@ -21,6 +21,8 @@
|
|||
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
|
||||
#include <gtsam/hybrid/MixtureFactor.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
|
@ -204,11 +206,11 @@ class SingleLeg {
|
|||
auto stance = boost::make_shared<ContactFactor>(
|
||||
keys.at(0), keys.at(1), Pose2(0, 0, 0), stance_model),
|
||||
lift = boost::make_shared<ContactFactor>(
|
||||
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<ContactFactor>(
|
||||
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<ContactFactor>(
|
||||
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<boost::shared_ptr<ContactFactor>> 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<Pose2>& pims, const std::vector<Pose2>& fk,
|
||||
const std::vector<bool>& contacts) {
|
||||
////// Create hybrid factor graph.
|
||||
|
||||
auto measurement_noise = noiseModel::Isotropic::Sigma(3, 1.0);
|
||||
|
||||
// Add prior on the first pose
|
||||
nonlinearFactorGraph_.emplace_shared<PriorFactor<Pose2>>(
|
||||
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<BetweenFactor<Pose2>>(
|
||||
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<BetweenFactor<Pose2>>(
|
||||
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<Pose2>;
|
||||
|
||||
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<ContactFactor>(
|
||||
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<ContactFactor>(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<ContactFactor>(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<ContactFactor>(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<Pose2>(X(k), Pose2(k, 2, 0));
|
||||
linearizationPoint_.insert<Pose2>(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<Pose2> pims = {Pose2(1, 0, 0)};
|
||||
// Leg is in stance throughout
|
||||
// std::vector<Pose2> fk = {Pose2(0, -2, 0), Pose2(-1, -2, 0)};
|
||||
// Leg is in swing
|
||||
// std::vector<Pose2> fk = {Pose2(0, -1, 0), Pose2(0, -1, 0)};
|
||||
// Leg is in toe-off
|
||||
// std::vector<Pose2> fk = {Pose2(0, -2, 0), Pose2(0, -1, 0)};
|
||||
// Leg is in land
|
||||
std::vector<Pose2> fk = {Pose2(0, -1, 0), Pose2(0, -2, 0)};
|
||||
|
||||
vector<bool> 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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue