Drake
spring_mass_system.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <string>
5 
12 
13 namespace drake {
14 namespace systems {
15 
27 template <typename T>
29  public:
31 
32 
33  SpringMassStateVector(const T& initial_position, const T& initial_velocity);
37  ~SpringMassStateVector() override;
38 
41  T get_position() const;
42 
44  void set_position(const T& q);
45 
47  T get_velocity() const;
48 
50  void set_velocity(const T& v);
51 
53  T get_conservative_work() const;
54 
56  void set_conservative_work(const T& e);
57 
58  private:
59  SpringMassStateVector<T>* DoClone() const override;
60 };
61 
77 template <typename T>
78 class SpringMassSystem : public LeafSystem<T> {
79  public:
81 
82 
83  SpringMassSystem(double spring_constant_N_per_m, double mass_kg,
90  bool system_is_forced = false);
91 
93  template <typename U>
94  explicit SpringMassSystem(const SpringMassSystem<U>&);
95 
96  // Provide methods specific to this System.
97 
99  const InputPortDescriptor<T>& get_force_port() const;
100 
102  const OutputPort<T>& get_output_port() const;
103 
105  double get_spring_constant() const { return spring_constant_N_per_m_; }
106 
108  double get_mass() const { return mass_kg_; }
109 
111  bool get_system_is_forced() const { return system_is_forced_; }
112 
114  T get_position(const Context<T>& context) const {
115  return get_state(context).get_position();
116  }
117 
119  T get_velocity(const Context<T>& context) const {
120  return get_state(context).get_velocity();
121  }
122 
124  T get_input_force(const Context<T>& context) const {
125  T external_force = 0;
126  DRAKE_ASSERT(system_is_forced_ == (context.get_num_input_ports() == 1));
127  if (system_is_forced_) {
128  external_force = this->EvalVectorInput(context, 0)->GetAtIndex(0);
129  }
130  return external_force;
131  }
132 
135  T get_conservative_work(const Context<T>& context) const {
136  return get_state(context).get_conservative_work();
137  }
138 
140  void set_position(Context<T>* context, const T& position) const {
141  get_mutable_state(context)->set_position(position);
142  }
143 
145  void set_velocity(Context<T>* context, const T& velocity) const {
146  get_mutable_state(context)->set_velocity(velocity);
147  }
148 
151  void set_conservative_work(Context<T>* context, const T& energy) const {
152  get_mutable_state(context)->set_conservative_work(energy);
153  }
154 
159  T EvalSpringForce(const Context<T>& context) const;
160 
171  T DoCalcPotentialEnergy(const Context<T>& context) const override;
172 
185  T DoCalcKineticEnergy(const Context<T>& context) const override;
186 
197  T DoCalcConservativePower(const Context<T>& context) const override;
198 
199  // TODO(sherm1) Currently this is a conservative system so there is no power
200  // generated or consumed. Add some kind of dissipation and/or actuation to
201  // make this more interesting. Russ suggests adding an Input which is a
202  // horizontal control force on the mass.
203 
206  T DoCalcNonConservativePower(const Context<T>& context) const override;
207 
208  void DoCalcTimeDerivatives(const Context<T>& context,
209  ContinuousState<T>* derivatives) const override;
210 
220  void GetClosedFormSolution(const T& x0, const T& v0, const T& tf,
221  T* xf, T* vf) const {
222  using std::sqrt;
223  using std::sin;
224  using std::cos;
225 
226  if (!xf || !vf)
227  throw std::logic_error("Passed final position/velocity is null.");
228  if (system_is_forced_)
229  throw std::logic_error("Can only compute closed form solution on "
230  "unforced system");
231 
232  // d^2x/dt^2 = -kx/m
233  // solution to this ODE: x(t) = c1*cos(omega*t) + c2*sin(omega*t)
234  // where omega = sqrt(k/m)
235  // ẋ(t) = -c1*sin(omega*t)*omega + c2*cos(omega*t)*omega
236  // for t = 0, x(0) = c1, ẋ(0) = c2*omega
237 
238  // Setup c1 and c2 for ODE constants.
239  const T omega = sqrt(get_spring_constant() / get_mass());
240  const T c1 = x0;
241  const T c2 = v0 / omega;
242  *xf = c1*cos(omega*tf) + c2*sin(omega*tf);
243  *vf = -c1*sin(omega*tf)*omega + c2*cos(omega*tf)*omega;
244  }
245 
246  protected:
249  SystemScalarConverter converter,
250  double spring_constant_N_per_m,
251  double mass_kg,
252  bool system_is_forced);
253 
254  private:
255  // This is the calculator method for the output port.
256  void SetOutputValues(const Context<T>& context,
257  SpringMassStateVector<T>* output) const;
258 
259  // TODO(david-german-tri): Add a cast that is dynamic_cast in Debug mode,
260  // and static_cast in Release mode.
261 
262  static const SpringMassStateVector<T>& get_state(
263  const ContinuousState<T>& cstate) {
264  return dynamic_cast<const SpringMassStateVector<T>&>(cstate.get_vector());
265  }
266 
267  static SpringMassStateVector<T>* get_mutable_state(
268  ContinuousState<T>* cstate) {
269  return dynamic_cast<SpringMassStateVector<T>*>(
270  cstate->get_mutable_vector());
271  }
272 
273  static const SpringMassStateVector<T>& get_state(const Context<T>& context) {
274  return get_state(*context.get_continuous_state());
275  }
276 
277  static SpringMassStateVector<T>* get_mutable_state(Context<T>* context) {
278  return get_mutable_state(context->get_mutable_continuous_state());
279  }
280 
281  const double spring_constant_N_per_m_{};
282  const double mass_kg_{};
283  const bool system_is_forced_{false};
284 };
285 
286 } // namespace systems
287 } // namespace drake
T get_input_force(const Context< T > &context) const
Definition: spring_mass_system.h:124
double position
Definition: system_identification_test.cc:205
Definition: automotive_demo.cc:88
void set_velocity(const T &v)
Sets the velocity of the mass in meters per second.
Definition: spring_mass_system.cc:52
void set_conservative_work(const T &e)
Initialize the conservative work integral to a given value.
Definition: spring_mass_system.cc:56
const ContinuousState< T > * get_continuous_state() const
Returns a const pointer to the continuous component of the state, which may be of size zero...
Definition: context.h:118
Expression sin(const Expression &e)
Definition: symbolic_expression.cc:599
Context is an abstract base class template that represents all the inputs to a System: time...
Definition: query_handle.h:10
T get_velocity() const
Returns the velocity of the mass in meters per second.
Definition: spring_mass_system.cc:40
bool get_system_is_forced() const
Returns true iff the system is forced.
Definition: spring_mass_system.h:111
double get_spring_constant() const
Returns the spring constant k that was provided at construction, in N/m.
Definition: spring_mass_system.h:105
T get_velocity(const Context< T > &context) const
Gets the current velocity of the mass in the given Context.
Definition: spring_mass_system.h:119
T get_conservative_work(const Context< T > &context) const
Gets the current value of the conservative power integral in the given Context.
Definition: spring_mass_system.h:135
InputPortDescriptor is a notation for specifying the kind of input a System accepts, on a given port.
Definition: input_port_descriptor.h:21
An OutputPort belongs to a System and represents the properties of one of that System&#39;s output ports...
Definition: output_port.h:67
virtual int get_num_input_ports() const =0
Returns the number of input ports.
int v
Definition: rgbd_camera_test.cc:165
#define DRAKE_ASSERT(condition)
DRAKE_ASSERT(condition) is similar to the built-in assert(condition) from the C++ system header <cas...
Definition: drake_assert.h:39
void set_position(Context< T > *context, const T &position) const
Sets the position of the mass in the given Context.
Definition: spring_mass_system.h:140
void set_velocity(Context< T > *context, const T &velocity) const
Sets the velocity of the mass in the given Context.
Definition: spring_mass_system.h:145
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:82
const VectorBase< T > & get_vector() const
Returns the entire continuous state vector.
Definition: continuous_state.h:105
VectorBase< T > * get_mutable_vector()
Returns a mutable pointer to the entire continuous state vector, which is never nullptr.
Definition: continuous_state.h:109
BasicVector is a semantics-free wrapper around an Eigen vector that satisfies VectorBase.
Definition: basic_vector.h:25
void set_position(const T &q)
Sets the position of the mass in meters.
Definition: spring_mass_system.cc:48
ContinuousState< T > * get_mutable_continuous_state()
Returns a mutable pointer to the continuous component of the state, which may be of size zero...
Definition: context.h:106
A model of a one-dimensional spring-mass system.
Definition: spring_mass_system.h:78
T get_conservative_work() const
Returns the integral of conservative power, in watts.
Definition: spring_mass_system.cc:44
~SpringMassStateVector() override
Definition: spring_mass_system.cc:31
void GetClosedFormSolution(const T &x0, const T &v0, const T &tf, T *xf, T *vf) const
Returns the closed-form position and velocity solution for this system from the given initial conditi...
Definition: spring_mass_system.h:220
T get_position(const Context< T > &context) const
Gets the current position of the mass in the given Context.
Definition: spring_mass_system.h:114
void set_conservative_work(Context< T > *context, const T &energy) const
Sets the initial value of the conservative power integral in the given Context.
Definition: spring_mass_system.h:151
Expression cos(const Expression &e)
Definition: symbolic_expression.cc:607
Expression sqrt(const Expression &e)
Definition: symbolic_expression.cc:553
double get_mass() const
Returns the mass m that was provided at construction, in kg.
Definition: spring_mass_system.h:108
ContinuousState is a container for all the continuous state variables xc.
Definition: continuous_state.h:44
Helper class to convert a System<U> into a System<T>, intended for internal use by the System framewo...
Definition: system_scalar_converter.h:35
double energy
Definition: MG_chaotic_babyboot_test.cc:25
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:35
SpringMassStateVector()
Creates a state with position and velocity set to zero.
Definition: spring_mass_system.cc:27
Vector6< double > velocity
Definition: pose_smoother.cc:29
The state of a one-dimensional spring-mass system, consisting of the position and velocity of the mas...
Definition: spring_mass_system.h:28
T get_position() const
Returns the position of the mass in meters, where zero is the point where the spring exerts no force...
Definition: spring_mass_system.cc:36
Provides careful macros to selectively enable or disable the special member functions for copy-constr...