-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
make_pendulum_plant.h
110 lines (98 loc) · 4.01 KB
/
make_pendulum_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
#pragma once
#include <limits>
#include <memory>
#include <string>
#include "drake/common/drake_copyable.h"
#include "drake/geometry/scene_graph.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/multibody/tree/revolute_joint.h"
namespace drake {
namespace multibody {
namespace benchmarks {
namespace pendulum {
/// This class is used to store the numerical parameters defining the model of
/// a simple pendulum with the method MakePendulumPlant().
/// Refer to this the documentation of this class's constructor for further
/// details on the parameters stored by this class and their default values.
class PendulumParameters {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(PendulumParameters)
/// Constructor used to initialize the physical parameters for a simple
/// pendulum model.
///
/// @param mass
/// Value of the mass of the pendulum's point mass [kg].
/// @param length
/// Length of the massless rod connecting the point mass to the world [m].
/// @param damping
/// The joint's damping in N⋅m⋅s.
/// @param gravity
/// Gravitational constant (m/s²).
PendulumParameters(
double mass = 1.0,
double length = 0.5,
double damping = 0.1,
double gravity = 9.81) :
mass_(mass),
length_(length),
damping_(damping),
g_(gravity) {}
// getters for pendulum parameters
double m() const { return mass_; }
double l() const { return length_; }
double damping() const { return damping_; }
double g() const { return g_; }
// Radius of the sphere used to visualize the point mass
double point_mass_radius() const { return 0.025; }
// Radius of the cylinder used to visualize the massless rod
double massless_rod_radius() const { return 0.007; }
// getters for modeling elements' names
const std::string& body_name() const { return body_name_; }
const std::string& pin_joint_name() const { return pin_joint_name_; }
const std::string& actuator_name() const { return actuator_name_; }
private:
// Helper method for NaN initialization.
static constexpr double nan() {
return std::numeric_limits<double>::quiet_NaN();
}
// The physical parameters of the model. They are initialized with NaN for a
// quick detection of uninitialized values.
double mass_{nan()}, // In kilograms.
length_{nan()}, // In meters.
damping_{nan()}, // Damping in N⋅m⋅s.
g_{nan()}; // In m/s².
// Modeling elements' names.
std::string body_name_{"PointMass"};
std::string pin_joint_name_{"PinJoint"};
std::string actuator_name_{"PinJointActuator"};
};
/// This method makes a MultibodyPlant model of an idealized pendulum with a
/// point mass at the end of a massless rigid rod.
/// The pendulum oscillates in the x-z plane with its revolute axis coincident
/// with the y-axis. Gravity points downwards in the -z axis direction.
///
/// The parameters of the plant are:
///
/// - mass: the mass of the idealized point mass.
/// - length: the length of the massless rod on which the mass is suspended.
/// - gravity: the acceleration of gravity.
///
/// The simple pendulum is a canonical dynamical system as described in <a
/// href="http://underactuated.csail.mit.edu/underactuated.html?chapter=pend">
/// Chapter 2 of Underactuated Robotics</a>.
///
/// @param[in] default_parameters
/// Default parameters of the model set at construction. Refer to the
/// documentation of PendulumParameters for further details.
/// @param scene_graph
/// If a SceneGraph is provided with this argument, this factory method
/// will register the new multibody plant to be a source for that geometry
/// system and it will also register geometry for visualization.
/// If this argument is omitted, no geometry will be registered.
std::unique_ptr<MultibodyPlant<double>> MakePendulumPlant(
const PendulumParameters& default_parameters = PendulumParameters(),
geometry::SceneGraph<double>* scene_graph = nullptr);
} // namespace pendulum
} // namespace benchmarks
} // namespace multibody
} // namespace drake