Skip to content

Commit

Permalink
Make JacoCommandReceiver use an input port for initial position (Robo…
Browse files Browse the repository at this point in the history
…tLocomotion#16483)

This mirrors the approach used in IiwaCommandReceiver.

This PR doesn't fully modernize the Jaco LCM classes to mimic the
behavior of the iiwa.  I have a longer-lived development branch for
that effort.  I'm moving this change to it's own commit because it's
needed more urgently.
  • Loading branch information
sammy-tri authored Feb 2, 2022
1 parent 8e285fa commit 44e4c39
Show file tree
Hide file tree
Showing 5 changed files with 252 additions and 39 deletions.
1 change: 1 addition & 0 deletions examples/kinova_jaco_arm/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,7 @@ drake_cc_binary(
"//multibody/plant",
"//systems/analysis:simulator",
"//systems/controllers:inverse_dynamics_controller",
"//systems/primitives:demultiplexer",
"@gflags",
],
)
Expand Down
13 changes: 9 additions & 4 deletions examples/kinova_jaco_arm/jaco_simulation.cc
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
#include "drake/systems/lcm/lcm_interface_system.h"
#include "drake/systems/lcm/lcm_publisher_system.h"
#include "drake/systems/lcm/lcm_subscriber_system.h"
#include "drake/systems/primitives/demultiplexer.h"

DEFINE_double(simulation_sec, std::numeric_limits<double>::infinity(),
"Number of seconds to simulate.");
Expand All @@ -43,6 +44,7 @@ using drake::multibody::Body;
using drake::multibody::MultibodyPlant;
using drake::multibody::Parser;
using drake::systems::controllers::InverseDynamicsController;
using drake::systems::Demultiplexer;

