Skip to content

Commit

Permalink
IiwaCommandReceiver latches initial position until first command (Rob…
Browse files Browse the repository at this point in the history 
…otLocomotion#13540)

This adjusts the semantics of the recently added position_measured input
port to better match the actual control cabinet where the robot is held
stiffly in place, instead of allowing external forces to push it around.
  • Loading branch information
@jwnimmer-tri
jwnimmer-tri authored Jun 12, 2020
1 parent b1c0482 commit 8d92fae
Show file tree
Hide file tree
Showing 5 changed files with 237 additions and 104 deletions.
212 changes: 129 additions & 83 deletions manipulation/kuka_iiwa/iiwa_command_receiver.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,115 +14,161 @@ using systems::BasicVector;
using systems::Context;
using systems::DiscreteValues;
using systems::DiscreteUpdateEvent;

namespace {
std::unique_ptr<AbstractValue> MakeCommandMessage() {
return std::make_unique<Value<lcmt_iiwa_command>>();
}
} // 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<double> 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<lcmt_iiwa_command>());
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<double>(VectorXd::Zero(num_joints)))};
position_measured_or_param_ = &DeclareCacheEntry(
"position_measured_or_param", BasicVector<double>(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<double>(num_joints),
[this](const Context<double>& context, BasicVector<double>* output) {
output->SetFromVector(this->input_position(context));
});
DeclareVectorOutputPort(
&IiwaCommandReceiver::CalcPositionOutput,
{defaulted_command_->ticket()});

commanded_torque_output_ = &DeclareVectorOutputPort(
"torque", BasicVector<double>(num_joints),
[this](const Context<double>& context, BasicVector<double>* output) {
output->SetFromVector(this->input_torque(context));
});
&IiwaCommandReceiver::CalcTorqueOutput,
{defaulted_command_->ticket()});
}

using InPort = systems::InputPort<double>;
const InPort& IiwaCommandReceiver::get_message_input_port() const {
return LeafSystem<double>::get_input_port(0);
}
const InPort& IiwaCommandReceiver::get_position_measured_input_port() const {
return LeafSystem<double>::get_input_port(1);
void IiwaCommandReceiver::set_initial_position(
Context<double>* context, const Eigen::Ref<const VectorXd>& q) const {
context->get_mutable_numeric_parameter(initial_state_param_).SetFromVector(q);
}
using OutPort = systems::OutputPort<double>;
const OutPort& IiwaCommandReceiver::get_commanded_position_output_port() const {
return LeafSystem<double>::get_output_port(0);

void IiwaCommandReceiver::CalcPositionMeasuredOrParam(
const systems::Context<double>& context,
systems::BasicVector<double>* 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<double>::get_output_port(1);

void IiwaCommandReceiver::LatchInitialPosition(
const Context<double>& context,
DiscreteValues<double>* 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<BasicVector<double>>(context));
}

void IiwaCommandReceiver::set_initial_position(
Context<double>* context, const Eigen::Ref<const VectorXd>& q) const {
DRAKE_THROW_UNLESS(q.size() == num_joints_);
context->get_mutable_numeric_parameter(0).SetFromVector(q);
void IiwaCommandReceiver::LatchInitialPosition(
systems::Context<double>* 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<double>& 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<double>& context,
systems::CompositeEventCollection<double>* events, double* time) const {
// We do not support events other than our own message timing events.
LeafSystem<double>::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<lcmt_iiwa_command>(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<DiscreteUpdateEvent<double>>(
[this](const Context<double>& event_context,
const DiscreteUpdateEvent<double>&,
DiscreteValues<double>* 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<double>& context, lcmt_iiwa_command* result) const {
// Copy the input value into our tentative result.
*result = message_input_->Eval<lcmt_iiwa_command>(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<double>& latch_is_set = context.get_discrete_state(
latched_position_measured_is_set_);
const BasicVector<double>& default_position =
latch_is_set[0]
? context.get_discrete_state(latched_position_measured_)
: position_measured_or_param_->Eval<BasicVector<double>>(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<double>& context, BasicVector<double>* output) const {
const auto& message = defaulted_command_->Eval<lcmt_iiwa_command>(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<double>& context) const {
const auto& message = groomed_input_->Eval<lcmt_iiwa_command>(context);
return Eigen::Map<const VectorXd>(
output->SetFromVector(Eigen::Map<const VectorXd>(
message.joint_position.data(),
message.joint_position.size());
message.joint_position.size()));
}

IiwaCommandReceiver::MapVectorXd IiwaCommandReceiver::input_torque(
const Context<double>& context) const {
const auto& message = groomed_input_->Eval<lcmt_iiwa_command>(context);
return Eigen::Map<const VectorXd>(
void IiwaCommandReceiver::CalcTorqueOutput(
const Context<double>& context, BasicVector<double>* output) const {
const auto& message = defaulted_command_->Eval<lcmt_iiwa_command>(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<const VectorXd>(
message.joint_torque.data(),
message.joint_torque.size());
message.joint_torque.size()));
}

} // namespace kuka_iiwa
Expand Down
76 changes: 60 additions & 16 deletions manipulation/kuka_iiwa/iiwa_command_receiver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double> {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(IiwaCommandReceiver)
Expand All @@ -54,12 +60,29 @@ class IiwaCommandReceiver : public systems::LeafSystem<double> {
void set_initial_position(systems::Context<double>* context,
const Eigen::Ref<const Eigen::VectorXd>& 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<double>* context) const;

/// @name Named accessors for this System's input and output ports.
//@{
const systems::InputPort<double>& get_message_input_port() const;
const systems::InputPort<double>& get_position_measured_input_port() const;
const systems::OutputPort<double>& get_commanded_position_output_port() const;
const systems::OutputPort<double>& get_commanded_torque_output_port() const;
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_;
}
//@}

DRAKE_DEPRECATED("2020-09-01", "Use get_message_input_port() instead.")
Expand All @@ -68,13 +91,34 @@ class IiwaCommandReceiver : public systems::LeafSystem<double> {
}

private:
using MapVectorXd = Eigen::Map<const Eigen::VectorXd>;
MapVectorXd input_position(const systems::Context<double>&) const;
MapVectorXd input_torque(const systems::Context<double>&) const;
void CalcInput(const systems::Context<double>&, lcmt_iiwa_command*) const;
void DoCalcNextUpdateTime(
const systems::Context<double>&,
systems::CompositeEventCollection<double>*, double*) const override;

void CalcPositionMeasuredOrParam(
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::CacheEntry* groomed_input_{};
const systems::InputPort<double>* message_input_{};
const systems::InputPort<double>* 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<double>* commanded_position_output_{};
const systems::OutputPort<double>* commanded_torque_output_{};

// Deprecated.
systems::NumericParameterIndex initial_state_param_;
};

} // namespace kuka_iiwa
Expand Down
4 changes: 2 additions & 2 deletions manipulation/kuka_iiwa/iiwa_command_sender.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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}
/// }
///
Expand Down
4 changes: 2 additions & 2 deletions manipulation/kuka_iiwa/iiwa_status_sender.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
Loading

0 comments on commit 8d92fae

Please sign in to comment.