-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
acrobot_geometry.h
83 lines (74 loc) · 3 KB
/
acrobot_geometry.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
#pragma once
#include "drake/examples/acrobot/gen/acrobot_params.h"
#include "drake/geometry/scene_graph.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/systems/framework/leaf_system.h"
namespace drake {
namespace examples {
namespace acrobot {
/// Expresses an AcrobotPlant's geometry to a SceneGraph.
///
/// @system
/// name: AcrobotGeometry
/// input_ports:
/// - state
/// output_ports:
/// - geometry_pose
/// @endsystem
///
/// This class has no public constructor; instead use the AddToBuilder() static
/// method to create and add it to a DiagramBuilder directly.
///
/// @ingroup acrobot_systems
class AcrobotGeometry final : public systems::LeafSystem<double> {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(AcrobotGeometry);
~AcrobotGeometry() final;
/// Creates, adds, and connects an AcrobotGeometry system into the given
/// `builder`. Both the `acrobot_state.get_system()` and `scene_graph`
/// systems must have been added to the given `builder` already.
///
/// @param acrobot_params sets the parameters of the geometry registered
/// with `scene_graph`.
///
/// The `scene_graph` pointer is not retained by the %AcrobotGeometry
/// system. The return value pointer is an alias of the new
/// %AcrobotGeometry system that is owned by the `builder`.
static const AcrobotGeometry* AddToBuilder(
systems::DiagramBuilder<double>* builder,
const systems::OutputPort<double>& acrobot_state_port,
const AcrobotParams<double>& acrobot_params,
geometry::SceneGraph<double>* scene_graph);
/// Creates, adds, and connects an AcrobotGeometry system into the given
/// `builder`. Both the `acrobot_state.get_system()` and `scene_graph`
/// systems must have been added to the given `builder` already.
///
/// Acrobot parameters are set to their default values.
///
/// The `scene_graph` pointer is not retained by the %AcrobotGeometry
/// system. The return value pointer is an alias of the new
/// %AcrobotGeometry system that is owned by the `builder`.
static const AcrobotGeometry* AddToBuilder(
systems::DiagramBuilder<double>* builder,
const systems::OutputPort<double>& acrobot_state_port,
geometry::SceneGraph<double>* scene_graph) {
return AddToBuilder(builder, acrobot_state_port, AcrobotParams<double>(),
scene_graph);
}
private:
AcrobotGeometry(const AcrobotParams<double>& acrobot_params,
geometry::SceneGraph<double>*);
void OutputGeometryPose(const systems::Context<double>&,
geometry::FramePoseVector<double>*) const;
// Geometry source identifier for this system to interact with SceneGraph.
geometry::SourceId source_id_{};
// The frames for the two links.
geometry::FrameId upper_link_frame_id_{};
geometry::FrameId lower_link_frame_id_{};
// Local copy of the parameter required for the forward kinematics (the
// length of the upper link).
const double l1_;
};
} // namespace acrobot
} // namespace examples
} // namespace drake