namespace drake {
namespace examples {
Expand Down Expand Up @@ -98,7 +100,13 @@ int DoMain() {
"KINOVA_JACO_COMMAND", lcm));
auto command_receiver = builder.AddSystem<JacoCommandReceiver>();
builder.Connect(command_sub->get_output_port(),
command_receiver->get_input_port());
command_receiver->get_message_input_port());
auto plant_state_demux = builder.AddSystem<Demultiplexer>(
2 * num_positions, num_positions);
builder.Connect(jaco_plant->get_state_output_port(jaco_id),
plant_state_demux->get_input_port());
builder.Connect(plant_state_demux->get_output_port(0),
command_receiver->get_position_measured_input_port());
builder.Connect(command_receiver->get_output_port(),
jaco_controller->get_input_port_desired_state());
builder.Connect(jaco_controller->get_output_port_control(),
Expand Down Expand Up @@ -133,9 +141,6 @@ int DoMain() {
initial_position(5) = 4.49;
initial_position(6) = 5.03;

command_receiver->set_initial_position(
&diagram->GetMutableSubsystemContext(*command_receiver, &root_context),
initial_position);
jaco_plant->SetPositions(
&diagram->GetMutableSubsystemContext(*jaco_plant, &root_context),
initial_position);
Expand Down
124 changes: 98 additions & 26 deletions manipulation/kinova_jaco/jaco_command_receiver.cc
Original file line number Diff line number Diff line change
Expand Up @@ -11,33 +11,41 @@ namespace kinova_jaco {

using Eigen::VectorXd;
using systems::BasicVector;
using systems::CompositeEventCollection;
using systems::Context;
using systems::DiscreteValues;
using systems::DiscreteUpdateEvent;

namespace {
std::unique_ptr<AbstractValue> MakeCommandMessage() {
return std::make_unique<Value<lcmt_jaco_command>>();
}
} // namespace
using systems::kVectorValued;

JacoCommandReceiver::JacoCommandReceiver(int num_joints, int num_fingers)
: num_joints_(num_joints),
num_fingers_(num_fingers) {
// Our parameters store the position when no message has been received.
const BasicVector<double> default_position(

message_input_ = &DeclareAbstractInputPort(
"lcmt_jaco_command", Value<lcmt_jaco_command>());
position_measured_input_ = &DeclareInputPort(
"position_measured", kVectorValued, num_joints + num_fingers);

// This cache entry provides either the input (iff connected) or else zero.
position_measured_or_zero_ = &DeclareCacheEntry(
"position_measured_or_zero",
BasicVector<double>(num_joints + num_fingers),
&JacoCommandReceiver::CalcPositionMeasuredOrZero,
{position_measured_input_->ticket()});

// When a simulation begins, we will latch positions 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 + num_fingers));
const systems::NumericParameterIndex arm_param{
DeclareNumericParameter(default_position)};
DRAKE_DEMAND(arm_param == 0); // We're depending on that elsewhere.

// Our input ports are mutually exclusive; exactly one connected input port
// feeds our cache entry. The computation may be dependent on the above
// parameter as well.
DeclareAbstractInputPort("lcmt_jaco_command", *MakeCommandMessage());

groomed_input_ = &DeclareCacheEntry(
"groomed_input", &JacoCommandReceiver::CalcInput,
{all_input_ports_ticket(), numeric_parameter_ticket(arm_param), });
{message_input_->ticket(),
discrete_state_ticket(latched_position_measured_is_set_),
discrete_state_ticket(latched_position_measured_),
position_measured_or_zero_->ticket()});

DeclareVectorOutputPort(
"state", (num_joints + num_fingers) * 2,
Expand All @@ -46,39 +54,103 @@ JacoCommandReceiver::JacoCommandReceiver(int num_joints, int num_fingers)
});
}

void JacoCommandReceiver::CalcPositionMeasuredOrZero(
const Context<double>& context,
BasicVector<double>* result) const {
if (position_measured_input_->HasValue(context)) {
result->SetFromVector(position_measured_input_->Eval(context));
} else {
result->SetZero();
}
}

void JacoCommandReceiver::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_value(bool_index)[0] = 1.0;
result->get_mutable_vector(value_index).SetFrom(
position_measured_or_zero_->Eval<BasicVector<double>>(context));
}

void JacoCommandReceiver::LatchInitialPosition(
Context<double>* context) const {
DRAKE_THROW_UNLESS(context != nullptr);
LatchInitialPosition(*context, &context->get_mutable_discrete_state());
}

void JacoCommandReceiver::set_initial_position(
Context<double>* context, const Eigen::Ref<const VectorXd>& q) const {
DRAKE_THROW_UNLESS(q.size() == num_joints_ + num_fingers_);
context->get_mutable_numeric_parameter(0).SetFromVector(q);

DiscreteValues<double>* values = &context->get_mutable_discrete_state();
const auto& bool_index = latched_position_measured_is_set_;
const auto& value_index = latched_position_measured_;
values->get_mutable_value(bool_index)[0] = 1.0;
values->get_mutable_vector(value_index).SetFromVector(q);
}

// 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 JacoCommandReceiver::DoCalcNextUpdateTime(
const Context<double>& context,
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;
}

// 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.AddEvent(DiscreteUpdateEvent<double>(
[this](const Context<double>& event_context,
const DiscreteUpdateEvent<double>&,
DiscreteValues<double>* next_values) {
LatchInitialPosition(event_context, next_values);
}));
}

