Skip to content
Permalink

Comparing changes

Choose two branches to see what’s changed or to start a new pull request. If you need to, you can also or learn more about diff comparisons.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also . Learn more about diff comparisons here.
base repository: mockingbirdnest/Principia
Failed to load repositories. Confirm that selected base ref is valid, then try again.
Loading
base: 87d3f691cf9d
Choose a base ref
...
head repository: mockingbirdnest/Principia
Failed to load repositories. Confirm that selected head ref is valid, then try again.
Loading
compare: 29570bd3f7f1
Choose a head ref
  • 19 commits
  • 14 files changed
  • 1 contributor

Commits on Apr 28, 2020

  1. Pile-up traces.

    pleroy committed Apr 28, 2020
    Copy the full SHA
    8da5eb4 View commit details

Commits on Apr 29, 2020

  1. Unique file names.

    pleroy committed Apr 29, 2020
    Copy the full SHA
    8af18b9 View commit details

Commits on May 1, 2020

  1. Median computation.

    pleroy committed May 1, 2020
    Copy the full SHA
    11a83b3 View commit details

Commits on May 2, 2020

  1. Tuning of the PID.

    pleroy committed May 2, 2020
    Copy the full SHA
    f5943e4 View commit details
  2. Copy the full SHA
    74ca452 View commit details
  3. Support for 1.23 / s.

    pleroy committed May 2, 2020
    Copy the full SHA
    55187e4 View commit details
  4. Copy the full SHA
    5d61c20 View commit details
  5. A test for the PID.

    pleroy committed May 2, 2020
    Copy the full SHA
    add7090 View commit details
  6. Copy the full SHA
    0093f94 View commit details
  7. Cleanups.

    pleroy committed May 2, 2020
    Copy the full SHA
    29cb436 View commit details
  8. Compilation error.

    pleroy committed May 2, 2020
    Copy the full SHA
    44b0245 View commit details

Commits on May 3, 2020

  1. After egg's review.

    pleroy committed May 3, 2020
    Copy the full SHA
    6ae431a View commit details
  2. Comment.

    pleroy committed May 3, 2020
    Copy the full SHA
    8a9489e View commit details
  3. Compilation error.

    pleroy committed May 3, 2020
    Copy the full SHA
    70d24c3 View commit details
  4. A trace.

    pleroy committed May 3, 2020
    Copy the full SHA
    472deb5 View commit details
  5. Copy the full SHA
    0d5af00 View commit details
  6. Lint.

    pleroy committed May 3, 2020
    Copy the full SHA
    b224d6c View commit details
  7. Copy the full SHA
    be5fe73 View commit details
  8. Merge pull request #2550 from pleroy/2519b

    Add a PID to smoothen the apparent angular momentum given by KSP
    pleroy authored May 3, 2020

    Verified

    This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
    Copy the full SHA
    29570bd View commit details
