Skip to content

Commit

Permalink
Replaces pointer versions of get_integrator() and get_mutable_integra…
Browse files Browse the repository at this point in the history
…tor().
  • Loading branch information
edrumwri authored Apr 15, 2019
1 parent 2679bb0 commit df4a24b
Show file tree
Hide file tree
Showing 31 changed files with 142 additions and 124 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -285,7 +285,7 @@ int do_main() {
x0.SetAtIndex(15, 0.055); // right gripper

simulator.set_target_realtime_rate(0);
simulator.get_mutable_integrator()->set_maximum_step_size(h);
simulator.get_mutable_integrator().set_maximum_step_size(h);
simulator.Initialize();
simulator.AdvanceTo(kTimes.back());

Expand Down
2 changes: 1 addition & 1 deletion attic/manipulation/dev/test/rod2d_time_stepping.cc
Original file line number Diff line number Diff line change
Expand Up @@ -202,7 +202,7 @@ VectorXd RunSimulation(const bool is_analytic) {

simulator.set_target_realtime_rate(0);
const double h = rod2d->get_period_sec();
simulator.get_mutable_integrator()->set_maximum_step_size(h);
simulator.get_mutable_integrator().set_maximum_step_size(h);
simulator.Initialize();
simulator.AdvanceTo(7.5);

Expand Down
4 changes: 2 additions & 2 deletions attic/manipulation/scene_generation/simulate_plant_to_rest.cc
Original file line number Diff line number Diff line change
Expand Up @@ -90,8 +90,8 @@ VectorX<double> SimulatePlantToRest::Run(const VectorX<double>& q_ik,

simulator.reset_integrator<systems::RungeKutta2Integrator<double>>(
*diagram_, 0.0001, &simulator.get_mutable_context());
simulator.get_mutable_integrator()->set_maximum_step_size(max_step_size);
simulator.get_mutable_integrator()->set_fixed_step_mode(true);
simulator.get_mutable_integrator().set_maximum_step_size(max_step_size);
simulator.get_mutable_integrator().set_fixed_step_mode(true);
step_time = 1.0;
step_delta = 0.1;
do {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -148,7 +148,7 @@ GTEST_TEST(TestAccelerometer, TestSensorAttachedToSwingingPendulum) {
Simulator<double> simulator(diagram, std::move(context));
// Decrease the step size to get x_dot(0) to be closer to x(1).
const double stepSize = 1e-4;
simulator.get_mutable_integrator()->set_maximum_step_size(stepSize);
simulator.get_mutable_integrator().set_maximum_step_size(stepSize);
simulator.Initialize();

const AccelerometerTestLogger& logger = diagram.get_logger();
Expand Down
2 changes: 1 addition & 1 deletion automotive/automotive_simulator.cc
Original file line number Diff line number Diff line change
Expand Up @@ -730,7 +730,7 @@ void AutomotiveSimulator<T>::Start(
const double max_step_size = 0.01;
simulator_->template reset_integrator<RungeKutta2Integrator<T>>(
*diagram_, max_step_size, &simulator_->get_mutable_context());
simulator_->get_mutable_integrator()->set_fixed_step_mode(true);
simulator_->get_mutable_integrator().set_fixed_step_mode(true);
simulator_->Initialize();
}

Expand Down
16 changes: 8 additions & 8 deletions automotive/maliput/multilane/road_curve.cc
Original file line number Diff line number Diff line change
Expand Up @@ -133,11 +133,11 @@ RoadCurve::RoadCurve(double linear_tolerance, double scale_length,
// accuracy balance). However, for the time being, the following
// constants (considering 0.0 <= p <= 1.0) work well as a heuristic
// approximation to appropriate step sizes.
systems::IntegratorBase<double>* s_from_p_integrator =
systems::IntegratorBase<double>& s_from_p_integrator =
s_from_p_func_->get_mutable_integrator();
s_from_p_integrator->request_initial_step_size_target(0.1);
s_from_p_integrator->set_maximum_step_size(1.0);
s_from_p_integrator->set_target_accuracy(relative_tolerance_);
s_from_p_integrator.request_initial_step_size_target(0.1);
s_from_p_integrator.set_maximum_step_size(1.0);
s_from_p_integrator.set_target_accuracy(relative_tolerance_);

// Sets `p_from_s`'s integration accuracy and step sizes. Said steps
// should not be too large, because that could make accuracy control
Expand All @@ -146,11 +146,11 @@ RoadCurve::RoadCurve(double linear_tolerance, double scale_length,
// optimal step sizes (in terms of their efficiency vs. accuracy balance).
// However, for the time being, the following proportions of the scale
// length work well as a heuristic approximation to appropriate step sizes.
systems::IntegratorBase<double>* p_from_s_integrator =
systems::IntegratorBase<double>& p_from_s_integrator =
p_from_s_ivp_->get_mutable_integrator();
p_from_s_integrator->request_initial_step_size_target(0.1 * scale_length);
p_from_s_integrator->set_maximum_step_size(scale_length);
p_from_s_integrator->set_target_accuracy(relative_tolerance_);
p_from_s_integrator.request_initial_step_size_target(0.1 * scale_length);
p_from_s_integrator.set_maximum_step_size(scale_length);
p_from_s_integrator.set_target_accuracy(relative_tolerance_);
}

bool RoadCurve::AreFastComputationsAccurate(double r) const {
Expand Down
4 changes: 2 additions & 2 deletions examples/acrobot/run_lqr_w_estimator.cc
Original file line number Diff line number Diff line change
Expand Up @@ -134,8 +134,8 @@ int do_main() {

// Simulate.
simulator.set_target_realtime_rate(FLAGS_realtime_factor);
simulator.get_mutable_integrator()->set_maximum_step_size(0.01);
simulator.get_mutable_integrator()->set_fixed_step_mode(true);
simulator.get_mutable_integrator().set_maximum_step_size(0.01);
simulator.get_mutable_integrator().set_fixed_step_mode(true);
simulator.Initialize();
simulator.AdvanceTo(FLAGS_simulation_sec);

Expand Down
4 changes: 2 additions & 2 deletions examples/bouncing_ball/test/bouncing_ball_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -166,8 +166,8 @@ TEST_F(BouncingBallTest, Simulate) {
simulator.reset_integrator<systems::RungeKutta3Integrator<double>>(*dut_,
&simulator.get_mutable_context());
simulator.get_mutable_context().SetAccuracy(accuracy);
simulator.get_mutable_integrator()->request_initial_step_size_target(1e-3);
simulator.get_mutable_integrator()->set_target_accuracy(accuracy);
simulator.get_mutable_integrator().request_initial_step_size_target(1e-3);
simulator.get_mutable_integrator().set_target_accuracy(accuracy);
simulator.Initialize();

// Set the initial state for the bouncing ball.
Expand Down
4 changes: 2 additions & 2 deletions examples/contact_model/rigid_gripper.cc
Original file line number Diff line number Diff line change
Expand Up @@ -146,8 +146,8 @@ int main() {
systems::Context<double>& context = simulator.get_mutable_context();

simulator.reset_integrator<RungeKutta3Integrator<double>>(*model, &context);
simulator.get_mutable_integrator()->request_initial_step_size_target(1e-4);
simulator.get_mutable_integrator()->set_target_accuracy(FLAGS_accuracy);
simulator.get_mutable_integrator().request_initial_step_size_target(1e-4);
simulator.get_mutable_integrator().set_target_accuracy(FLAGS_accuracy);
std::cout << "Variable-step integrator accuracy: " << FLAGS_accuracy << "\n";

simulator.Initialize();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ int DoMain() {
systems::Simulator<double> simulator(*diagram);
simulator.Initialize();
simulator.set_target_realtime_rate(1.0);
simulator.get_mutable_integrator()->set_target_accuracy(1e-3);
simulator.get_mutable_integrator().set_target_accuracy(1e-3);

simulator.AdvanceTo(FLAGS_simulation_sec);
return 0;
Expand Down
10 changes: 5 additions & 5 deletions examples/manipulation_station/differential_ik.py
Original file line number Diff line number Diff line change
Expand Up @@ -39,15 +39,15 @@ def __init__(self, robot, frame_E, parameters, time_step):
self.robot_context)[-robot.num_velocities():].any()

# Store the robot positions as state.
self._DeclareDiscreteState(robot.num_positions())
self._DeclarePeriodicDiscreteUpdate(time_step)
self.DeclareDiscreteState(robot.num_positions())
self.DeclarePeriodicDiscreteUpdate(time_step)

# Desired pose of frame E in world frame.
self._DeclareInputPort("rpy_xyz_desired",
PortDataType.kVectorValued, 6)
self.DeclareInputPort("rpy_xyz_desired",
PortDataType.kVectorValued, 6)

# Provide the output as desired positions.
self._DeclareVectorOutputPort("joint_position_desired", BasicVector(
self.DeclareVectorOutputPort("joint_position_desired", BasicVector(
robot.num_positions()), self.CopyPositionOut)

def SetPositions(self, context, q):
Expand Down
14 changes: 7 additions & 7 deletions examples/manipulation_station/end_effector_teleop_mouse.py
Original file line number Diff line number Diff line change
Expand Up @@ -123,16 +123,16 @@ def get_events(self):
class MouseKeyboardTeleop(LeafSystem):
def __init__(self, grab_focus=True):
LeafSystem.__init__(self)
self._DeclareVectorOutputPort("rpy_xyz", BasicVector(6),
self._DoCalcOutput)
self._DeclareVectorOutputPort("position", BasicVector(1),
self.CalcPositionOutput)
self._DeclareVectorOutputPort("force_limit", BasicVector(1),
self.CalcForceLimitOutput)
self.DeclareVectorOutputPort("rpy_xyz", BasicVector(6),
self._DoCalcOutput)
self.DeclareVectorOutputPort("position", BasicVector(1),
self.CalcPositionOutput)
self.DeclareVectorOutputPort("force_limit", BasicVector(1),
self.CalcForceLimitOutput)

# Note: This timing affects the keyboard teleop performance. A larger
# time step causes more lag in the response.
self._DeclarePeriodicPublish(0.01, 0.0)
self.DeclarePeriodicPublish(0.01, 0.0)

self.teleop_manager = TeleopMouseKeyboardManager(grab_focus=grab_focus)
self.roll = self.pitch = self.yaw = 0
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -162,12 +162,12 @@ int do_main() {
plant.SetFreeBodyPoseInWorldFrame(&plant_context, bodyB, X_WB);

systems::Simulator<double> simulator(*diagram, std::move(diagram_context));
systems::IntegratorBase<double>* integrator =
systems::IntegratorBase<double>& integrator =
simulator.get_mutable_integrator();

// Set the integration accuracy when the plant is integrated with a variable-
// step integrator. This value is not used if time_step > 0 (fixed-time step).
integrator->set_target_accuracy(FLAGS_integration_accuracy);
integrator.set_target_accuracy(FLAGS_integration_accuracy);

simulator.set_publish_every_time_step(false);
simulator.set_target_realtime_rate(FLAGS_target_realtime_rate);
Expand Down
2 changes: 1 addition & 1 deletion examples/quadrotor/run_quadrotor_lqr.cc
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ int do_main() {

// The following accuracy is necessary for the example to satisfy its
// ending state tolerances.
simulator.get_mutable_integrator()->set_target_accuracy(5e-5);
simulator.get_mutable_integrator().set_target_accuracy(5e-5);
simulator.AdvanceTo(FLAGS_trial_duration);

// Goal state verification.
Expand Down
4 changes: 2 additions & 2 deletions examples/rod2d/rod2d_sim.cc
Original file line number Diff line number Diff line change
Expand Up @@ -136,8 +136,8 @@ int main(int argc, char* argv[]) {
simulator.reset_integrator<RungeKutta3Integrator<double>>(*diagram,
&mut_context);
}
simulator.get_mutable_integrator()->set_target_accuracy(FLAGS_accuracy);
simulator.get_mutable_integrator()->set_maximum_step_size(FLAGS_dt);
simulator.get_mutable_integrator().set_target_accuracy(FLAGS_accuracy);
simulator.get_mutable_integrator().set_maximum_step_size(FLAGS_dt);

// Start simulating.
simulator.set_target_realtime_rate(1.0);
Expand Down
2 changes: 1 addition & 1 deletion examples/scene_graph/bouncing_ball_run_dynamics.cc
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ int do_main() {
init_ball(bouncing_ball1, 0.3, 0.);
init_ball(bouncing_ball2, 0.3, 0.3);

simulator.get_mutable_integrator()->set_maximum_step_size(0.002);
simulator.get_mutable_integrator().set_maximum_step_size(0.002);
simulator.set_target_realtime_rate(1.f);
simulator.Initialize();
simulator.AdvanceTo(FLAGS_simulation_time);
Expand Down
2 changes: 1 addition & 1 deletion examples/scene_graph/dev/bouncing_ball_run_dynamics.cc
Original file line number Diff line number Diff line change
Expand Up @@ -176,7 +176,7 @@ int do_main() {
init_ball(bouncing_ball1, 0.3, 0.);
init_ball(bouncing_ball2, 0.3, 0.3);

simulator.get_mutable_integrator()->set_maximum_step_size(0.002);
simulator.get_mutable_integrator().set_maximum_step_size(0.002);
simulator.Initialize();
simulator.set_publish_every_time_step(false);
simulator.set_publish_at_initialization(false);
Expand Down
2 changes: 1 addition & 1 deletion examples/scene_graph/dev/solar_system_run_dynamics.cc
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ int do_main() {

systems::Simulator<double> simulator(*diagram);

simulator.get_mutable_integrator()->set_maximum_step_size(0.002);
simulator.get_mutable_integrator().set_maximum_step_size(0.002);
simulator.set_publish_every_time_step(false);
simulator.set_target_realtime_rate(1);
simulator.Initialize();
Expand Down
2 changes: 1 addition & 1 deletion examples/scene_graph/solar_system_run_dynamics.cc
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ int do_main() {

systems::Simulator<double> simulator(*diagram);

simulator.get_mutable_integrator()->set_maximum_step_size(0.002);
simulator.get_mutable_integrator().set_maximum_step_size(0.002);
simulator.set_publish_every_time_step(false);
simulator.set_target_realtime_rate(1);
simulator.Initialize();
Expand Down
4 changes: 2 additions & 2 deletions multibody/plant/test/inclined_plane_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -135,8 +135,8 @@ TEST_P(InclinedPlaneTest, RollingSphereTest) {
plant.SetFreeBodyPoseInWorldFrame(&plant_context, ball, X_WB_initial);

Simulator<double> simulator(*diagram, std::move(diagram_context));
IntegratorBase<double>* integrator = simulator.get_mutable_integrator();
integrator->set_target_accuracy(target_accuracy);
IntegratorBase<double>& integrator = simulator.get_mutable_integrator();
integrator.set_target_accuracy(target_accuracy);
simulator.set_publish_every_time_step(true);
simulator.Initialize();
simulator.AdvanceTo(simulation_time);
Expand Down
6 changes: 3 additions & 3 deletions multibody/plant/test/spring_mass_system_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -141,7 +141,7 @@ TEST_F(SpringMassSystemTest, UnDampedCase) {
slider_->set_translation(&context, free_length_ + amplitude);
slider_->set_translation_rate(&context, 0.0);
simulator.Initialize();
simulator.get_mutable_integrator()->set_target_accuracy(integration_accuracy);
simulator.get_mutable_integrator().set_target_accuracy(integration_accuracy);
simulator.AdvanceTo(simulation_time);

const double x_analytic = CalcAnalyticSolution(
Expand Down Expand Up @@ -172,7 +172,7 @@ TEST_F(SpringMassSystemTest, UnderDampedCase) {
slider_->set_translation(&context, free_length_ + amplitude);
slider_->set_translation_rate(&context, 0.0);
simulator.Initialize();
simulator.get_mutable_integrator()->set_target_accuracy(integration_accuracy);
simulator.get_mutable_integrator().set_target_accuracy(integration_accuracy);
simulator.AdvanceTo(simulation_time);

const double x_analytic = CalcAnalyticSolution(
Expand Down Expand Up @@ -203,7 +203,7 @@ TEST_F(SpringMassSystemTest, OverDampedCase) {
slider_->set_translation(&context, free_length_ + amplitude);
slider_->set_translation_rate(&context, 0.0);
simulator.Initialize();
simulator.get_mutable_integrator()->set_target_accuracy(integration_accuracy);
simulator.get_mutable_integrator().set_target_accuracy(integration_accuracy);
simulator.AdvanceTo(simulation_time);

const double x_analytic = CalcAnalyticSolution(
Expand Down
1 change: 1 addition & 0 deletions systems/analysis/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -255,6 +255,7 @@ drake_cc_googletest(
":implicit_euler_integrator",
":runge_kutta3_integrator",
":simulator",
"//common/test_utilities:expect_throws_message",
"//common/test_utilities:is_dynamic_castable",
"//systems/analysis/test_utilities",
"//systems/primitives",
Expand Down
8 changes: 4 additions & 4 deletions systems/analysis/antiderivative_function.h
Original file line number Diff line number Diff line change
Expand Up @@ -196,13 +196,13 @@ class AntiderivativeFunction {
std::forward<Args>(args)...);
}

/// Gets a pointer to the internal integrator instance.
const IntegratorBase<T>* get_integrator() const {
/// Gets a reference to the internal integrator instance.
const IntegratorBase<T>& get_integrator() const {
return scalar_ivp_->get_integrator();
}

/// Gets a pointer to the internal mutable integrator instance.
IntegratorBase<T>* get_mutable_integrator() {
/// Gets a mutable reference to the internal integrator instance.
IntegratorBase<T>& get_mutable_integrator() {
return scalar_ivp_->get_mutable_integrator();
}

Expand Down
14 changes: 8 additions & 6 deletions systems/analysis/initial_value_problem.h
Original file line number Diff line number Diff line change
Expand Up @@ -200,14 +200,16 @@ class InitialValueProblem {
return static_cast<Integrator*>(integrator_.get());
}

/// Gets a pointer to the internal integrator instance.
const IntegratorBase<T>* get_integrator() const {
return integrator_.get();
/// Gets a reference to the internal integrator instance.
const IntegratorBase<T>& get_integrator() const {
DRAKE_DEMAND(integrator_.get());
return *integrator_.get();
}

/// Gets a pointer to the internal mutable integrator instance.
IntegratorBase<T>* get_mutable_integrator() {
return integrator_.get();
/// Gets a mutable reference to the internal integrator instance.
IntegratorBase<T>& get_mutable_integrator() {
DRAKE_DEMAND(integrator_.get());
return *integrator_.get();
}

private:
Expand Down
8 changes: 4 additions & 4 deletions systems/analysis/scalar_initial_value_problem.h
Original file line number Diff line number Diff line change
Expand Up @@ -194,13 +194,13 @@ class ScalarInitialValueProblem {
std::forward<Args>(args)...);
}

/// Gets a pointer to the internal integrator instance.
const IntegratorBase<T>* get_integrator() const {
/// Gets a reference to the internal integrator instance.
const IntegratorBase<T>& get_integrator() const {
return vector_ivp_->get_integrator();
}

/// Gets a pointer to the internal mutable integrator instance.
IntegratorBase<T>* get_mutable_integrator() {
/// Gets a mutable reference to the internal integrator instance.
IntegratorBase<T>& get_mutable_integrator() {
return vector_ivp_->get_mutable_integrator();
}

Expand Down
13 changes: 8 additions & 5 deletions systems/analysis/simulator.h
Original file line number Diff line number Diff line change
Expand Up @@ -464,13 +464,13 @@ class Simulator {
int64_t get_num_unrestricted_updates() const {
return num_unrestricted_updates_; }

/// Gets a pointer to the integrator used to advance the continuous aspects
/// Gets a reference to the integrator used to advance the continuous aspects
/// of the system.
const IntegratorBase<T>* get_integrator() const { return integrator_.get(); }
const IntegratorBase<T>& get_integrator() const { return *integrator_.get(); }

/// Gets a pointer to the mutable integrator used to advance the continuous
/// aspects of the system.
IntegratorBase<T>* get_mutable_integrator() { return integrator_.get(); }
/// Gets a reference to the mutable integrator used to advance the continuous
/// state of the system.
IntegratorBase<T>& get_mutable_integrator() { return *integrator_.get(); }

/// Resets the integrator with a new one. An example usage is:
/// @code
Expand All @@ -480,8 +480,11 @@ class Simulator {
/// ensure the integrator is properly initialized. You can do that explicitly
/// with the Initialize() method or it will be done implicitly at the first
/// time step.
/// @throws std::logic_error if `integrator` is nullptr.
template <class U>
U* reset_integrator(std::unique_ptr<U> integrator) {
if (!integrator)
throw std::logic_error("Integrator cannot be null.");
initialization_done_ = false;
integrator_ = std::move(integrator);
return static_cast<U*>(integrator_.get());
Expand Down
Loading

0 comments on commit df4a24b

Please sign in to comment.