-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
bouncing_ball-inl.h
131 lines (104 loc) · 4.4 KB
/
bouncing_ball-inl.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
#pragma once
/// @file
/// Template method implementations for bouncing_ball.h.
/// Most users should only include that file, not this one.
/// For background, see http://drake.mit.edu/cxx_inl.html.
/* clang-format off to disable clang-format-includes */
#include "drake/examples/bouncing_ball/bouncing_ball.h"
/* clang-format on */
#include <algorithm>
#include <limits>
#include <memory>
#include <utility>
#include <vector>
#include "drake/common/drake_assert.h"
#include "drake/systems/framework/basic_vector.h"
namespace drake {
namespace bouncing_ball {
template <typename T>
BouncingBall<T>::BouncingBall() {}
template <typename T>
T BouncingBall<T>::EvalGuard(const systems::Context<T>& context) const {
DRAKE_ASSERT_VOID(systems::System<T>::CheckValidContext(context));
// Evaluate the guard function.
const systems::VectorBase<T>& state = context.get_continuous_state_vector();
// The guard is satisfied (returns a non-positive value) when
// the ball's position is less than or equal to zero and its
// velocity is non-positive.
return std::max(state.GetAtIndex(0), state.GetAtIndex(1));
}
template <typename T>
void BouncingBall<T>::PerformReset(systems::Context<T>* context) const {
DRAKE_ASSERT(context != nullptr);
DRAKE_ASSERT_VOID(systems::System<T>::CheckValidContext(*context));
// Define a pointer to the continuous state in the context.
auto& result = context->get_mutable_continuous_state_vector();
// Perform the reset: map the position to itself and negate the
// velocity and attenuate by the coefficient of restitution.
auto& state = context->get_mutable_continuous_state_vector();
result.SetAtIndex(1, -1.0 * this->restitution_coef_ * state.GetAtIndex(1));
}
template <typename T>
void BouncingBall<T>::DoCalcNextUpdateTime(
const systems::Context<T>& context,
systems::CompositeEventCollection<T>* events, T* time) const {
using std::sqrt;
// Get the state of the guard function.
const systems::VectorBase<T>& state = context.get_continuous_state_vector();
// Two cases: (1) the ball is in ballistic flight and (2) the ball is on
// the ground and about to return upward.
T x0 = state.GetAtIndex(0);
const T v0 = state.GetAtIndex(1);
if (x0 <= std::numeric_limits<double>::epsilon()) {
// Case (2) encountered. Verify that ball is returning upward.
DRAKE_DEMAND(v0 > 0.0);
// Update x0 such that the ball is slightly above the ground.
x0 = std::numeric_limits<double>::epsilon();
}
// The time that the ball will impact the ground is:
// gt^2/2 + v0*t + x0 = 0
// Solve the quadratic equation for t. We expect that b^2 >> 4ac in some
// cases, which means that we must use a special algorithm to combat
// cancellation error.
const T a = Ball<T>::get_gravitational_acceleration() / 2;
const T b = v0;
const T c = x0;
const T disc = (b * b - 4 * a * c);
DRAKE_DEMAND(disc > 0.0);
T r1 = (-b - sgn(b) * sqrt(disc)) / (2 * a);
T r2 = c / (a * r1);
// We want the smallest positive root.
if (r1 <= 0.0) r1 = std::numeric_limits<T>::infinity();
if (r2 <= 0.0) r2 = std::numeric_limits<T>::infinity();
// Verify that the impact time is reasonable.
DRAKE_DEMAND(std::min(r1, r2) > 0.0);
// Create an event.
std::unique_ptr<systems::UnrestrictedUpdateEvent<T>> event =
std::make_unique<systems::UnrestrictedUpdateEvent<T>>(
systems::Event<T>::TriggerType::kWitness);
// Compute the impact time.
*time = context.get_time() + std::min(r1, r2);
systems::EventCollection<systems::UnrestrictedUpdateEvent<T>>& uu_events =
events->get_mutable_unrestricted_update_events();
uu_events.add_event(std::move(event));
}
template <typename T>
void BouncingBall<T>::DoCalcUnrestrictedUpdate(
const systems::Context<T>& context,
const std::vector<const systems::UnrestrictedUpdateEvent<T>*>&,
systems::State<T>* next_state) const {
systems::VectorBase<T>& next_cstate =
next_state->get_mutable_continuous_state().get_mutable_vector();
// Get present state.
const systems::VectorBase<T>& cstate =
context.get_continuous_state().get_vector();
// Copy the present state to the new one.
next_state->CopyFrom(context.get_state());
// Verify that velocity is non-positive.
DRAKE_DEMAND(cstate.GetAtIndex(1) <= 0.0);
// Update the velocity.
next_cstate.SetAtIndex(1,
cstate.GetAtIndex(1) * this->restitution_coef_ * -1.);
}
} // namespace bouncing_ball
} // namespace drake