-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
acrobot_plant.h
155 lines (129 loc) · 5.03 KB
/
acrobot_plant.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
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
#pragma once
#include <memory>
#include "drake/examples/acrobot/gen/acrobot_input.h"
#include "drake/examples/acrobot/gen/acrobot_params.h"
#include "drake/examples/acrobot/gen/acrobot_state.h"
#include "drake/systems/framework/basic_vector.h"
#include "drake/systems/framework/diagram.h"
#include "drake/systems/framework/leaf_system.h"
#include "drake/systems/primitives/affine_system.h"
namespace drake {
namespace examples {
namespace acrobot {
/// @defgroup acrobot_systems Acrobot
/// @{
/// @brief Systems related to the Acrobot example.
/// @ingroup example_systems
/// @}
/// The Acrobot - a canonical underactuated system as described in <a
/// href="http://underactuated.mit.edu/underactuated.html?chapter=3">Chapter 3
/// of Underactuated Robotics</a>.
///
/// @system
/// name: AcrobotPlant
/// input_ports:
/// - elbow_torque (optional)
/// output_ports:
/// - acrobot_state
/// @endsystem
///
/// Note: If the elbow_torque input port is not connected, then the torque is
/// taken to be zero.
///
/// @tparam_default_scalar
/// @ingroup acrobot_systems
template <typename T>
class AcrobotPlant : public systems::LeafSystem<T> {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(AcrobotPlant)
/// Constructs the plant. The parameters of the system are stored as
/// Parameters in the Context (see acrobot_params_named_vector.yaml).
AcrobotPlant();
/// Scalar-converting copy constructor. See @ref system_scalar_conversion.
template <typename U>
explicit AcrobotPlant(const AcrobotPlant<U>&);
/// Sets the parameters to describe MIT Robot Locomotion Group's hardware
/// acrobot.
void SetMITAcrobotParameters(systems::Parameters<T>* parameters) const;
///@{
/// Manipulator equation of Acrobot: M(q)q̈ + bias(q,q̇) = B*u.
///
/// - M[2x2] is the mass matrix.
/// - bias[2x1] includes the Coriolis term, gravity term and the damping term,
/// i.e. bias[2x1] = C(q,v)*v - τ_g(q) + [b1*q̇₁;b2*q̇₂].
// TODO(russt): Update this to the newest conventions.
Vector2<T> DynamicsBiasTerm(const systems::Context<T> &context) const;
Matrix2<T> MassMatrix(const systems::Context<T> &context) const;
///@}
/// Evaluates the input port and returns the scalar value of the commanded
/// torque. If the input port is not connected, then the torque is taken to
/// be zero.
const T get_tau(const systems::Context<T>& context) const {
const systems::BasicVector<T>* u_vec = this->EvalVectorInput(context, 0);
return u_vec ? u_vec->GetAtIndex(0) : 0.0;
}
static const AcrobotState<T>& get_state(
const systems::ContinuousState<T>& cstate) {
return dynamic_cast<const AcrobotState<T>&>(cstate.get_vector());
}
static const AcrobotState<T>& get_state(const systems::Context<T>& context) {
return get_state(context.get_continuous_state());
}
static AcrobotState<T>& get_mutable_state(
systems::ContinuousState<T>* cstate) {
return dynamic_cast<AcrobotState<T>&>(cstate->get_mutable_vector());
}
static AcrobotState<T>& get_mutable_state(systems::Context<T>* context) {
return get_mutable_state(&context->get_mutable_continuous_state());
}
const AcrobotParams<T>& get_parameters(
const systems::Context<T>& context) const {
return this->template GetNumericParameter<AcrobotParams>(context, 0);
}
AcrobotParams<T>& get_mutable_parameters(systems::Context<T>* context) const {
return this->template GetMutableNumericParameter<AcrobotParams>(context, 0);
}
private:
T DoCalcKineticEnergy(const systems::Context<T>& context) const override;
T DoCalcPotentialEnergy(const systems::Context<T>& context) const override;
void DoCalcTimeDerivatives(
const systems::Context<T>& context,
systems::ContinuousState<T>* derivatives) const override;
void DoCalcImplicitTimeDerivativesResidual(
const systems::Context<T>& context,
const systems::ContinuousState<T>& proposed_derivatives,
EigenPtr<VectorX<T>> residual) const override;
};
/// Constructs the Acrobot with (only) encoder outputs.
///
/// @system
/// name: AcrobotWEncoder
/// input_ports:
/// - elbow_torque
/// output_ports:
/// - measured_joint_positions
/// - <span style="color:gray">acrobot_state</span>
/// @endsystem
///
/// The `acrobot_state` output port is present only if the construction
/// parameter `acrobot_state_as_second_output` is true.
///
/// @ingroup acrobot_systems
template <typename T>
class AcrobotWEncoder : public systems::Diagram<T> {
public:
explicit AcrobotWEncoder(bool acrobot_state_as_second_output = false);
const AcrobotPlant<T>* acrobot_plant() const { return acrobot_plant_; }
AcrobotState<T>& get_mutable_acrobot_state(
systems::Context<T>* context) const;
private:
AcrobotPlant<T>* acrobot_plant_{nullptr};
};
/// Constructs the LQR controller for stabilizing the upright fixed point using
/// default LQR cost matrices which have been tested for this system.
/// @ingroup acrobot_systems
std::unique_ptr<systems::AffineSystem<double>> BalancingLQRController(
const AcrobotPlant<double>& acrobot);
} // namespace acrobot
} // namespace examples
} // namespace drake