Skip to content

Commit

Permalink
Check input dependency
Browse files Browse the repository at this point in the history
  • Loading branch information
@wei-chen-li
wei-chen-li committed Feb 21, 2025
1 parent bccbb16 commit c4cba67
Show file tree
Hide file tree
Showing 2 changed files with 51 additions and 36 deletions.
70 changes: 48 additions & 22 deletions systems/analysis/discrete_time_approximation.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

#include "drake/systems/analysis/simulator.h"
#include "drake/systems/analysis/simulator_config_functions.h"
#include "drake/systems/framework/diagram_output_port.h"
#include "drake/systems/framework/leaf_system.h"

namespace drake {
Expand Down Expand Up @@ -104,8 +105,14 @@ class DiscreteTimeSystem final : public LeafSystem<T> {
// safely modify it without a (non-thread-safe) mutable member.
continuous_context_cache_entry_ = &this->DeclareCacheEntry(
"continuous system context", *continuous_context_model_value_,
&DiscreteTimeSystem<T>::CopyDiscreteContextToContinuousContext,
&DiscreteTimeSystem<T>::CopyAllSources,
{SystemBase::all_sources_ticket()});
// Another cache entry for the continuous system context, but this one does
// not copy the input port values.
continuous_context2_cache_entry_ = &this->DeclareCacheEntry(
"continuous system context2", *continuous_context_model_value_,
&DiscreteTimeSystem<T>::CopyAllSourcesExceptInput,
{SystemBase::all_sources_except_input_ports_ticket()});

// Declare input ports.
for (int i = 0; i < continuous_system_->num_input_ports(); ++i) {
Expand All @@ -117,29 +124,40 @@ class DiscreteTimeSystem final : public LeafSystem<T> {
// Declare output ports.
for (int i = 0; i < continuous_system_->num_output_ports(); ++i) {
const OutputPort<T>& port = continuous_system_->get_output_port(i);
bool input_dependent = continuous_system_->HasDirectFeedthrough(i);

std::set<DependencyTicket> prerequisites_of_calc{
input_dependent
? SystemBase::all_sources_ticket()
: SystemBase::all_sources_except_input_ports_ticket()};

if (port.get_data_type() == PortDataType::kVectorValued) {
this->DeclareVectorOutputPort(
port.get_name(), port.size(),
[this, &port](const Context<T>& discrete_context,
BasicVector<T>* out) {
[this, &port, input_dependent](const Context<T>& discrete_context,
BasicVector<T>* out) {
const Context<T>& continuous_context =
continuous_context_cache_entry_->template Eval<Context<T>>(
discrete_context);
(input_dependent ? continuous_context_cache_entry_
: continuous_context2_cache_entry_)
->template Eval<Context<T>>(discrete_context);
out->SetFromVector(port.Eval(continuous_context));
});
},
prerequisites_of_calc);
} else if (port.get_data_type() == PortDataType::kAbstractValued) {
this->DeclareAbstractOutputPort(
port.get_name(),
[&port] {
return port.Allocate();
},
[this, &port](const Context<T>& discrete_context,
AbstractValue* out) {
[this, &port, input_dependent](const Context<T>& discrete_context,
AbstractValue* out) {
const Context<T>& continuous_context =
continuous_context_cache_entry_->template Eval<Context<T>>(
discrete_context);
(input_dependent ? continuous_context_cache_entry_
: continuous_context2_cache_entry_)
->template Eval<Context<T>>(discrete_context);
port.Calc(continuous_context, out);
});
},
prerequisites_of_calc);
}
}

Expand Down Expand Up @@ -194,29 +212,34 @@ class DiscreteTimeSystem final : public LeafSystem<T> {
continuous_context.get_continuous_state_vector().CopyToVector());
}

void CopyDiscreteContextToContinuousContext(
const Context<T>& discrete_context,
Context<T>* continuous_context) const {
void CopyAllSourcesExceptInput(const Context<T>& from_discrete_context,
Context<T>* to_continuous_context) const {
// Copy time.
continuous_context->SetTime(discrete_context.get_time());
to_continuous_context->SetTime(from_discrete_context.get_time());
// Copy state.
continuous_context->SetContinuousState(
discrete_context.get_discrete_state_vector().value());
to_continuous_context->SetContinuousState(
from_discrete_context.get_discrete_state_vector().value());
// Copy parameters.
continuous_context->get_mutable_parameters().SetFrom(
discrete_context.get_parameters());
to_continuous_context->get_mutable_parameters().SetFrom(
from_discrete_context.get_parameters());
// Copy accuracy.
continuous_context->SetAccuracy(discrete_context.get_accuracy());
to_continuous_context->SetAccuracy(from_discrete_context.get_accuracy());
}

void CopyAllSources(const Context<T>& from_discrete_context,
Context<T>* to_continuous_context) const {
CopyAllSourcesExceptInput(from_discrete_context, to_continuous_context);
// Copy fixed input port values.
for (int i = 0; i < continuous_system_->num_input_ports(); ++i) {
continuous_context->FixInputPort(
i, *(this->EvalAbstractInput(discrete_context, i)));
to_continuous_context->FixInputPort(
i, *this->EvalAbstractInput(from_discrete_context, i));
}
}

const std::unique_ptr<const System<T>> continuous_system_;
const std::unique_ptr<const Context<T>> continuous_context_model_value_;
const CacheEntry* continuous_context_cache_entry_;
const CacheEntry* continuous_context2_cache_entry_;
const double time_period_;
const double time_offset_;
const SimulatorConfig integrator_config_;
Expand All @@ -233,8 +256,11 @@ template <typename T>
std::unique_ptr<System<T>> DiscreteTimeApproximation(
const System<T>& system, double time_period, double time_offset,
const SimulatorConfig& integrator_config) {
// Check that the original system is continuous.
DRAKE_THROW_UNLESS(system.IsDifferentialEquationSystem());
// Check that the discrete time_period is greater than zero.
DRAKE_THROW_UNLESS(time_period > 0);

return std::make_unique<DiscreteTimeSystem<T>>(
system.Clone(), time_period, time_offset, integrator_config);
}
Expand Down
17 changes: 3 additions & 14 deletions systems/analysis/test/discrete_time_approximation_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -249,33 +249,22 @@ TEST_F(DiscreteTimeApproximatedMultibodyPlantTest, SynthesizeDiscreteTimeLQR) {

TEST_F(DiscreteTimeApproximatedMultibodyPlantTest,
DiscreteTimeSystemSimulatable) {
const int num_states = discrete_context_->num_total_states();

DiagramBuilder<double> builder;
auto controller = builder.AddSystem(DiscreteStabilizingController());
auto plant = builder.AddSystem(std::move(discrete_sys_));
auto z_inv =
builder.AddSystem<DiscreteTimeDelay>(time_period_, 0, num_states);
// Add a discrete-time delay to avoid algebraic loop.
builder.Connect(plant->get_output_port(0), z_inv->get_input_port());
builder.Connect(z_inv->get_output_port(), controller->get_input_port());
builder.Connect(plant->get_output_port(0), controller->get_input_port());
builder.Connect(controller->get_output_port(), plant->get_input_port());
auto diagram = builder.Build();
auto diagram_context = diagram->CreateDefaultContext();

EXPECT_EQ(diagram_context->num_discrete_state_groups(), 2);
EXPECT_EQ(diagram_context->get_discrete_state(0).size(), num_states);
EXPECT_EQ(diagram_context->get_discrete_state(1).size(), num_states);

const double theta_init = M_PI * 0.9;
Eigen::VectorXd x_init(4);
x_init << 0, theta_init, 0, 0;
diagram_context->SetDiscreteState(0, x_init);
diagram_context->SetDiscreteState(1, x_init);
diagram_context->SetDiscreteState(x_init);

Simulator simulator(*diagram, std::move(diagram_context));
simulator.AdvanceTo(10);
const double theta_final = simulator.get_context().get_discrete_state(0)[1];
const double theta_final = simulator.get_context().get_discrete_state()[1];
// Check discrete-time controller stabilizes discrete-time cart-pole.
const double theta_d = M_PI;
EXPECT_LT(std::abs(theta_final - theta_d), std::abs(theta_init - theta_d));
Expand Down

0 comments on commit c4cba67

Please sign in to comment.