4 changes: 4 additions & 0 deletions base/flags.hpp
Original file line number Diff line number Diff line change
@@ -8,6 +8,10 @@
namespace principia {
namespace base {

// Example of usage: create a file zfp.cfg containing:
// principia_flags {
// zfp = off
// }
class Flags {
public:
// Remove all the flags.
53 changes: 48 additions & 5 deletions ksp_plugin/pile_up.cpp
Original file line number Diff line number Diff line change
@@ -8,17 +8,20 @@
#include <memory>
#include <utility>

#include "base/flags.hpp"
#include "base/map_util.hpp"
#include "geometry/identity.hpp"
#include "ksp_plugin/integrators.hpp"
#include "ksp_plugin/part.hpp"
#include "quantities/parser.hpp"

namespace principia {
namespace ksp_plugin {
namespace internal_pile_up {

using base::check_not_null;
using base::FindOrDie;
using base::Flags;
using base::make_not_null_unique;
using geometry::AngularVelocity;
using geometry::BarycentreCalculator;
@@ -41,12 +44,38 @@ using physics::RigidMotion;
using quantities::Angle;
using quantities::AngularFrequency;
using quantities::AngularMomentum;
using quantities::Inverse;
using quantities::ParseQuantity;
using quantities::Time;
using quantities::si::Kilogram;
using quantities::si::Metre;
using quantities::si::Radian;
using quantities::si::Second;
using ::std::placeholders::_1;
using ::std::placeholders::_2;
using ::std::placeholders::_3;

// NOTE(phl): The default values were tuned on "Fubini oscillator".
constexpr double kp_default = 0.65;
constexpr Inverse<Time> ki_default = 0.03 / Second;
constexpr Time kd_default = 0.0025 * Second;

// Configuration example:
// principia_flags {
// kp = 0.65
// ki = 0.03 / s
// kd = 0.0025 s
// }
template<typename Q>
Q GetPIDFlagOr(std::string_view const name, Q const default) {
auto const values = Flags::Values(name);
if (values.empty()) {
return default;
} else {
return ParseQuantity<Q>(*values.cbegin());
}
}

PileUp::PileUp(
std::list<not_null<Part*>>&& parts,
Instant const& t,
@@ -60,7 +89,10 @@ PileUp::PileUp(
adaptive_step_parameters_(std::move(adaptive_step_parameters)),
fixed_step_parameters_(std::move(fixed_step_parameters)),
history_(make_not_null_unique<DiscreteTrajectory<Barycentric>>()),
deletion_callback_(std::move(deletion_callback)) {
deletion_callback_(std::move(deletion_callback)),
apparent_angular_momentum_controller_(GetPIDFlagOr("kp", kp_default),
GetPIDFlagOr("ki", ki_default),
GetPIDFlagOr("kd", kd_default)) {
LOG(INFO) << "Constructing pile up at " << this;
MechanicalSystem<Barycentric, NonRotatingPileUp> mechanical_system;
for (not_null<Part*> const part : parts_) {
@@ -356,7 +388,10 @@ PileUp::PileUp(
history_(std::move(history)),
psychohistory_(psychohistory),
angular_momentum_(angular_momentum),
deletion_callback_(std::move(deletion_callback)) {}
deletion_callback_(std::move(deletion_callback)),
apparent_angular_momentum_controller_(GetPIDFlagOr("kp", kp_default),
GetPIDFlagOr("ki", ki_default),
GetPIDFlagOr("kd", kd_default)) {}

void PileUp::MakeEulerSolver(
InertiaTensor<NonRotatingPileUp> const& inertia_tensor,
@@ -381,6 +416,7 @@ void PileUp::MakeEulerSolver(

void PileUp::DeformPileUpIfNeeded(Instant const& t) {
if (apparent_part_rigid_motion_.empty()) {
apparent_angular_momentum_controller_.Clear();
Bivector<AngularMomentum, PileUpPrincipalAxes> const angular_momentum =
euler_solver_->AngularMomentumAt(t);
Rotation<PileUpPrincipalAxes, NonRotatingPileUp> const attitude =
@@ -415,15 +451,22 @@ void PileUp::DeformPileUpIfNeeded(Instant const& t) {
CHECK(Contains(apparent_part_rigid_motion_, part));
}

using ApparentPileUp = Frame<enum class ApparentPileUpTag, NonRotating>;
auto const angular_momentum_in_apparent_pile_up =
Identity<NonRotatingPileUp, ApparentPileUp>()(angular_momentum_);

MechanicalSystem<ApparentBubble, ApparentPileUp> apparent_system;
for (auto const& [part, apparent_part_rigid_motion] :
apparent_part_rigid_motion_) {
apparent_system.AddRigidBody(
apparent_part_rigid_motion, part->mass(), part->inertia_tensor());
}
auto const apparent_centre_of_mass = apparent_system.centre_of_mass();
auto const apparent_angular_momentum = apparent_system.AngularMomentum();
auto const apparent_angular_momentum =
angular_momentum_in_apparent_pile_up -
apparent_angular_momentum_controller_.ComputeControlVariable(
apparent_system.AngularMomentum(),
angular_momentum_in_apparent_pile_up,
t);
// Note that the inertia tensor is with respect to the centre of mass, so it
// is unaffected by the apparent-bubble-to-pile-up correction, which is rigid
// and involves no change in axes.
@@ -437,7 +480,7 @@ void PileUp::DeformPileUpIfNeeded(Instant const& t) {
// actual_correction_axis.coordinates()|.
auto const apparent_correction_axis = NormalizeOrZero(Commutator(
apparent_angular_momentum,
Identity<NonRotatingPileUp, ApparentPileUp>()(angular_momentum_)));
angular_momentum_in_apparent_pile_up));
auto const actual_correction_axis = NormalizeOrZero(Commutator(
Identity<ApparentPileUp, NonRotatingPileUp>()(apparent_angular_momentum),
angular_momentum_));
15 changes: 15 additions & 0 deletions ksp_plugin/pile_up.hpp
Original file line number Diff line number Diff line change
@@ -16,6 +16,7 @@
#include "geometry/grassmann.hpp"
#include "geometry/named_quantities.hpp"
#include "integrators/integrators.hpp"
#include "numerics/pid.hpp"
#include "physics/discrete_trajectory.hpp"
#include "physics/ephemeris.hpp"
#include "physics/euler_solver.hpp"
@@ -46,6 +47,7 @@ using geometry::NonRotating;
using geometry::RigidTransformation;
using geometry::Vector;
using integrators::Integrator;
using numerics::PID;
using physics::DiscreteTrajectory;
using physics::DegreesOfFreedom;
using physics::Ephemeris;
@@ -143,6 +145,12 @@ class PileUp {
using AppendToPartTrajectory =
void (Part::*)(Instant const&, DegreesOfFreedom<Barycentric> const&);

// The axes are those of Barycentric. The origin is the centre of mass of the
// pile up. This frame is distinguished from NonRotatingPileUp in that it is
// used to hold uncorrected (apparent) coordinates given by the game, before
// the enforcement of conservation laws; see also ApparentBubble.
using ApparentPileUp = Frame<enum class ApparentPileUpTag, NonRotating>;

// For deserialization.
PileUp(
std::list<not_null<Part*>>&& parts,
@@ -236,6 +244,13 @@ class PileUp {
// Called in the destructor.
std::function<void()> deletion_callback_;

// A PID used to smoothen the value of the apparent angular momentum obtained
// from KSP.
PID<Bivector<AngularMomentum, ApparentPileUp>,
Bivector<AngularMomentum, ApparentPileUp>,
/*horizon=*/25,
/*finite_difference_order=*/5> apparent_angular_momentum_controller_;

friend class TestablePileUp;
};

7 changes: 6 additions & 1 deletion mathematica/mathematica.hpp
Original file line number Diff line number Diff line change
@@ -180,7 +180,10 @@ std::string Escape(std::string const& str);
class Logger final {
public:
// Creates a logger object that will, at destruction, write to the given file.
explicit Logger(std::filesystem::path const& path);
// If make_unique is true, a unique id is inserted before the file extension
// to identify different loggers.
Logger(std::filesystem::path const& path,
bool make_unique = false);
~Logger();

// Appends an element to the list of values for the variable |name|. The
@@ -193,6 +196,8 @@ class Logger final {
private:
OFStream file_;
std::map<std::string, std::vector<std::string>> names_and_values_;

static std::atomic_uint64_t id_;
};

} // namespace internal_mathematica
14 changes: 13 additions & 1 deletion mathematica/mathematica_body.hpp
Original file line number Diff line number Diff line change
@@ -284,7 +284,17 @@ inline std::string Escape(std::string const& str) {
return result;
}

inline Logger::Logger(std::filesystem::path const& path) : file_(path) {}
inline Logger::Logger(std::filesystem::path const& path, bool const make_unique)
: file_([make_unique, &path]() {
if (make_unique) {
std::filesystem::path filename = path.stem();
filename += std::to_string(id_++);
filename += path.extension();
return path.parent_path() / filename;
} else {
return path;
}
}()) {}

inline Logger::~Logger() {
for (auto const& [name, values] : names_and_values_) {
@@ -297,6 +307,8 @@ void Logger::Append(std::string const& name, Args... args) {
names_and_values_[name].push_back(ToMathematica(args...));
}

inline std::atomic_uint64_t Logger::id_ = 0;

} // namespace internal_mathematica
} // namespace mathematica
} // namespace principia
4 changes: 2 additions & 2 deletions mathematica/mathematica_test.cpp
Original file line number Diff line number Diff line change
@@ -359,7 +359,7 @@ TEST_F(MathematicaTest, ExpressIn) {

TEST_F(MathematicaTest, Logger) {
{
Logger logger(TEMP_DIR / "mathematica_test.wl");
Logger logger(TEMP_DIR / "mathematica_test.wl", /*make_unique=*/true);
logger.Append("a", std::vector{1, 2, 3});
logger.Append("b", 4 * Metre / Second);
logger.Append("a", F::origin);
@@ -391,7 +391,7 @@ TEST_F(MathematicaTest, Logger) {
"\" m s^-1\"]]];\n",
(
std::stringstream{}
<< std::ifstream(TEMP_DIR / "mathematica_test.wl").rdbuf())
<< std::ifstream(TEMP_DIR / "mathematica_test0.wl").rdbuf())
.str());
}

3 changes: 3 additions & 0 deletions numerics/numerics.vcxproj
Original file line number Diff line number Diff line change
@@ -39,6 +39,8 @@
<ClInclude Include="newhall.hpp" />
<ClInclude Include="newhall.mathematica.h" />
<ClInclude Include="newhall_body.hpp" />
<ClInclude Include="pid.hpp" />
<ClInclude Include="pid_body.hpp" />
<ClInclude Include="polynomial.hpp" />
<ClInclude Include="polynomial_body.hpp" />
<ClInclude Include="polynomial_evaluators.hpp" />
@@ -68,6 +70,7 @@
<ClCompile Include="legendre_test.cpp" />
<ClCompile Include="max_abs_normalized_associated_legendre_functions_test.cc" />
<ClCompile Include="newhall_test.cpp" />
<ClCompile Include="pid_test.cpp" />
<ClCompile Include="polynomial_evaluators_test.cpp" />
<ClCompile Include="polynomial_test.cpp" />
<ClCompile Include="root_finders_test.cpp" />
9 changes: 9 additions & 0 deletions numerics/numerics.vcxproj.filters
Original file line number Diff line number Diff line change
@@ -119,6 +119,12 @@
<ClInclude Include="elliptic_functions.hpp">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="pid.hpp">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="pid_body.hpp">
<Filter>Source Files</Filter>
</ClInclude>
</ItemGroup>
<ItemGroup>
<ClCompile Include="чебышёв_series_test.cpp">
@@ -184,6 +190,9 @@
<ClCompile Include="elliptic_integrals_test.cpp">
<Filter>Test Files</Filter>
</ClCompile>
<ClCompile Include="pid_test.cpp">
<Filter>Test Files</Filter>
</ClCompile>
</ItemGroup>
<ItemGroup>
<Text Include="xgscd.proto.txt">
70 changes: 70 additions & 0 deletions numerics/pid.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,70 @@

#pragma once

#include <deque>
#include <optional>

#include "geometry/named_quantities.hpp"
#include "quantities/named_quantities.hpp"
#include "quantities/quantities.hpp"

namespace principia {
namespace numerics {
namespace internal_pid {

using geometry::Instant;
using geometry::Normed;
using quantities::Difference;
using quantities::Product;
using quantities::Quotient;
using quantities::Time;
using quantities::Variation;

template<typename Input,
typename Output,
int horizon,
int finite_difference_order>
class PID {
public:
static_assert(finite_difference_order <= horizon);

using Kp = Quotient<typename Normed<Output>::NormType,
typename Normed<Difference<Input>>::NormType>;
using Ki = Quotient<typename Normed<Output>::NormType,
typename Normed<Product<Difference<Input>, Time>>::
NormType>;
using Kd = Quotient<typename Normed<Output>::NormType,
typename Normed<Difference<Variation<Input>>>::NormType>;

PID(Kp const& kp, Ki const& ki, Kd const& kd);

// Clears the state of the PID.
void Clear();

// Adds the error between the process variable and the set-point to the state
// of the PID and returns the control variable.
Output ComputeControlVariable(Input const& process_variable,
Input const& set_point,
Instant const& t);

private:
Kp const kp_;
Ki const ki_;
Kd const kd_;

// The PID is not prepared to receive inputs that are not equally spaced.
std::optional<Time> previous_Δt_;
std::optional<Instant> previous_t_;

// The front element is the oldest.
std::deque<Difference<Input>> errors_;
};

} // namespace internal_pid

using internal_pid::PID;

} // namespace numerics
} // namespace principia

#include "numerics/pid_body.hpp"
Loading