diff --git a/manipulation/kuka_iiwa/iiwa_command_receiver.cc b/manipulation/kuka_iiwa/iiwa_command_receiver.cc index 255778c64eca..4a0481c4bb6a 100644 --- a/manipulation/kuka_iiwa/iiwa_command_receiver.cc +++ b/manipulation/kuka_iiwa/iiwa_command_receiver.cc @@ -14,115 +14,161 @@ using systems::BasicVector; using systems::Context; using systems::DiscreteValues; using systems::DiscreteUpdateEvent; - -namespace { -std::unique_ptr MakeCommandMessage() { - return std::make_unique>(); -} -} // namespace +using systems::NumericParameterIndex; IiwaCommandReceiver::IiwaCommandReceiver(int num_joints) : num_joints_(num_joints) { - // Our parameter stores the position when no message has been received. - const BasicVector default_position(VectorXd::Zero(num_joints)); - const systems::NumericParameterIndex param{ - DeclareNumericParameter(default_position)}; - DRAKE_DEMAND(param == 0); // We're depending on that elsewhere. - - DeclareAbstractInputPort("lcmt_iiwa_command", *MakeCommandMessage()); - DeclareInputPort("position_measured", systems::kVectorValued, num_joints_); - groomed_input_ = &DeclareCacheEntry( - "groomed_input", &IiwaCommandReceiver::CalcInput, - {all_input_ports_ticket(), numeric_parameter_ticket(param)}); - - DeclareVectorOutputPort( + DRAKE_THROW_UNLESS(num_joints > 0); + + message_input_ = &DeclareAbstractInputPort( + "lcmt_iiwa_command", Value()); + position_measured_input_ = &DeclareInputPort( + "position_measured", systems::kVectorValued, num_joints); + + // This (deprecated) parameter stores a value to use for the initial position + // iff the position_measured input is not connected. The cache entry + // provides either the input (iff connected) or else the parameter value. + // TODO(jwnimmer-tri) On 2020-09-01 when we remove the set_initial_position + // method, we should remove this parameter and cache entry and just use the + // input port directly (and use zero when the port is not connected). + initial_state_param_ = NumericParameterIndex{ + DeclareNumericParameter(BasicVector(VectorXd::Zero(num_joints)))}; + position_measured_or_param_ = &DeclareCacheEntry( + "position_measured_or_param", BasicVector(num_joints), + &IiwaCommandReceiver::CalcPositionMeasuredOrParam, + {position_measured_input_->ticket(), + numeric_parameter_ticket(initial_state_param_)}); + + // When a simulation begins, we will latch position_measured_or_param into a + // state variable, so that we will hold that pose until the first message is + // received. Prior to that event, we continue to use the unlatched value. + latched_position_measured_is_set_ = DeclareDiscreteState(VectorXd::Zero(1)); + latched_position_measured_ = DeclareDiscreteState(VectorXd::Zero(num_joints)); + defaulted_command_ = &DeclareCacheEntry( + "defaulted_command", &IiwaCommandReceiver::CalcDefaultedCommand, + {message_input_->ticket(), + discrete_state_ticket(latched_position_measured_is_set_), + discrete_state_ticket(latched_position_measured_), + position_measured_or_param_->ticket()}); + + commanded_position_output_ = &DeclareVectorOutputPort( "position", BasicVector(num_joints), - [this](const Context& context, BasicVector* output) { - output->SetFromVector(this->input_position(context)); - }); - DeclareVectorOutputPort( + &IiwaCommandReceiver::CalcPositionOutput, + {defaulted_command_->ticket()}); + + commanded_torque_output_ = &DeclareVectorOutputPort( "torque", BasicVector(num_joints), - [this](const Context& context, BasicVector* output) { - output->SetFromVector(this->input_torque(context)); - }); + &IiwaCommandReceiver::CalcTorqueOutput, + {defaulted_command_->ticket()}); } -using InPort = systems::InputPort; -const InPort& IiwaCommandReceiver::get_message_input_port() const { - return LeafSystem::get_input_port(0); -} -const InPort& IiwaCommandReceiver::get_position_measured_input_port() const { - return LeafSystem::get_input_port(1); +void IiwaCommandReceiver::set_initial_position( + Context* context, const Eigen::Ref& q) const { + context->get_mutable_numeric_parameter(initial_state_param_).SetFromVector(q); } -using OutPort = systems::OutputPort; -const OutPort& IiwaCommandReceiver::get_commanded_position_output_port() const { - return LeafSystem::get_output_port(0); + +void IiwaCommandReceiver::CalcPositionMeasuredOrParam( + const systems::Context& context, + systems::BasicVector* result) const { + if (position_measured_input_->HasValue(context)) { + result->SetFromVector(position_measured_input_->Eval(context)); + } else { + result->SetFrom(context.get_numeric_parameter(initial_state_param_)); + } } -const OutPort& IiwaCommandReceiver::get_commanded_torque_output_port() const { - return LeafSystem::get_output_port(1); + +void IiwaCommandReceiver::LatchInitialPosition( + const Context& context, + DiscreteValues* result) const { + const auto& bool_index = latched_position_measured_is_set_; + const auto& value_index = latched_position_measured_; + result->get_mutable_vector(bool_index).get_mutable_value()[0] = 1.0; + result->get_mutable_vector(value_index).SetFrom( + position_measured_or_param_->Eval>(context)); } -void IiwaCommandReceiver::set_initial_position( - Context* context, const Eigen::Ref& q) const { - DRAKE_THROW_UNLESS(q.size() == num_joints_); - context->get_mutable_numeric_parameter(0).SetFromVector(q); +void IiwaCommandReceiver::LatchInitialPosition( + systems::Context* context) const { + DRAKE_THROW_UNLESS(context != nullptr); + LatchInitialPosition(*context, &context->get_mutable_discrete_state()); } -// Returns (in "result") the command message input, or if a message has not -// been received yet returns the a fallback value. The result always has -// num_joints_ positions and torques. -void IiwaCommandReceiver::CalcInput( - const Context& context, lcmt_iiwa_command* result) const { - if (!get_message_input_port().HasValue(context)) { - throw std::logic_error("IiwaCommandReceiver has no input connected"); +// TODO(jwnimmer-tri) This is quite a cumbersome syntax to use for declaring a +// "now" event. We should try to consolidate it with other similar uses within +// the source tree. Relates to #11403 somewhat. +void IiwaCommandReceiver::DoCalcNextUpdateTime( + const systems::Context& context, + systems::CompositeEventCollection* events, double* time) const { + // We do not support events other than our own message timing events. + LeafSystem::DoCalcNextUpdateTime(context, events, time); + DRAKE_THROW_UNLESS(events->HasEvents() == false); + DRAKE_THROW_UNLESS(std::isinf(*time)); + + // If we have a latched position already, then we do not have any updates. + if (context.get_discrete_state(0).get_value()[0] != 0.0) { + return; } - // Copies the input value into our tentative result. - *result = get_message_input_port().Eval(context); + // Schedule a discrete update event at now to latch the current position. + *time = context.get_time(); + auto& discrete_events = events->get_mutable_discrete_update_events(); + discrete_events.add_event(std::make_unique>( + [this](const Context& event_context, + const DiscreteUpdateEvent&, + DiscreteValues* next_values) { + LatchInitialPosition(event_context, next_values); + })); +} - // If we haven't received a non-default message yet, use nominal values - // instead. N.B. This works due to lcm::Serializer<>::CreateDefaultValue() - // using value-initialization. +void IiwaCommandReceiver::CalcDefaultedCommand( + const systems::Context& context, lcmt_iiwa_command* result) const { + // Copy the input value into our tentative result. + *result = message_input_->Eval(context); + + // If we haven't received a message yet, then fall back to the default + // position. if (lcm::AreLcmMessagesEqual(*result, lcmt_iiwa_command{})) { - const VectorXd positions = - get_position_measured_input_port().HasValue(context) ? - get_position_measured_input_port().Eval(context) : - context.get_numeric_parameter(0).get_value(); - result->num_joints = positions.size(); - result->joint_position = - {positions.data(), positions.data() + positions.size()}; + const BasicVector& latch_is_set = context.get_discrete_state( + latched_position_measured_is_set_); + const BasicVector& default_position = + latch_is_set[0] + ? context.get_discrete_state(latched_position_measured_) + : position_measured_or_param_->Eval>(context); + const VectorXd vec = default_position.CopyToVector(); + result->num_joints = vec.size(); + result->joint_position = {vec.data(), vec.data() + vec.size()}; } +} - // Sanity check the joint sizes. If torques were not sent, pad with zeros. - if (result->num_joints != num_joints_) { +void IiwaCommandReceiver::CalcPositionOutput( + const Context& context, BasicVector* output) const { + const auto& message = defaulted_command_->Eval(context); + if (message.num_joints != num_joints_) { throw std::runtime_error(fmt::format( "IiwaCommandReceiver expected num_joints = {}, but received {}", - num_joints_, result->num_joints)); - } - if (result->num_torques == 0) { - result->num_torques = num_joints_; - result->joint_torque.resize(num_joints_, 0.0); - } else if (result->num_torques != num_joints_) { - throw std::runtime_error(fmt::format( - "IiwaCommandReceiver expected num_torques = {}, but received {}", - num_joints_, result->num_torques)); + num_joints_, message.num_joints)); } -} - -IiwaCommandReceiver::MapVectorXd IiwaCommandReceiver::input_position( - const Context& context) const { - const auto& message = groomed_input_->Eval(context); - return Eigen::Map( + output->SetFromVector(Eigen::Map( message.joint_position.data(), - message.joint_position.size()); + message.joint_position.size())); } -IiwaCommandReceiver::MapVectorXd IiwaCommandReceiver::input_torque( - const Context& context) const { - const auto& message = groomed_input_->Eval(context); - return Eigen::Map( +void IiwaCommandReceiver::CalcTorqueOutput( + const Context& context, BasicVector* output) const { + const auto& message = defaulted_command_->Eval(context); + if (message.num_torques == 0) { + // If torques were not sent, use zeros. + output->SetZero(); + return; + } + if (message.num_torques != num_joints_) { + throw std::runtime_error(fmt::format( + "IiwaCommandReceiver expected num_torques = {}, but received {}", + num_joints_, message.num_torques)); + } + output->SetFromVector(Eigen::Map( message.joint_torque.data(), - message.joint_torque.size()); + message.joint_torque.size())); } } // namespace kuka_iiwa diff --git a/manipulation/kuka_iiwa/iiwa_command_receiver.h b/manipulation/kuka_iiwa/iiwa_command_receiver.h index 06e00670f82d..86c99ba0fd53 100644 --- a/manipulation/kuka_iiwa/iiwa_command_receiver.h +++ b/manipulation/kuka_iiwa/iiwa_command_receiver.h @@ -24,20 +24,26 @@ namespace kuka_iiwa { /// /// It has one required input port, "lcmt_iiwa_command". /// -/// It offers an optional "position_measured" input, which (when connected) -/// will be used to set the output position when no command messages have been -/// received yet. When this port is not connected, the default position will -/// be all zeros. -/// /// It has two output ports: one for the commanded position for each joint, and /// one for commanded additional feedforward joint torque. /// -/// @system { IiwaCommandReceiver, +/// @system{IiwaCommandReceiver, /// @input_port{lcmt_iiwa_command} -/// @input_port{position_measured} +/// @input_port{position_measured (optional)}, /// @output_port{position} /// @output_port{torque} /// } +/// +/// @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 { public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(IiwaCommandReceiver) @@ -54,12 +60,29 @@ class IiwaCommandReceiver : public systems::LeafSystem { void set_initial_position(systems::Context* context, const Eigen::Ref& q) const; + /// (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* context) const; + /// @name Named accessors for this System's input and output ports. //@{ - const systems::InputPort& get_message_input_port() const; - const systems::InputPort& get_position_measured_input_port() const; - const systems::OutputPort& get_commanded_position_output_port() const; - const systems::OutputPort& get_commanded_torque_output_port() const; + const systems::InputPort& get_message_input_port() const { + return *message_input_; + } + const systems::InputPort& get_position_measured_input_port() const { + return *position_measured_input_; + } + const systems::OutputPort& get_commanded_position_output_port() + const { + return *commanded_position_output_; + } + const systems::OutputPort& get_commanded_torque_output_port() const { + return *commanded_torque_output_; + } //@} DRAKE_DEPRECATED("2020-09-01", "Use get_message_input_port() instead.") @@ -68,13 +91,34 @@ class IiwaCommandReceiver : public systems::LeafSystem { } private: - using MapVectorXd = Eigen::Map; - MapVectorXd input_position(const systems::Context&) const; - MapVectorXd input_torque(const systems::Context&) const; - void CalcInput(const systems::Context&, lcmt_iiwa_command*) const; + void DoCalcNextUpdateTime( + const systems::Context&, + systems::CompositeEventCollection*, double*) const override; + + void CalcPositionMeasuredOrParam( + const systems::Context&, systems::BasicVector*) const; + void LatchInitialPosition( + const systems::Context&, + systems::DiscreteValues*) const; + void CalcDefaultedCommand( + const systems::Context&, lcmt_iiwa_command*) const; + void CalcPositionOutput( + const systems::Context&, systems::BasicVector*) const; + void CalcTorqueOutput( + const systems::Context&, systems::BasicVector*) const; const int num_joints_; - const systems::CacheEntry* groomed_input_{}; + const systems::InputPort* message_input_{}; + const systems::InputPort* position_measured_input_{}; + const systems::CacheEntry* position_measured_or_param_{}; + systems::DiscreteStateIndex latched_position_measured_is_set_; + systems::DiscreteStateIndex latched_position_measured_; + const systems::CacheEntry* defaulted_command_{}; + const systems::OutputPort* commanded_position_output_{}; + const systems::OutputPort* commanded_torque_output_{}; + + // Deprecated. + systems::NumericParameterIndex initial_state_param_; }; } // namespace kuka_iiwa diff --git a/manipulation/kuka_iiwa/iiwa_command_sender.h b/manipulation/kuka_iiwa/iiwa_command_sender.h index 2b69566e43ae..5b3f75883cb6 100644 --- a/manipulation/kuka_iiwa/iiwa_command_sender.h +++ b/manipulation/kuka_iiwa/iiwa_command_sender.h @@ -23,9 +23,9 @@ namespace kuka_iiwa { /// /// This system has one abstract-valued output port of type lcmt_iiwa_command. /// -/// @system { +/// @system{IiwaCommandSender, /// @input_port{position} -/// @input_port{torque (optional)} +/// @input_port{torque (optional)}, /// @output_port{lcmt_iiwa_command} /// } /// diff --git a/manipulation/kuka_iiwa/iiwa_status_sender.h b/manipulation/kuka_iiwa/iiwa_status_sender.h index f4fd8443b4c8..1747a1587474 100644 --- a/manipulation/kuka_iiwa/iiwa_status_sender.h +++ b/manipulation/kuka_iiwa/iiwa_status_sender.h @@ -36,13 +36,13 @@ namespace kuka_iiwa { /// This system is presently only used in simulation. The robot hardware drivers /// publish directly to LCM and do not make use of this system. /// -/// @system { IiwaStatusSender, +/// @system{IiwaStatusSender, /// @input_port{position_commanded} /// @input_port{position_measured} /// @input_port{velocity_estimated (optional)} /// @input_port{torque_commanded} /// @input_port{torque_measured (optional)} -/// @input_port{torque_external (optional)} +/// @input_port{torque_external (optional)}, /// @output_port{lcmt_iiwa_status} /// } /// @see `lcmt_iiwa_status.lcm` for additional documentation. diff --git a/manipulation/kuka_iiwa/test/iiwa_command_receiver_test.cc b/manipulation/kuka_iiwa/test/iiwa_command_receiver_test.cc index 6e2df1f09462..9f0fea23bbc7 100644 --- a/manipulation/kuka_iiwa/test/iiwa_command_receiver_test.cc +++ b/manipulation/kuka_iiwa/test/iiwa_command_receiver_test.cc @@ -51,12 +51,16 @@ class IiwaCommandReceiverTest : public testing::Test { systems::FixedInputPortValue& fixed_input_; }; -TEST_F(IiwaCommandReceiverTest, AcceptanceTest) { +TEST_F(IiwaCommandReceiverTest, AcceptanceTestWithoutMeasuredPositionInput) { // When no message has been received and *no* position measurement is // connected, the command is all zeros. const VectorXd zero = VectorXd::Zero(N); EXPECT_TRUE(CompareMatrices(position(), zero)); EXPECT_TRUE(CompareMatrices(torque(), zero)); +} + +TEST_F(IiwaCommandReceiverTest, AcceptanceTestWithMeasuredPositionInput) { + const VectorXd zero = VectorXd::Zero(N); // When no message has been received and a measurement *is* connected, the // command is to hold at the current position. @@ -85,6 +89,45 @@ TEST_F(IiwaCommandReceiverTest, AcceptanceTest) { EXPECT_TRUE(CompareMatrices(torque(), t1)); } +TEST_F(IiwaCommandReceiverTest, AcceptanceTestWithLatching) { + const VectorXd zero = VectorXd::Zero(N); + + // When no message has been received and a measurement *is* connected, the + // command is to hold at the current position. + const VectorXd q0 = VectorXd::LinSpaced(N, 0.0, 0.1); + dut_.get_position_measured_input_port().FixValue(&context_, q0); + EXPECT_TRUE(CompareMatrices(position(), q0)); + EXPECT_TRUE(CompareMatrices(torque(), zero)); + + // Prior to any update events, changes to position_measured feed through. + const VectorXd q1 = VectorXd::LinSpaced(N, 0.1, 0.2); + dut_.get_position_measured_input_port().FixValue(&context_, q1); + EXPECT_TRUE(CompareMatrices(position(), q1)); + EXPECT_TRUE(CompareMatrices(torque(), zero)); + + // Once an update event occurs, further changes to position_measured have no + // effect. + dut_.LatchInitialPosition(&context_); + const VectorXd q2 = VectorXd::LinSpaced(N, 0.3, 0.4); + dut_.get_position_measured_input_port().FixValue(&context_, q2); + EXPECT_TRUE(CompareMatrices(position(), q1)); + EXPECT_TRUE(CompareMatrices(torque(), zero)); + + // Check that a real command trumps the initial position. + // First, try with empty torques. + const VectorXd q3 = VectorXd::LinSpaced(N, 0.4, 0.5); + const VectorXd t3 = VectorXd::LinSpaced(N, 0.5, 0.6); + lcmt_iiwa_command command{}; + command.utime = 0; + command.num_joints = N; + command.joint_position = {q3.data(), q3.data() + q3.size()}; + command.num_torques = N; + command.joint_torque = {t3.data(), t3.data() + t3.size()}; + SetInput(command); + EXPECT_TRUE(CompareMatrices(position(), q3)); + EXPECT_TRUE(CompareMatrices(torque(), t3)); +} + #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" TEST_F(IiwaCommandReceiverTest, DeprecatedAcceptanceTest) {