// Returns (in "result") the command message input, or if a message has not
// been received yet returns the initial command (as optionally set by the
// user). The result will always have num_joints_ positions and torques.
void JacoCommandReceiver::CalcInput(
const Context<double>& context, lcmt_jaco_command* result) const {
if (!get_input_port().HasValue(context)) {
if (!get_message_input_port().HasValue(context)) {
throw std::logic_error("JacoCommandReceiver has no input connected");
}

// Copies the (sole) input value, converting from JacoCommand if necessary.
*result = get_input_port().Eval<lcmt_jaco_command>(context);
// Copy the input value into our tentative result.
*result = get_message_input_port().Eval<lcmt_jaco_command>(context);

// If we haven't received a non-default message yet, use the initial command.
// N.B. This works due to lcm::Serializer<>::CreateDefaultValue() using
// value-initialization.
if (lcm::AreLcmMessagesEqual(*result, lcmt_jaco_command{})) {
const VectorXd arm_param = context.get_numeric_parameter(0).get_value();
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_zero_->Eval<BasicVector<double>>(context);

result->num_joints = num_joints_;
result->joint_position =
{arm_param.data(), arm_param.data() + num_joints_};
const VectorXd& vec = default_position.value();
result->joint_position = {vec.data(), vec.data() + num_joints_};
result->joint_velocity.resize(num_joints_, 0);

result->num_fingers = num_fingers_;
if (num_fingers_) {
result->finger_position =
{arm_param.data() + num_joints_,
arm_param.data() + num_joints_ + num_fingers_};
{vec.data() + num_joints_,
vec.data() + num_joints_ + num_fingers_};
result->finger_velocity.resize(num_fingers_, 0);
} else {
result->finger_position.clear();
Expand Down
63 changes: 57 additions & 6 deletions manipulation/kinova_jaco/jaco_command_receiver.h
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
#include <vector>

#include "drake/common/drake_copyable.h"
#include "drake/common/drake_deprecated.h"
#include "drake/common/eigen_types.h"
#include "drake/lcmt_jaco_command.hpp"
#include "drake/manipulation/kinova_jaco/jaco_constants.h"
Expand All @@ -20,7 +21,7 @@ namespace kinova_jaco {
/// receive the message, the input of this system should be connected to a
/// LcmSubscriberSystem::Make<drake::lcmt_jaco_command>().
///
/// It has one input port, "lcmt_jaco_command".
/// It has one required input port, "lcmt_jaco_command".
///
/// This system has a single output port which contains the commanded position
/// and velocity for each joint. Finger velocities will be translated from
Expand All @@ -31,8 +32,19 @@ namespace kinova_jaco {
/// name: JacoCommandReceiver
/// input_ports:
/// - lcmt_jaco_command
/// - position_measured (optional)
/// output_ports:
/// - state
///
/// @par Output prior to receiving a valid lcmt_jaco_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.
///
/// @endsystem
class JacoCommandReceiver : public systems::LeafSystem<double> {
public:
Expand All @@ -41,21 +53,60 @@ class JacoCommandReceiver : public systems::LeafSystem<double> {
JacoCommandReceiver(int num_joints = kJacoDefaultArmNumJoints,
int num_fingers = kJacoDefaultArmNumFingers);

/// Sets the initial commanded position of the controlled jaco prior to any
/// command messages being received. If this function is not called, the
/// starting position will be the zero configuration. Finger positions
/// should be specified as values appropriate for the Jaco description (see
/// jaco_constants.h), not in Kinova SDK values.
/// (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_;
}
//@}

DRAKE_DEPRECATED("2022-06-01",
"To provide position commands prior to receiving the first message, "
"connect the position_measured ports instead of setting this "
"parameter")
void set_initial_position(
systems::Context<double>* context,
const Eigen::Ref<const Eigen::VectorXd>& q) const;

DRAKE_DEPRECATED("2022-06-01", "Use get_message_input_port() instead.")
const systems::InputPort<double>& get_input_port() const {
return get_message_input_port();
}

private:
Eigen::VectorXd input_state(const systems::Context<double>&) const;
void CalcInput(const systems::Context<double>&, lcmt_jaco_command*) const;

void DoCalcNextUpdateTime(
const systems::Context<double>&,
systems::CompositeEventCollection<double>*, double*) const final;
void CalcPositionMeasuredOrZero(
const systems::Context<double>&, systems::BasicVector<double>*) const;

// Copies the current "position measured" input (or zero if not connected)
// into the @p result.
void LatchInitialPosition(
const systems::Context<double>&,
systems::DiscreteValues<double>* result) const;

const int num_joints_;
const int num_fingers_;
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* groomed_input_{};
};

Expand Down
Loading

0 comments on commit 44e4c39

Please sign in to comment.