-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
planar_gripper_lcm.h
256 lines (219 loc) · 8.61 KB
/
planar_gripper_lcm.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
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
#pragma once
/// @file
/// This file contains classes dealing with sending/receiving
/// LCM messages related to the planar gripper.
/// TODO(rcory) Create doxygen system diagrams for the classes below.
///
/// All (q, v) state vectors in this file are of the format
/// (joint_positions, joint_velocities).
#include <memory>
#include <vector>
#include "drake/common/drake_copyable.h"
#include "drake/common/eigen_types.h"
#include "drake/lcmt_planar_gripper_command.hpp"
#include "drake/lcmt_planar_gripper_status.hpp"
#include "drake/systems/framework/event_status.h"
#include "drake/systems/framework/leaf_system.h"
namespace drake {
namespace examples {
namespace planar_gripper {
using systems::OutputPort;
using systems::InputPort;
// By default the planar gripper has 3 fingers.
constexpr int kGripperDefaultNumFingers = 3;
// This is rather arbitrary, for now.
// TODO(rcory) Refine this value once the planner comes online.
constexpr double kGripperLcmStatusPeriod = 0.010;
/// Handles lcmt_planar_gripper_command messages from a LcmSubscriberSystem.
///
/// This system has one abstract valued input port which expects a
/// Value object templated on type `lcmt_planar_gripper_command`.
///
/// This system has two output ports. The first reports the commanded position
/// and velocity for all joints, and the second reports the commanded joint
/// torques.
///
/// @system
/// name: GripperCommandDecoder
/// input_ports:
/// - lcmt_planar_gripper_command
/// output_ports:
/// - state
/// - torques
/// @endsystem
class GripperCommandDecoder : public systems::LeafSystem<double> {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(GripperCommandDecoder)
/// Constructor.
/// @param num_fingers The total number of fingers used on the planar-gripper.
explicit GripperCommandDecoder(int num_fingers = kGripperDefaultNumFingers);
/// Sets the initial position of the controlled gripper prior to any
/// commands being received. @p x contains the starting position.
/// This position will be the commanded position (with zero
/// velocity) until a position message is received. If this
/// function is not called, the starting position will be the zero
/// configuration.
void set_initial_position(
systems::Context<double>* context,
const Eigen::Ref<const VectorX<double>> x) const;
const systems::OutputPort<double>& get_state_output_port() const {
DRAKE_DEMAND(state_output_port_ != nullptr);
return *state_output_port_;
}
const systems::OutputPort<double>& get_torques_output_port() const {
DRAKE_DEMAND(torques_output_port_ != nullptr);
return *torques_output_port_;
}
private:
void OutputStateCommand(const systems::Context<double>& context,
systems::BasicVector<double>* output) const;
void OutputTorqueCommand(const systems::Context<double>& context,
systems::BasicVector<double>* output) const;
/// Event handler of the periodic discrete state update.
systems::EventStatus UpdateDiscreteState(
const systems::Context<double>& context,
systems::DiscreteValues<double>* discrete_state) const;
const int num_fingers_;
const int num_joints_;
const OutputPort<double>* state_output_port_{};
const OutputPort<double>* torques_output_port_{};
};
/// Creates and outputs lcmt_planar_gripper_command messages.
///
/// This system has two vector-valued input ports containing the
/// desired position and velocity in the first port, and commanded torque on the
/// second port.
///
/// This system has one abstract valued output port that contains a
/// Value object templated on type `lcmt_planar_gripper_command`. Note
/// that this system does not actually send this message on an LCM
/// channel. To send the message, the output of this system should be
/// connected to an input port of a systems::lcm::LcmPublisherSystem
/// that accepts a Value object templated on type
/// `lcmt_planar_gripper_command`.
///
/// @system
/// name: GripperCommandEncoder
/// input_ports:
/// - state
/// - torque
/// output_ports:
/// - lcmt_gripper_command
/// @endsystem
class GripperCommandEncoder : public systems::LeafSystem<double> {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(GripperCommandEncoder)
/// Constructor.
/// @param num_joints The total number of fingers used on the planar-gripper.
explicit GripperCommandEncoder(int num_fingers = kGripperDefaultNumFingers);
const systems::InputPort<double>& get_state_input_port() const {
DRAKE_DEMAND(state_input_port_ != nullptr);
return *state_input_port_;
}
const systems::InputPort<double>& get_torques_input_port() const {
DRAKE_DEMAND(torques_input_port_ != nullptr);
return *torques_input_port_;
}
private:
void OutputCommand(const systems::Context<double>& context,
lcmt_planar_gripper_command* output) const;
const int num_fingers_;
const int num_joints_;
const InputPort<double>* state_input_port_{};
const InputPort<double>* torques_input_port_{};
};
/// Handles lcmt_planar_gripper_status messages from a LcmSubscriberSystem.
///
/// This system has one abstract valued input port which expects a
/// Value object templated on type `lcmt_planar_gripper_status`.
///
/// This system has two vector valued output ports which report
/// measured position and velocity (state) as well as fingertip forces (fy, fz).
///
/// All ports will continue to output their initial state (typically
/// zero) until a message is received.
///
/// @system
/// name: GripperStatusDecoder
/// input_ports:
/// - lcmt_planar_gripper_status
/// output_ports:
/// - state
/// - fingertip_force
/// @endsystem
class GripperStatusDecoder : public systems::LeafSystem<double> {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(GripperStatusDecoder)
/// Constructor.
/// @param num_fingers The total number of fingers used on the planar-gripper.
explicit GripperStatusDecoder(int num_fingers = kGripperDefaultNumFingers);
const systems::OutputPort<double>& get_state_output_port() const {
DRAKE_DEMAND(state_output_port_ != nullptr);
return *state_output_port_;
}
const systems::OutputPort<double>& get_force_output_port() const {
DRAKE_DEMAND(force_output_port_ != nullptr);
return *force_output_port_;
}
private:
void OutputStateStatus(const systems::Context<double>& context,
systems::BasicVector<double>* output) const;
void OutputForceStatus(const systems::Context<double>& context,
systems::BasicVector<double>* output) const;
/// Event handler of the periodic discrete state update.
systems::EventStatus UpdateDiscreteState(
const systems::Context<double>& context,
systems::DiscreteValues<double>* discrete_state) const;
const int num_fingers_;
const int num_joints_;
const int num_tip_forces_;
const OutputPort<double>* state_output_port_{};
const OutputPort<double>* force_output_port_{};
};
/// Creates and outputs lcmt_planar_gripper_status messages.
///
/// This system has two vector-valued input ports containing the
/// current position and velocity (state) as well as fingertip forces (fy, fz).
///
/// This system has one abstract valued output port that contains a
/// Value object templated on type `lcmt_planar_gripper_status`. Note that this
/// system does not actually send this message on an LCM channel. To send the
/// message, the output of this system should be connected to an input port of
/// a systems::lcm::LcmPublisherSystem that accepts a
/// Value object templated on type `lcmt_planar_gripper_status`.
///
/// @system
/// name: GripperStatusEncoder
/// input_ports:
/// - state
/// - fingertip_force
/// output_ports:
/// - lcmt_gripper_status
/// @endsystem
class GripperStatusEncoder : public systems::LeafSystem<double> {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(GripperStatusEncoder)
/// Constructor.
/// @param num_joints The total number of fingers used on the planar-gripper.
explicit GripperStatusEncoder(int num_fingers = kGripperDefaultNumFingers);
const systems::InputPort<double>& get_state_input_port() const {
DRAKE_DEMAND(state_input_port_ != nullptr);
return *state_input_port_;
}
const systems::InputPort<double>& get_force_input_port() const {
DRAKE_DEMAND(force_input_port_ != nullptr);
return *force_input_port_;
}
private:
// This is the calculator method for the output port.
void OutputStatus(const systems::Context<double>& context,
lcmt_planar_gripper_status* output) const;
const int num_fingers_;
const int num_joints_;
const int num_tip_forces_;
const InputPort<double>* state_input_port_{};
const InputPort<double>* force_input_port_{};
};
} // namespace planar_gripper
} // namespace examples
} // namespace drake