12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

single leg robot test

release/4.3a0
Varun Agrawal 2022-09-04 01:59:18 -04:00
parent b2ca7476d6
commit 8f94f726a9
1 changed files with 46 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
gtsam/hybrid/tests/testHybridEstimation.cpp
View File

@ -15,7 +15,6 @@
* @author Varun Agrawal * @author Varun Agrawal
*/ */
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/hybrid/HybridBayesNet.h> #include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridBayesTree.h> #include <gtsam/hybrid/HybridBayesTree.h>
@ -40,7 +39,6 @@ using symbol_shorthand::M;
using symbol_shorthand::X; using symbol_shorthand::X;
class Robot { class Robot {
size_t K_;
DiscreteKeys modes_; DiscreteKeys modes_;
HybridNonlinearFactorGraph nonlinearFactorGraph_; HybridNonlinearFactorGraph nonlinearFactorGraph_;
HybridGaussianFactorGraph linearizedFactorGraph_; HybridGaussianFactorGraph linearizedFactorGraph_;
@ -118,12 +116,10 @@ TEST(Estimation, StillRobot) {
vector<double> measurements = {0, 0}; vector<double> measurements = {0, 0};
Robot robot(K, measurements); Robot robot(K, measurements);
// robot.print();
HybridValues delta = robot.optimize(); HybridValues delta = robot.optimize();
delta.continuous().print("delta update:"); delta.continuous().print("delta update:");
if (delta.discrete()[M(0)] == 0) { if (delta.discrete()[M(0)] == 0) {
std::cout << "The robot is stationary!" << std::endl; std::cout << "The robot is stationary!" << std::endl;
} else { } else {
@ -132,17 +128,16 @@ TEST(Estimation, StillRobot) {
EXPECT_LONGS_EQUAL(0, delta.discrete()[M(0)]); EXPECT_LONGS_EQUAL(0, delta.discrete()[M(0)]);
} }
/* ****************************************************************************/
TEST(Estimation, MovingRobot) { TEST(Estimation, MovingRobot) {
size_t K = 2; size_t K = 2;
vector<double> measurements = {0, 2}; vector<double> measurements = {0, 2};
Robot robot(K, measurements); Robot robot(K, measurements);
// robot.print();
HybridValues delta = robot.optimize(); HybridValues delta = robot.optimize();
delta.continuous().print("delta update:"); delta.continuous().print("delta update:");
if (delta.discrete()[M(0)] == 0) { if (delta.discrete()[M(0)] == 0) {
std::cout << "The robot is stationary!" << std::endl; std::cout << "The robot is stationary!" << std::endl;
} else { } else {
@ -153,7 +148,6 @@ TEST(Estimation, MovingRobot) {
/// A robot with a single leg. /// A robot with a single leg.
class SingleLeg { class SingleLeg {
size_t K_;
DiscreteKeys modes_; DiscreteKeys modes_;
HybridNonlinearFactorGraph nonlinearFactorGraph_; HybridNonlinearFactorGraph nonlinearFactorGraph_;
HybridGaussianFactorGraph linearizedFactorGraph_; HybridGaussianFactorGraph linearizedFactorGraph_;
@ -165,10 +159,9 @@ class SingleLeg {
* *
* @param K The number of discrete timesteps * @param K The number of discrete timesteps
* @param pims std::vector of preintegrated IMU measurements. * @param pims std::vector of preintegrated IMU measurements.
* @param contacts std::vector denoting whether the leg was in contact at each * @param fk std::vector of forward kinematic measurements for the leg.
* timestep.
*/ */
SingleLeg(size_t K, std::vector<Pose2> pims, std::vector<uint64_t> contacts) { SingleLeg(size_t K, std::vector<Pose2> pims, std::vector<Pose2> fk) {
// Create DiscreteKeys for binary K modes // Create DiscreteKeys for binary K modes
for (size_t k = 0; k < K; k++) { for (size_t k = 0; k < K; k++) {
modes_.emplace_back(M(k), 2); modes_.emplace_back(M(k), 2);
@ -176,10 +169,15 @@ class SingleLeg {
////// Create hybrid factor graph. ////// Create hybrid factor graph.
auto measurement_noise = noiseModel::Isotropic::Sigma(3, 1.0);
// Add prior on the first pose
nonlinearFactorGraph_.emplace_nonlinear<PriorFactor<Pose2>>(
X(0), Pose2(0, 2, 0), measurement_noise);
// Add measurement factors. // Add measurement factors.
// These are the preintegrated IMU measurements of the base. // 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 - 1; k++) {
for (size_t k = 0; k < K; k++) {
nonlinearFactorGraph_.emplace_nonlinear<BetweenFactor<Pose2>>( nonlinearFactorGraph_.emplace_nonlinear<BetweenFactor<Pose2>>(
X(k), X(k + 1), pims.at(k), measurement_noise); X(k), X(k + 1), pims.at(k), measurement_noise);
} }
@ -187,35 +185,30 @@ class SingleLeg {
// Forward kinematics from base X to foot L // Forward kinematics from base X to foot L
auto fk_noise = noiseModel::Isotropic::Sigma(3, 1.0); auto fk_noise = noiseModel::Isotropic::Sigma(3, 1.0);
for (size_t k = 0; k < K; k++) { for (size_t k = 0; k < K; k++) {
if (contacts.at(k) == 1) {
nonlinearFactorGraph_.emplace_nonlinear<BetweenFactor<Pose2>>( nonlinearFactorGraph_.emplace_nonlinear<BetweenFactor<Pose2>>(
X(k), L(k), Pose2(), fk_noise) X(k), L(k), fk.at(k), fk_noise);
} else {
nonlinearFactorGraph_.emplace_nonlinear<BetweenFactor<Pose2>>(
X(k), L(k), Pose2(), fk_noise)
}
} }
// 2 noise models where moving has a higher covariance. // 2 noise models where moving has a higher covariance.
auto stance_model = noiseModel::Isotropic::Sigma(1, 1e-2); auto stance_model = noiseModel::Isotropic::Sigma(3, 1e-2);
auto swing_model = noiseModel::Isotropic::Sigma(1, 1e2); auto swing_model = noiseModel::Isotropic::Sigma(3, 1e2);
// Add "contact models" for the foot. // Add "contact models" for the foot.
// The idea is that the robot's leg has a tight covariance for stance and // The idea is that the robot's leg has a tight covariance for stance and
// loose covariance for swing. // loose covariance for swing.
using ContactFactor = BetweenFactor<double>; using ContactFactor = BetweenFactor<Pose2>;
for (size_t k = 0; k < K; k++) { for (size_t k = 0; k < K - 1; k++) {
KeyVector keys = {L(k), L(k + 1)}; KeyVector keys = {L(k), L(k + 1)};
DiscreteKeys dkeys{modes_[k], modes_[k + 1]}; DiscreteKeys dkeys{modes_[k], modes_[k + 1]};
auto stance = boost::make_shared<ContactFactor>(keys.at(0), keys.at(1), auto stance = boost::make_shared<ContactFactor>(
0.0, stance_model), keys.at(0), keys.at(1), Pose2(0, 0, 0), stance_model),
lift = boost::make_shared<ContactFactor>(keys.at(0), keys.at(1), 0.0, lift = boost::make_shared<ContactFactor>(
swing_model), keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model),
land = boost::make_shared<ContactFactor>(keys.at(0), keys.at(1), 0.0, land = boost::make_shared<ContactFactor>(
swing_model), keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model),
swing = boost::make_shared<ContactFactor>(keys.at(0), keys.at(1), swing = boost::make_shared<ContactFactor>(
0.0, swing_model); keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model);
// 00 - swing, 01 - land, 10 - toe-off, 11 - stance // 00 - swing, 01 - land, 10 - toe-off, 11 - stance
std::vector<boost::shared_ptr<ContactFactor>> components = {swing, land, std::vector<boost::shared_ptr<ContactFactor>> components = {swing, land,
lift, stance}; lift, stance};
@ -225,7 +218,8 @@ class SingleLeg {
// Create the linearization point. // Create the linearization point.
for (size_t k = 0; k < K; k++) { for (size_t k = 0; k < K; k++) {
linearizationPoint_.insert<double>(X(k), static_cast<double>(k + 1)); linearizationPoint_.insert<Pose2>(X(k), Pose2(k, 2, 0));
linearizationPoint_.insert<Pose2>(L(k), Pose2(0, 0, 0));
} }
linearizedFactorGraph_ = linearizedFactorGraph_ =
@ -243,11 +237,31 @@ class SingleLeg {
HybridBayesNet::shared_ptr hybridBayesNet = HybridBayesNet::shared_ptr hybridBayesNet =
linearizedFactorGraph_.eliminateSequential(hybridOrdering); linearizedFactorGraph_.eliminateSequential(hybridOrdering);
hybridBayesNet->print();
HybridValues delta = hybridBayesNet->optimize(); HybridValues delta = hybridBayesNet->optimize();
return delta; return delta;
} }
Values linearizationPoint() const { return linearizationPoint_; }
}; };
/* ****************************************************************************/
TEST(Estimation, LeggedRobot) {
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)};
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() { int main() {
TestResult tr; TestResult tr;