Drake
spring_mass_system.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <cmath>
4 #include <stdexcept>
5 
9 
10 namespace drake {
11 namespace systems {
12 
13 /// The state of a one-dimensional spring-mass system, consisting of the
14 /// position and velocity of the mass, in meters and meters/s.
15 ///
16 /// @tparam T The vector element type, which must be a valid Eigen scalar.
17 ///
18 /// Instantiated templates for the following kinds of T's are provided:
19 /// - double
20 /// - AutoDiffXd
21 ///
22 /// They are already available to link against in the containing library.
23 /// No other values for T are currently supported.
24 template <typename T>
26  public:
28 
29  /// @param initial_position The position of the mass in meters.
30  /// @param initial_velocity The velocity of the mass in meters / second.
31  SpringMassStateVector(const T& initial_position, const T& initial_velocity);
32  /// Creates a state with position and velocity set to zero.
34  ~SpringMassStateVector() override;
35 
36  /// Returns the position of the mass in meters, where zero is the point
37  /// where the spring exerts no force.
38  T get_position() const;
39 
40  /// Sets the position of the mass in meters.
41  void set_position(const T& q);
42 
43  /// Returns the velocity of the mass in meters per second.
44  T get_velocity() const;
45 
46  /// Sets the velocity of the mass in meters per second.
47  void set_velocity(const T& v);
48 
49  /// Returns the integral of conservative power, in watts.
50  T get_conservative_work() const;
51 
52  /// Initialize the conservative work integral to a given value.
53  void set_conservative_work(const T& e);
54 
55  private:
56  SpringMassStateVector<T>* DoClone() const override;
57 };
58 
59 /// A model of a one-dimensional spring-mass system.
60 ///
61 /// @verbatim
62 /// |-----\/\/ k /\/\----( m ) +x
63 /// @endverbatim
64 /// Units are MKS (meters-kilograms-seconds).
65 ///
66 /// Instantiated templates for the following kinds of T's are provided:
67 /// - double
68 /// - AutoDiffXd
69 ///
70 /// They are already available to link against in the containing library.
71 /// No other values for T are currently supported.
72 ///
73 /// @ingroup rigid_body_systems
74 template <typename T>
75 class SpringMassSystem : public LeafSystem<T> {
76  public:
78 
79  /// Constructs a spring-mass system with a fixed spring constant and given
80  /// mass. Subclasses must use the protected constructor, not this one.
81  /// @param[in] spring_constant_N_per_m The spring constant in N/m.
82  /// @param[in] mass_Kg The actual value in Kg of the mass attached to the
83  /// spring.
84  /// @param[in] system_is_forced If `true`, the system has an input port for an
85  /// external force. If `false`, the system has no inputs.
86  SpringMassSystem(double spring_constant_N_per_m, double mass_kg,
87  bool system_is_forced = false);
88 
89  /// Scalar-converting copy constructor. See @ref system_scalar_conversion.
90  template <typename U>
91  explicit SpringMassSystem(const SpringMassSystem<U>&);
92 
93  // Provide methods specific to this System.
94 
95  /// Returns the input port to the externally applied force.
96  const InputPortDescriptor<T>& get_force_port() const;
97 
98  /// Returns the port to output state.
99  const OutputPort<T>& get_output_port() const;
100 
101  /// Returns the spring constant k that was provided at construction, in N/m.
102  double get_spring_constant() const { return spring_constant_N_per_m_; }
103 
104  /// Returns the mass m that was provided at construction, in kg.
105  double get_mass() const { return mass_kg_; }
106 
107  /// Returns true iff the system is forced.
108  bool get_system_is_forced() const { return system_is_forced_; }
109 
110  /// Gets the current position of the mass in the given Context.
111  T get_position(const Context<T>& context) const {
112  return get_state(context).get_position();
113  }
114 
115  /// Gets the current velocity of the mass in the given Context.
116  T get_velocity(const Context<T>& context) const {
117  return get_state(context).get_velocity();
118  }
119 
120  /// @returns the external driving force to the system.
121  T get_input_force(const Context<T>& context) const {
122  T external_force = 0;
123  DRAKE_ASSERT(system_is_forced_ == (context.get_num_input_ports() == 1));
124  if (system_is_forced_) {
125  external_force = this->EvalVectorInput(context, 0)->GetAtIndex(0);
126  }
127  return external_force;
128  }
129 
130  /// Gets the current value of the conservative power integral in the given
131  /// Context.
132  T get_conservative_work(const Context<T>& context) const {
133  return get_state(context).get_conservative_work();
134  }
135 
136  /// Sets the position of the mass in the given Context.
137  void set_position(Context<T>* context, const T& position) const {
138  get_mutable_state(context).set_position(position);
139  }
140 
141  /// Sets the velocity of the mass in the given Context.
142  void set_velocity(Context<T>* context, const T& velocity) const {
143  get_mutable_state(context).set_velocity(velocity);
144  }
145 
146  /// Sets the initial value of the conservative power integral in the given
147  /// Context.
148  void set_conservative_work(Context<T>* context, const T& energy) const {
149  get_mutable_state(context).set_conservative_work(energy);
150  }
151 
152  /// Returns the force being applied by the spring to the mass in the given
153  /// Context. This force f is given by `f = -k (x-x0)`; the spring applies the
154  /// opposite force -f to the world attachment point at the other end. The
155  /// force is in newtons N (kg-m/s^2).
156  T EvalSpringForce(const Context<T>& context) const;
157 
158  /// Returns the potential energy currently stored in the spring in the given
159  /// Context. For this linear spring, `pe = k (x-x0)^2 / 2`, so that spring
160  /// force `f = -k (x-x0)` is the negative gradient of pe. The rate of change
161  /// of potential energy (that is, power that adding to potential energy) is
162  /// @verbatim
163  /// power_pe = d/dt pe
164  /// = k (x-x0) v
165  /// = -f v.
166  /// @endverbatim
167  /// Energy is in joules J (N-m).
168  T DoCalcPotentialEnergy(const Context<T>& context) const override;
169 
170  /// Returns the current kinetic energy of the moving mass in the given
171  /// Context. This is `ke = m v^2 / 2` for this system. The rate of change of
172  /// kinetic energy (that is, power that adding to kinetic energy) is
173  /// @verbatim
174  /// power_ke = d/dt ke
175  /// = m v a
176  /// = m v (f/m)
177  /// = f v
178  /// = -power_pe
179  /// @endverbatim
180  /// (assuming the only force is due to the spring). Energy is in joules.
181  /// @see EvalSpringForce(), EvalPotentialEnergy()
182  T DoCalcKineticEnergy(const Context<T>& context) const override;
183 
184  /// Returns the rate at which mechanical energy is being converted from
185  /// potential energy in the spring to kinetic energy of the mass by this
186  /// spring-mass system in the given Context. For this
187  /// system, we have conservative power @verbatim
188  /// power_c = f v
189  /// = power_ke
190  /// = -power_pe
191  /// @endverbatim
192  /// This quantity is positive when the spring is accelerating the mass and
193  /// negative when the spring is decelerating the mass.
194  T DoCalcConservativePower(const Context<T>& context) const override;
195 
196  // TODO(sherm1) Currently this is a conservative system so there is no power
197  // generated or consumed. Add some kind of dissipation and/or actuation to
198  // make this more interesting. Russ suggests adding an Input which is a
199  // horizontal control force on the mass.
200 
201  /// Returns power that doesn't involve the conservative spring element. (There
202  /// is none in this system.)
203  T DoCalcNonConservativePower(const Context<T>& context) const override;
204 
205  void DoCalcTimeDerivatives(const Context<T>& context,
206  ContinuousState<T>* derivatives) const override;
207 
208  /// Returns the closed-form position and velocity solution for this system
209  /// from the given initial conditions.
210  /// @param x0 the position of the spring at time t = 0.
211  /// @param v0 the velocity of the spring at time t = 0.
212  /// @param tf the time at which to return the position and velocity.
213  /// @param[out] xf the position of the spring at time tf, on return.
214  /// @param[out] vf the velocity of the spring at time tf, on return.
215  /// @throws std::logic_error if xf or vf is nullptr or if the system is
216  /// forced.
217  void GetClosedFormSolution(const T& x0, const T& v0, const T& tf,
218  T* xf, T* vf) const {
219  using std::sqrt;
220  using std::sin;
221  using std::cos;
222 
223  if (!xf || !vf)
224  throw std::logic_error("Passed final position/velocity is null.");
225  if (system_is_forced_)
226  throw std::logic_error("Can only compute closed form solution on "
227  "unforced system");
228 
229  // d^2x/dt^2 = -kx/m
230  // solution to this ODE: x(t) = c1*cos(omega*t) + c2*sin(omega*t)
231  // where omega = sqrt(k/m)
232  // ẋ(t) = -c1*sin(omega*t)*omega + c2*cos(omega*t)*omega
233  // for t = 0, x(0) = c1, ẋ(0) = c2*omega
234 
235  // Setup c1 and c2 for ODE constants.
236  const T omega = sqrt(get_spring_constant() / get_mass());
237  const T c1 = x0;
238  const T c2 = v0 / omega;
239  *xf = c1*cos(omega*tf) + c2*sin(omega*tf);
240  *vf = -c1*sin(omega*tf)*omega + c2*cos(omega*tf)*omega;
241  }
242 
243  protected:
244  /// Constructor that specifies @ref system_scalar_conversion support.
246  SystemScalarConverter converter,
247  double spring_constant_N_per_m,
248  double mass_kg,
249  bool system_is_forced);
250 
251  private:
252  // This is the calculator method for the output port.
253  void SetOutputValues(const Context<T>& context,
254  SpringMassStateVector<T>* output) const;
255 
256  // TODO(david-german-tri): Add a cast that is dynamic_cast in Debug mode,
257  // and static_cast in Release mode.
258 
259  static const SpringMassStateVector<T>& get_state(
260  const ContinuousState<T>& cstate) {
261  return dynamic_cast<const SpringMassStateVector<T>&>(cstate.get_vector());
262  }
263 
264  static SpringMassStateVector<T>& get_mutable_state(
265  ContinuousState<T>* cstate) {
266  return dynamic_cast<SpringMassStateVector<T>&>(
267  cstate->get_mutable_vector());
268  }
269 
270  static const SpringMassStateVector<T>& get_state(const Context<T>& context) {
271  return get_state(context.get_continuous_state());
272  }
273 
274  static SpringMassStateVector<T>& get_mutable_state(Context<T>* context) {
275  return get_mutable_state(&context->get_mutable_continuous_state());
276  }
277 
278  const double spring_constant_N_per_m_{};
279  const double mass_kg_{};
280  const bool system_is_forced_{false};
281 };
282 
283 } // namespace systems
284 } // namespace drake
T get_input_force(const Context< T > &context) const
Definition: spring_mass_system.h:121
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:49
void set_conservative_work(const T &e)
Initialize the conservative work integral to a given value.
Definition: spring_mass_system.cc:53
Expression sin(const Expression &e)
Definition: symbolic_expression.cc:599
const double position
Definition: robot_plan_interpolator_test.cc:65
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:37
bool get_system_is_forced() const
Returns true iff the system is forced.
Definition: spring_mass_system.h:108
double get_spring_constant() const
Returns the spring constant k that was provided at construction, in N/m.
Definition: spring_mass_system.h:102
T get_velocity(const Context< T > &context) const
Gets the current velocity of the mass in the given Context.
Definition: spring_mass_system.h:116
VectorBase< T > & get_mutable_vector()
Returns a mutable reference to the entire continuous state vector.
Definition: continuous_state.h:111
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:132
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.
#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:37
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:137
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:142
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 a reference to the entire continuous state vector.
Definition: continuous_state.h:105
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:45
A model of a one-dimensional spring-mass system.
Definition: spring_mass_system.h:75
T get_conservative_work() const
Returns the integral of conservative power, in watts.
Definition: spring_mass_system.cc:41
~SpringMassStateVector() override
Definition: spring_mass_system.cc:28
ContinuousState< T > & get_mutable_continuous_state()
Returns a mutable reference to the continuous component of the state, which may be of size zero...
Definition: context.h:106
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:217
T get_position(const Context< T > &context) const
Gets the current position of the mass in the given Context.
Definition: spring_mass_system.h:111
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:148
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:105
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:33
SpringMassStateVector()
Creates a state with position and velocity set to zero.
Definition: spring_mass_system.cc:24
Vector6< double > velocity
Definition: pose_smoother.cc:29
const ContinuousState< T > & get_continuous_state() const
Returns a const reference to the continuous component of the state, which may be of size zero...
Definition: context.h:118
The state of a one-dimensional spring-mass system, consisting of the position and velocity of the mas...
Definition: spring_mass_system.h:25
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:33
Provides careful macros to selectively enable or disable the special member functions for copy-constr...