diff --git a/examples/kinova_jaco_arm/BUILD.bazel b/examples/kinova_jaco_arm/BUILD.bazel index bed0bbeb7f17..dfb1cae470f6 100644 --- a/examples/kinova_jaco_arm/BUILD.bazel +++ b/examples/kinova_jaco_arm/BUILD.bazel @@ -52,6 +52,7 @@ drake_cc_binary( "//multibody/plant", "//systems/analysis:simulator", "//systems/controllers:inverse_dynamics_controller", + "//systems/primitives:demultiplexer", "@gflags", ], ) diff --git a/examples/kinova_jaco_arm/jaco_simulation.cc b/examples/kinova_jaco_arm/jaco_simulation.cc index bf2fe1c565a5..0299a23efee2 100644 --- a/examples/kinova_jaco_arm/jaco_simulation.cc +++ b/examples/kinova_jaco_arm/jaco_simulation.cc @@ -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::infinity(), "Number of seconds to simulate."); @@ -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 { @@ -98,7 +100,13 @@ int DoMain() { "KINOVA_JACO_COMMAND", lcm)); auto command_receiver = builder.AddSystem(); builder.Connect(command_sub->get_output_port(), - command_receiver->get_input_port()); + command_receiver->get_message_input_port()); + auto plant_state_demux = builder.AddSystem( + 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(), @@ -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); diff --git a/manipulation/kinova_jaco/jaco_command_receiver.cc b/manipulation/kinova_jaco/jaco_command_receiver.cc index f13ee71d6cfd..3055c4b1e6c3 100644 --- a/manipulation/kinova_jaco/jaco_command_receiver.cc +++ b/manipulation/kinova_jaco/jaco_command_receiver.cc @@ -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 MakeCommandMessage() { - return std::make_unique>(); -} -} // 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 default_position( + + message_input_ = &DeclareAbstractInputPort( + "lcmt_jaco_command", Value()); + 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(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, @@ -46,10 +54,68 @@ JacoCommandReceiver::JacoCommandReceiver(int num_joints, int num_fingers) }); } +void JacoCommandReceiver::CalcPositionMeasuredOrZero( + const Context& context, + BasicVector* result) const { + if (position_measured_input_->HasValue(context)) { + result->SetFromVector(position_measured_input_->Eval(context)); + } else { + result->SetZero(); + } +} + +void JacoCommandReceiver::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_value(bool_index)[0] = 1.0; + result->get_mutable_vector(value_index).SetFrom( + position_measured_or_zero_->Eval>(context)); +} + +void JacoCommandReceiver::LatchInitialPosition( + Context* context) const { + DRAKE_THROW_UNLESS(context != nullptr); + LatchInitialPosition(*context, &context->get_mutable_discrete_state()); +} + void JacoCommandReceiver::set_initial_position( Context* context, const Eigen::Ref& q) const { DRAKE_THROW_UNLESS(q.size() == num_joints_ + num_fingers_); - context->get_mutable_numeric_parameter(0).SetFromVector(q); + + DiscreteValues* 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& context, + 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; + } + + // 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( + [this](const Context& event_context, + const DiscreteUpdateEvent&, + DiscreteValues* next_values) { + LatchInitialPosition(event_context, next_values); + })); } // Returns (in "result") the command message input, or if a message has not @@ -57,28 +123,34 @@ void JacoCommandReceiver::set_initial_position( // user). The result will always have num_joints_ positions and torques. void JacoCommandReceiver::CalcInput( const Context& 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(context); + // Copy the input value into our tentative result. + *result = get_message_input_port().Eval(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& 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_zero_->Eval>(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(); diff --git a/manipulation/kinova_jaco/jaco_command_receiver.h b/manipulation/kinova_jaco/jaco_command_receiver.h index 5b69479a2831..abc5ceac5f3e 100644 --- a/manipulation/kinova_jaco/jaco_command_receiver.h +++ b/manipulation/kinova_jaco/jaco_command_receiver.h @@ -5,6 +5,7 @@ #include #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" @@ -20,7 +21,7 @@ namespace kinova_jaco { /// receive the message, the input of this system should be connected to a /// LcmSubscriberSystem::Make(). /// -/// 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 @@ -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 { public: @@ -41,21 +53,60 @@ class JacoCommandReceiver : public systems::LeafSystem { 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* context) const; + + /// @name Named accessors for this System's input and output ports. + //@{ + const systems::InputPort& get_message_input_port() const { + return *message_input_; + } + const systems::InputPort& 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* context, const Eigen::Ref& q) const; + DRAKE_DEPRECATED("2022-06-01", "Use get_message_input_port() instead.") + const systems::InputPort& get_input_port() const { + return get_message_input_port(); + } + private: Eigen::VectorXd input_state(const systems::Context&) const; void CalcInput(const systems::Context&, lcmt_jaco_command*) const; + void DoCalcNextUpdateTime( + const systems::Context&, + systems::CompositeEventCollection*, double*) const final; + void CalcPositionMeasuredOrZero( + const systems::Context&, systems::BasicVector*) const; + + // Copies the current "position measured" input (or zero if not connected) + // into the @p result. + void LatchInitialPosition( + const systems::Context&, + systems::DiscreteValues* result) const; + const int num_joints_; const int num_fingers_; + const systems::InputPort* message_input_{}; + const systems::InputPort* 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_{}; }; diff --git a/manipulation/kinova_jaco/test/jaco_command_receiver_test.cc b/manipulation/kinova_jaco/test/jaco_command_receiver_test.cc index 88e018b3e022..047b2d1be824 100644 --- a/manipulation/kinova_jaco/test/jaco_command_receiver_test.cc +++ b/manipulation/kinova_jaco/test/jaco_command_receiver_test.cc @@ -22,7 +22,8 @@ class JacoCommandReceiverTest : public testing::Test { // For use only by our constructor. systems::FixedInputPortValue& FixInput() { - return dut_.get_input_port().FixValue(&context_, lcmt_jaco_command{}); + return dut_.get_message_input_port().FixValue( + &context_, lcmt_jaco_command{}); } // Test cases should call this to set the DUT's input value. @@ -43,11 +44,14 @@ class JacoCommandReceiverTest : public testing::Test { systems::FixedInputPortValue& fixed_input_; }; -TEST_F(JacoCommandReceiverTest, AcceptanceTest) { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + +TEST_F(JacoCommandReceiverTest, DeprecatedInitialPositionTest) { constexpr int total_dof = kJacoDefaultArmNumJoints + kJacoDefaultArmNumFingers; - // Check that the commanded pose starts out at zero + // Check that the commanded pose starts out at zero. EXPECT_TRUE(CompareMatrices(state(), VectorXd::Zero(total_dof * 2))); // Check that we can set a different initial position. @@ -56,6 +60,23 @@ TEST_F(JacoCommandReceiverTest, AcceptanceTest) { EXPECT_TRUE(CompareMatrices(state().head(total_dof), q0)); EXPECT_TRUE(CompareMatrices(state().tail(total_dof), VectorXd::Zero(total_dof))); +} + +#pragma GCC diagnostic pop + +TEST_F(JacoCommandReceiverTest, AcceptanceTest) { + constexpr int total_dof = + kJacoDefaultArmNumJoints + kJacoDefaultArmNumFingers; + + // Check that the commanded pose starts out at zero. + EXPECT_TRUE(CompareMatrices(state(), VectorXd::Zero(total_dof * 2))); + + // Check that we can set a different initial position. + const VectorXd q0 = VectorXd::LinSpaced(total_dof, 0.1, 0.2); + dut_.get_position_measured_input_port().FixValue(&context_, q0); + EXPECT_TRUE(CompareMatrices(state().head(total_dof), q0)); + EXPECT_TRUE(CompareMatrices(state().tail(total_dof), + VectorXd::Zero(total_dof))); // Check that a real command trumps the initial position. const VectorXd q1_arm = @@ -91,6 +112,69 @@ TEST_F(JacoCommandReceiverTest, AcceptanceTest) { v1_finger * kFingerSdkToUrdf)); } +TEST_F(JacoCommandReceiverTest, AcceptanceTestWithLatching) { + constexpr int total_dof = + kJacoDefaultArmNumJoints + kJacoDefaultArmNumFingers; + + // Check that the commanded pose starts out at zero. + EXPECT_TRUE(CompareMatrices(state(), VectorXd::Zero(total_dof * 2))); + + // Check that we can set a different initial position. + const VectorXd q0 = VectorXd::LinSpaced(total_dof, 0.1, 0.2); + dut_.get_position_measured_input_port().FixValue(&context_, q0); + EXPECT_TRUE(CompareMatrices(state().head(total_dof), q0)); + EXPECT_TRUE(CompareMatrices(state().tail(total_dof), + VectorXd::Zero(total_dof))); + + // Prior to any update events, changes to position_measured feed through. + const VectorXd q1 = VectorXd::LinSpaced(total_dof, 0.2, 0.3); + dut_.get_position_measured_input_port().FixValue(&context_, q1); + EXPECT_TRUE(CompareMatrices(state().head(total_dof), q1)); + EXPECT_TRUE(CompareMatrices(state().tail(total_dof), + VectorXd::Zero(total_dof))); + + // Once an update event occurs, further changes to position_measured have no + // effect. + const VectorXd q2 = VectorXd::LinSpaced(total_dof, 0.3, 0.4); + dut_.get_position_measured_input_port().FixValue(&context_, q2); + EXPECT_TRUE(CompareMatrices(state().head(total_dof), q2)); + EXPECT_TRUE(CompareMatrices(state().tail(total_dof), + VectorXd::Zero(total_dof))); + + // Check that a real command trumps the initial position. + const VectorXd q3_arm = + VectorXd::LinSpaced(kJacoDefaultArmNumJoints, 1.3, 1.4); + const VectorXd v3_arm = + VectorXd::LinSpaced(kJacoDefaultArmNumJoints, 1.5, 1.6); + const VectorXd q3_finger = + VectorXd::LinSpaced(kJacoDefaultArmNumFingers, 2.3, 2.4) * + kFingerUrdfToSdk; + const VectorXd v3_finger = + VectorXd::LinSpaced(kJacoDefaultArmNumFingers, 2.5, 2.6) * + kFingerUrdfToSdk; + lcmt_jaco_command command{}; + command.utime = 0; + command.num_joints = kJacoDefaultArmNumJoints; + command.joint_position = {q3_arm.data(), q3_arm.data() + q3_arm.size()}; + command.joint_velocity = {v3_arm.data(), v3_arm.data() + v3_arm.size()}; + command.num_fingers = kJacoDefaultArmNumFingers; + command.finger_position = + {q3_finger.data(), q3_finger.data() + q3_finger.size()}; + command.finger_velocity = + {v3_finger.data(), v3_finger.data() + v3_finger.size()}; + SetInput(command); + EXPECT_TRUE(CompareMatrices(state().head(kJacoDefaultArmNumJoints), q3_arm)); + EXPECT_TRUE(CompareMatrices(state().segment( + total_dof, kJacoDefaultArmNumJoints), v3_arm)); + + EXPECT_TRUE(CompareMatrices(state().segment( + kJacoDefaultArmNumJoints, kJacoDefaultArmNumFingers), q3_finger * + kFingerSdkToUrdf)); + EXPECT_TRUE( + CompareMatrices(state().tail(kJacoDefaultArmNumFingers), + v3_finger * kFingerSdkToUrdf)); +} + } // namespace } // namespace kinova_jaco } // namespace manipulation