-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
iiwa_command_receiver.h
110 lines (99 loc) · 4.2 KB
/
iiwa_command_receiver.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 <memory>
#include <string>
#include <vector>
#include "drake/common/drake_copyable.h"
#include "drake/common/eigen_types.h"
#include "drake/lcmt_iiwa_command.hpp"
#include "drake/manipulation/kuka_iiwa/iiwa_constants.h"
#include "drake/systems/framework/leaf_system.h"
#include "drake/systems/lcm/lcm_subscriber_system.h"
namespace drake {
namespace manipulation {
namespace kuka_iiwa {
/// Handles lcmt_iiwa_command message from a LcmSubscriberSystem.
///
/// Note that this system does not actually subscribe to an LCM channel. To
/// receive the message, the input of this system should be connected to a
/// LcmSubscriberSystem::Make<drake::lcmt_iiwa_command>().
///
/// It has one required input port, "lcmt_iiwa_command".
///
/// It has two output ports: one for the commanded position for each joint, and
/// one for commanded additional feedforward joint torque.
///
/// @system
/// name: IiwaCommandReceiver
/// input_ports:
/// - lcmt_iiwa_command
/// - position_measured (optional)
/// output_ports:
/// - position
/// - torque
/// @endsystem
///
/// @par Output prior to receiving a valid lcmt_iiwa_command message:
/// The "position" output initially feeds through from the "position_measured"
/// input port -- or if not connected, outputs zero. When discrete update
/// events are enabled (e.g., during a simulation), the system latches the
/// "position_measured" input into state during the first event, and the
/// "position" output comes from the latched state, no longer fed through from
/// the "position" input. Alternatively, the LatchInitialPosition() method is
/// available to achieve the same effect without using events.
/// @par
/// The "torque" output will always be a vector of zeros.
class IiwaCommandReceiver : public systems::LeafSystem<double> {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(IiwaCommandReceiver)
explicit IiwaCommandReceiver(int num_joints = kIiwaArmNumJoints);
/// (Advanced.) Copies the current "position_measured" input (or zero if not
/// connected) into Context state, and changes the behavior of the "position"
/// output to produce the latched state if no message has been received yet.
/// The latching already happens automatically during the first discrete
/// update event (e.g., when using a Simulator); this method exists for use
/// when not already using a Simulator or other special cases.
void LatchInitialPosition(systems::Context<double>* context) const;
/// @name Named accessors for this System's input and output ports.
//@{
const systems::InputPort<double>& get_message_input_port() const {
return *message_input_;
}
const systems::InputPort<double>& get_position_measured_input_port() const {
return *position_measured_input_;
}
const systems::OutputPort<double>& get_commanded_position_output_port()
const {
return *commanded_position_output_;
}
const systems::OutputPort<double>& get_commanded_torque_output_port() const {
return *commanded_torque_output_;
}
//@}
private:
void DoCalcNextUpdateTime(
const systems::Context<double>&,
systems::CompositeEventCollection<double>*, double*) const override;
void CalcPositionMeasuredOrZero(
const systems::Context<double>&, systems::BasicVector<double>*) const;
void LatchInitialPosition(
const systems::Context<double>&,
systems::DiscreteValues<double>*) const;
void CalcDefaultedCommand(
const systems::Context<double>&, lcmt_iiwa_command*) const;
void CalcPositionOutput(
const systems::Context<double>&, systems::BasicVector<double>*) const;
void CalcTorqueOutput(
const systems::Context<double>&, systems::BasicVector<double>*) const;
const int num_joints_;
const systems::InputPort<double>* message_input_{};
const systems::InputPort<double>* position_measured_input_{};
const systems::CacheEntry* position_measured_or_zero_{};
systems::DiscreteStateIndex latched_position_measured_is_set_;
systems::DiscreteStateIndex latched_position_measured_;
const systems::CacheEntry* defaulted_command_{};
const systems::OutputPort<double>* commanded_position_output_{};
const systems::OutputPort<double>* commanded_torque_output_{};
};
} // namespace kuka_iiwa
} // namespace manipulation
} // namespace drake