Skip to content

Commit

Permalink
wip
Browse files Browse the repository at this point in the history
  • Loading branch information
@ewfuentes
ewfuentes committed Oct 12, 2023
1 parent 8f93ed8 commit 96f62d9
Show file tree
Hide file tree
Showing 5 changed files with 99 additions and 25 deletions.
4 changes: 2 additions & 2 deletions WORKSPACE
View file
Original file line number Diff line number Diff line change
Expand Up @@ -263,10 +263,10 @@ http_archive(

http_archive(
name = "drake_lib",
url = "https://github.com/RobotLocomotion/drake/releases/download/v1.19.0/drake-20230713-jammy.tar.gz",
url = "https://github.com/RobotLocomotion/drake/releases/download/v1.21.0/drake-20230914-jammy.tar.gz",
strip_prefix="drake",
build_file_content="#",
sha256 = "c229c50c6718989a8f1f79daa474b8b6107f44a1a066d47233c9e962ddd1008f",
sha256="bc7259271c058d4ad68a898b9f2aeec44cbaa6e25a45eb0bfd57387905bdfca5",
)
load("@drake_lib//:share/drake/repo.bzl", "drake_repository")
drake_repository(name="drake", excludes=["eigen", "fmt"])
Expand Down
1 change: 1 addition & 0 deletions experimental/beacon_sim/BUILD
View file
Original file line number Diff line number Diff line change
Expand Up @@ -511,6 +511,7 @@ cc_test(
deps = [
":make_belief_updater",
":test_helpers",
"//common:drake",
"//common/math:redheffer_star",
"@com_google_googletest//:gtest_main",
]
Expand Down
2 changes: 1 addition & 1 deletion experimental/beacon_sim/belief_road_map_planner_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -225,7 +225,7 @@ TEST(ExpectedBeliefRoadMapPlannerTest, grid_road_map) {
.on_map_load_heading_uncertainty_rad = 0.1,
};

constexpr double P_BEACON = 0.5;
constexpr double P_BEACON = 0.01;
const auto &[road_map, ekf_slam, potential] = create_grid_environment(ekf_config, P_BEACON);
const Eigen::Vector2d GOAL_STATE = {10, -5};
constexpr ExpectedBeliefRoadMapOptions OPTIONS = {
Expand Down
15 changes: 15 additions & 0 deletions experimental/beacon_sim/make_belief_updater.cc
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,9 @@

#include "experimental/beacon_sim/make_belief_updater.hh"

#include <iostream>
#include <stdexcept>

#include "common/geometry/nearest_point_on_segment.hh"
#include "common/math/redheffer_star.hh"

Expand Down Expand Up @@ -295,6 +298,7 @@ std::tuple<liegroups::SE2, TypedTransform> compute_edge_belief_transform(
scattering_transform = ScatteringTransform<TransformType::INFORMATION>::Identity();
}

//int i = 0;
for (Eigen::Vector2d end_in_robot = local_from_robot.inverse() * end_state_in_local;
end_in_robot.norm() > TOL;
end_in_robot = local_from_new_robot.inverse() * end_state_in_local) {
Expand Down Expand Up @@ -331,8 +335,19 @@ std::tuple<liegroups::SE2, TypedTransform> compute_edge_belief_transform(
compute_measurement_transform(local_from_new_robot, ekf_config, ekf_estimate,
available_beacons, max_sensor_range_m, type);


// std::cout << "process transform: " << std::endl << std::get<ScatteringTransform<TransformType::COVARIANCE>>(process_transform) << std::endl;
// std::cout << "measurement transform: " << std::endl << std::get<ScatteringTransform<TransformType::COVARIANCE>>(measurement_transform) << std::endl;
//
scattering_transform = (scattering_transform * process_transform).value();
scattering_transform = (scattering_transform * measurement_transform).value();

// std::cout << "aggregate transform: " << std::endl << std::get<ScatteringTransform<TransformType::COVARIANCE>>(scattering_transform) << std::endl;

// if (i ==2) {
// throw std::runtime_error("All Done!");
// }
// i++;
}

return std::make_tuple(liegroups::SE2(local_from_new_robot.so2().log(), end_state_in_local),
Expand Down
102 changes: 80 additions & 22 deletions experimental/beacon_sim/make_belief_updater_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,25 @@
#include "experimental/beacon_sim/test_helpers.hh"
#include "gtest/gtest.h"

#include "drake/math/discrete_algebraic_riccati_equation.h"

namespace robot::experimental::beacon_sim {

TEST(MakeBeliefUpdaterTest, compute_edge_transform_no_measurements) {
template <TransformType T>
struct EnumValue {
static const TransformType value = T;
};

template <typename T>
class MakeBeliefUpdaterTest : public ::testing::Test {
static const TransformType value = T::value;
};

using MyTypes =
::testing::Types<EnumValue<TransformType::INFORMATION>, EnumValue<TransformType::COVARIANCE>>;
TYPED_TEST_SUITE(MakeBeliefUpdaterTest, MyTypes);

TYPED_TEST(MakeBeliefUpdaterTest, compute_edge_transform_no_measurements) {
// Setup
const EkfSlamConfig ekf_config{
.max_num_beacons = 1,
Expand Down Expand Up @@ -36,14 +52,14 @@ TEST(MakeBeliefUpdaterTest, compute_edge_transform_no_measurements) {
// Action
const auto edge_belief_transform = compute_edge_belief_transform(
local_from_robot, end_pos, ekf_slam.config(), ekf_slam.estimate(), {}, MAX_SENSOR_RANGE_M,
MAX_NUM_TRANSFORMS, TransformType::COVARIANCE);
MAX_NUM_TRANSFORMS, TypeParam::value);

// Verification
EXPECT_EQ(edge_belief_transform.local_from_robot.translation().x(), end_pos.x());
EXPECT_EQ(edge_belief_transform.local_from_robot.translation().y(), end_pos.y());
}

TEST(MakeBeliefUpdaterTest, compute_edge_transform_with_measurement) {
TYPED_TEST(MakeBeliefUpdaterTest, compute_edge_transform_with_measurement) {
// Setup
const EkfSlamConfig ekf_config{
.max_num_beacons = 1,
Expand Down Expand Up @@ -72,14 +88,14 @@ TEST(MakeBeliefUpdaterTest, compute_edge_transform_with_measurement) {
// Action
const auto edge_belief_transform = compute_edge_belief_transform(
local_from_robot, end_pos, ekf_slam.config(), ekf_slam.estimate(), {}, MAX_SENSOR_RANGE_M,
MAX_NUM_TRANSFORMS, TransformType::COVARIANCE);
MAX_NUM_TRANSFORMS, TypeParam::value);

// Verification
EXPECT_EQ(edge_belief_transform.local_from_robot.translation().x(), end_pos.x());
EXPECT_EQ(edge_belief_transform.local_from_robot.translation().y(), end_pos.y());
}

TEST(MakeBeliefUpdaterTest, compute_edge_transform_no_measurements_information) {
TYPED_TEST(MakeBeliefUpdaterTest, compute_edge_transform_no_measurements_information) {
// Setup
const EkfSlamConfig ekf_config{
.max_num_beacons = 1,
Expand Down Expand Up @@ -108,14 +124,14 @@ TEST(MakeBeliefUpdaterTest, compute_edge_transform_no_measurements_information)
// Action
const auto edge_belief_transform = compute_edge_belief_transform(
local_from_robot, end_pos, ekf_slam.config(), ekf_slam.estimate(), {}, MAX_SENSOR_RANGE_M,
MAX_NUM_TRANSFORMS, TransformType::INFORMATION);
MAX_NUM_TRANSFORMS, TypeParam::value);

// Verification
EXPECT_EQ(edge_belief_transform.local_from_robot.translation().x(), end_pos.x());
EXPECT_EQ(edge_belief_transform.local_from_robot.translation().y(), end_pos.y());
}

TEST(MakeBeliefUpdaterTest, compute_edge_transform_with_measurement_information) {
TYPED_TEST(MakeBeliefUpdaterTest, compute_edge_transform_with_measurement_information) {
// Setup
const EkfSlamConfig ekf_config{
.max_num_beacons = 1,
Expand All @@ -140,18 +156,17 @@ TEST(MakeBeliefUpdaterTest, compute_edge_transform_with_measurement_information)
const liegroups::SE2 local_from_robot =
liegroups::SE2::trans(road_map.points.at(START_NODE_IDX));
const Eigen::Vector2d end_pos = road_map.points.at(END_NODE_IDX);

// Action
// Action
const auto edge_belief_transform = compute_edge_belief_transform(
local_from_robot, end_pos, ekf_slam.config(), ekf_slam.estimate(), {}, MAX_SENSOR_RANGE_M,
MAX_NUM_TRANSFORMS, TransformType::INFORMATION);
MAX_NUM_TRANSFORMS, TypeParam::value);

// Verification
EXPECT_EQ(edge_belief_transform.local_from_robot.translation().x(), end_pos.x());
EXPECT_EQ(edge_belief_transform.local_from_robot.translation().y(), end_pos.y());
}

TEST(MakeBeliefUpdaterTest, stack_covariance_updates_with_observation) {
TYPED_TEST(MakeBeliefUpdaterTest, stack_covariance_updates_with_observation) {
// Setup
const EkfSlamConfig ekf_config{
.max_num_beacons = 1,
Expand All @@ -178,37 +193,65 @@ TEST(MakeBeliefUpdaterTest, stack_covariance_updates_with_observation) {

const auto &[local_from_end, transform] = compute_edge_belief_transform(
local_from_start, road_map.points.at(END_IDX), ekf_slam.config(), ekf_slam.estimate(),
{{GRID_BEACON_ID}}, MAX_SENSOR_RANGE_M, TransformType::COVARIANCE);
{{GRID_BEACON_ID}}, MAX_SENSOR_RANGE_M, TypeParam::value);
(void)local_from_end;

// Action
const TypedTransformVector stacked_transforms = [&transform = transform]() {
TypedTransformVector stacked = {transform};
for (int i = 0; i < 30; i++) {
for (int i = 0; i < 50; i++) {
stacked.push_back((stacked.back() * stacked.back()).value());
}
return stacked;
}();

// Verification
const ScatteringTransformBase info_transform = std::get<ScatteringTransform<TypeParam::value>>(transform);
// A = F, B = F^-1, Q = M, R = Q^-1
//
std::cout << "Transform: " << std::endl << info_transform << std::endl;
const Eigen::Matrix3d A = info_transform.bottomRightCorner(3, 3);
const Eigen::Matrix3d B = A;
const Eigen::Matrix3d Q = info_transform.topRightCorner(3, 3);
const Eigen::Matrix3d R = -(A.inverse() * info_transform.bottomLeftCorner(3,3) * A.inverse().transpose()).inverse();

std::cout << "A" << std::endl << A << std::endl;
std::cout << "B" << std::endl << B << std::endl;
std::cout << "Q" << std::endl << Q << std::endl;
std::cout << "R" << std::endl << R << std::endl;

std::cout << "Eigen values of Information matrix:" << std::endl;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(Q);
std::cout << solver.eigenvalues().transpose() << std::endl;

Eigen::LDLT<Eigen::Matrix3d> ldlt(Q);
std::cout << "LDLT success? " << ldlt.info() << std::endl;
std::cout << "LDLT diagonal: " << ldlt.vectorD().transpose() << std::endl;


const Eigen::MatrixXd steady_state_info = drake::math::DiscreteAlgebraicRiccatiEquation(A, B, Q, R);

std::cout << "Steady state info: " << std::endl << steady_state_info << std::endl;



std::cout << stacked_transforms.size() << std::endl;

for (int i = 0; i < static_cast<int>(stacked_transforms.size()); i++) {
for (int i = 0; i < static_cast<int>(stacked_transforms.size()); i += 4) {
std::cout << "===============" << i << " " << (1 << i) << " transforms" << std::endl;
std::cout << std::get<ScatteringTransform<TransformType::COVARIANCE>>(
stacked_transforms.at(i))
std::cout << std::get<ScatteringTransform<TypeParam::value>>(stacked_transforms.at(i))
<< std::endl;

Eigen::JacobiSVD<ScatteringTransformBase> svd(
std::get<ScatteringTransform<TransformType::COVARIANCE>>(stacked_transforms.at(i)));
std::get<ScatteringTransform<TypeParam::value>>(stacked_transforms.at(i)));
std::cout << "SVD Success? " << (svd.info() == Eigen::ComputationInfo::Success)
<< std::endl;
std::cout << "SV: " << svd.singularValues().transpose() << std::endl;
}

}

TEST(MakeBeliefUpdaterTest, stack_covariance_updates_with_no_observation) {
TYPED_TEST(MakeBeliefUpdaterTest, stack_covariance_updates_with_no_observation) {
// Setup
const EkfSlamConfig ekf_config{
.max_num_beacons = 1,
Expand All @@ -235,7 +278,7 @@ TEST(MakeBeliefUpdaterTest, stack_covariance_updates_with_no_observation) {

const auto &[local_from_end, transform] = compute_edge_belief_transform(
local_from_start, road_map.points.at(END_IDX), ekf_slam.config(), ekf_slam.estimate(), {{}},
MAX_SENSOR_RANGE_M, TransformType::INFORMATION);
MAX_SENSOR_RANGE_M, TypeParam::value);
(void)local_from_end;

// Action
Expand All @@ -253,15 +296,30 @@ TEST(MakeBeliefUpdaterTest, stack_covariance_updates_with_no_observation) {

for (int i = 0; i < static_cast<int>(stacked_transforms.size()); i++) {
std::cout << "===============" << i << " " << (1 << i) << " transforms" << std::endl;
std::cout << std::get<ScatteringTransform<TransformType::INFORMATION>>(
stacked_transforms.at(i))
std::cout << std::get<ScatteringTransform<TypeParam::value>>(stacked_transforms.at(i))
<< std::endl;

Eigen::JacobiSVD<ScatteringTransformBase> svd(
std::get<ScatteringTransform<TransformType::INFORMATION>>(stacked_transforms.at(i)));
std::get<ScatteringTransform<TypeParam::value>>(stacked_transforms.at(i)));
std::cout << "SVD Success? " << (svd.info() == Eigen::ComputationInfo::Success)
<< std::endl;
std::cout << "SV: " << svd.singularValues().transpose() << std::endl;
}

const ScatteringTransformBase info_transform = std::get<ScatteringTransform<TypeParam::value>>(transform);
// A = F, B = F^-1, Q = M, R = Q^-1
const Eigen::Matrix3d B = info_transform.bottomRightCorner(3, 3);
const Eigen::Matrix3d A = B.inverse();
const Eigen::Matrix3d Q = info_transform.topRightCorner(3, 3);
const Eigen::Matrix3d R = - A * info_transform.bottomLeftCorner(3,3) * A.transpose();

std::cout << "A" << std::endl << A << std::endl;
std::cout << "B" << std::endl << B << std::endl;
std::cout << "Q" << std::endl << Q << std::endl;
std::cout << "R" << std::endl << R << std::endl;

const Eigen::MatrixXd steady_state_info = drake::math::DiscreteAlgebraicRiccatiEquation(A, B, Q, R);

std::cout << "Steady state info: " << std::endl << steady_state_info << std::endl;
}
} // namespace robot::experimental::beacon_sim

0 comments on commit 96f62d9

Please sign in to comment.