From 974bbc0d4df125cc30ce28340d6cf15b48def1a7 Mon Sep 17 00:00:00 2001 From: Peter Date: Thu, 9 Oct 2025 15:20:49 -0400 Subject: [PATCH 01/12] passing tests --- bindings/generated_docstrings/BUILD.bazel | 1 + bindings/pydrake/manipulation/BUILD.bazel | 10 + .../pydrake/manipulation/manipulation_py.cc | 1 + .../pydrake/manipulation/manipulation_py.h | 3 + .../manipulation_py_franka_panda.cc | 148 ++++++++++ .../manipulation/test/franka_panda_test.py | 110 +++++++ manipulation/franka_panda/BUILD.bazel | 117 ++++++++ .../franka_panda/panda_command_receiver.cc | 269 ++++++++++++++++++ .../franka_panda/panda_command_receiver.h | 121 ++++++++ .../franka_panda/panda_command_sender.cc | 109 +++++++ .../franka_panda/panda_command_sender.h | 65 +++++ manipulation/franka_panda/panda_constants.h | 9 + .../franka_panda/panda_status_receiver.cc | 96 +++++++ .../franka_panda/panda_status_receiver.h | 68 +++++ .../franka_panda/panda_status_sender.cc | 145 ++++++++++ .../franka_panda/panda_status_sender.h | 68 +++++ .../test/panda_command_receiver_test.cc | 168 +++++++++++ .../test/panda_command_sender_test.cc | 142 +++++++++ .../test/panda_status_receiver_test.cc | 137 +++++++++ .../test/panda_status_sender_test.cc | 99 +++++++ 20 files changed, 1886 insertions(+) create mode 100644 bindings/pydrake/manipulation/manipulation_py_franka_panda.cc create mode 100644 bindings/pydrake/manipulation/test/franka_panda_test.py create mode 100644 manipulation/franka_panda/BUILD.bazel create mode 100644 manipulation/franka_panda/panda_command_receiver.cc create mode 100644 manipulation/franka_panda/panda_command_receiver.h create mode 100644 manipulation/franka_panda/panda_command_sender.cc create mode 100644 manipulation/franka_panda/panda_command_sender.h create mode 100644 manipulation/franka_panda/panda_constants.h create mode 100644 manipulation/franka_panda/panda_status_receiver.cc create mode 100644 manipulation/franka_panda/panda_status_receiver.h create mode 100644 manipulation/franka_panda/panda_status_sender.cc create mode 100644 manipulation/franka_panda/panda_status_sender.h create mode 100644 manipulation/franka_panda/test/panda_command_receiver_test.cc create mode 100644 manipulation/franka_panda/test/panda_command_sender_test.cc create mode 100644 manipulation/franka_panda/test/panda_status_receiver_test.cc create mode 100644 manipulation/franka_panda/test/panda_status_sender_test.cc diff --git a/bindings/generated_docstrings/BUILD.bazel b/bindings/generated_docstrings/BUILD.bazel index 904016d9737a..e9560f1b9154 100644 --- a/bindings/generated_docstrings/BUILD.bazel +++ b/bindings/generated_docstrings/BUILD.bazel @@ -27,6 +27,7 @@ _SUBDIRS = [ "geometry/render_vtk", "lcm", "manipulation/kuka_iiwa", + "manipulation/franka_panda", "manipulation/schunk_wsg", "manipulation/util", "math", diff --git a/bindings/pydrake/manipulation/BUILD.bazel b/bindings/pydrake/manipulation/BUILD.bazel index 7383b1e16876..9ce4b31ab823 100644 --- a/bindings/pydrake/manipulation/BUILD.bazel +++ b/bindings/pydrake/manipulation/BUILD.bazel @@ -24,6 +24,7 @@ drake_pybind_library( name = "manipulation", cc_deps = [ "//bindings/generated_docstrings:manipulation_kuka_iiwa", + "//bindings/generated_docstrings:manipulation_franka_panda", "//bindings/generated_docstrings:manipulation_schunk_wsg", "//bindings/generated_docstrings:manipulation_util", "//bindings/pydrake/common:eigen_pybind", @@ -36,6 +37,7 @@ drake_pybind_library( "manipulation_py.h", "manipulation_py.cc", "manipulation_py_kuka_iiwa.cc", + "manipulation_py_franka_panda.cc", "manipulation_py_schunk_wsg.cc", "manipulation_py_util.cc", ], @@ -66,6 +68,14 @@ drake_py_unittest( ], ) +drake_py_unittest( + name = "franka_panda_test", + deps = [ + ":manipulation", + "//bindings/pydrake/systems", + ], +) + drake_py_unittest( name = "schunk_wsg_test", data = [ diff --git a/bindings/pydrake/manipulation/manipulation_py.cc b/bindings/pydrake/manipulation/manipulation_py.cc index 5d17f6ce2f79..dac1d98b5a4d 100644 --- a/bindings/pydrake/manipulation/manipulation_py.cc +++ b/bindings/pydrake/manipulation/manipulation_py.cc @@ -12,6 +12,7 @@ PYBIND11_MODULE(manipulation, m) { // The order of these calls matters. Some modules rely on prior definitions. internal::DefineManipulationKukaIiwa(m); + internal::DefineManipulationFrankaPanda(m); internal::DefineManipulationSchunkWsg(m); internal::DefineManipulationUtil(m); diff --git a/bindings/pydrake/manipulation/manipulation_py.h b/bindings/pydrake/manipulation/manipulation_py.h index f622f2f4a420..ad42fe6e01c6 100644 --- a/bindings/pydrake/manipulation/manipulation_py.h +++ b/bindings/pydrake/manipulation/manipulation_py.h @@ -15,6 +15,9 @@ namespace internal { /* Defines bindings per manipulation_py_kuka_iiwa.cc. */ void DefineManipulationKukaIiwa(py::module m); +/* Defines bindings per manipulation_py_franka_panda.cc. */ +void DefineManipulationFrankaPanda(py::module m); + /* Defines bindings per manipulation_py_schunk_wsg.cc. */ void DefineManipulationSchunkWsg(py::module m); diff --git a/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc b/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc new file mode 100644 index 000000000000..18cabb7645fa --- /dev/null +++ b/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc @@ -0,0 +1,148 @@ +#include "drake/bindings/generated_docstrings/manipulation_franka_panda.h" +#include "drake/bindings/pydrake/common/serialize_pybind.h" +#include "drake/bindings/pydrake/manipulation/manipulation_py.h" +#include "drake/bindings/pydrake/pydrake_pybind.h" +#include "drake/manipulation/franka_panda/panda_command_receiver.h" +#include "drake/manipulation/franka_panda/panda_command_sender.h" +#include "drake/manipulation/franka_panda/panda_constants.h" +#include "drake/manipulation/franka_panda/panda_status_receiver.h" +#include "drake/manipulation/franka_panda/panda_status_sender.h" + +namespace drake { +namespace pydrake { +namespace internal { + +using systems::LeafSystem; + +void DefineManipulationFrankaPanda(py::module m) { + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::manipulation; + constexpr auto& doc = + pydrake_doc_manipulation_franka_panda.drake.manipulation; + + // Constants. + m.attr("kPandaArmNumJoints") = kPandaArmNumJoints; + + { + using Class = PandaCommandReceiver; + constexpr auto& cls_doc = doc.PandaCommandReceiver; + py::class_>(m, "PandaCommandReceiver", cls_doc.doc) + .def(py::init(), py::arg("num_joints") = kPandaArmNumJoints, + py::arg("control_mode"), cls_doc.ctor.doc) + .def("LatchInitialPosition", + overload_cast_explicit*>( + &Class::LatchInitialPosition), + py::arg("context"), cls_doc.LatchInitialPosition.doc) + .def("get_message_input_port", &Class::get_message_input_port, + py_rvp::reference_internal, cls_doc.get_message_input_port.doc) + .def("get_position_measured_input_port", + &Class::get_position_measured_input_port, + py_rvp::reference_internal, + cls_doc.get_position_measured_input_port.doc) + .def("get_commanded_position_output_port", + &Class::get_commanded_position_output_port, + py_rvp::reference_internal, + cls_doc.get_commanded_position_output_port.doc) + .def("get_commanded_velocity_output_port", + &Class::get_commanded_velocity_output_port, + py_rvp::reference_internal, + cls_doc.get_commanded_velocity_output_port.doc) + .def("get_commanded_torque_output_port", + &Class::get_commanded_torque_output_port, + py_rvp::reference_internal, + cls_doc.get_commanded_torque_output_port.doc); + } + + { + using Class = PandaCommandSender; + constexpr auto& cls_doc = doc.PandaCommandSender; + py::class_>(m, "PandaCommandSender", cls_doc.doc) + .def(py::init(), py::arg("num_joints") = kPandaArmNumJoints, + py::arg("control_mode"), cls_doc.ctor.doc) + .def("get_position_input_port", &Class::get_position_input_port, + py_rvp::reference_internal, cls_doc.get_position_input_port.doc) + .def("get_velocity_input_port", &Class::get_velocity_input_port, + py_rvp::reference_internal, cls_doc.get_velocity_input_port.doc) + .def("get_torque_input_port", &Class::get_torque_input_port, + py_rvp::reference_internal, cls_doc.get_torque_input_port.doc); + } + + { + using Class = PandaStatusReceiver; + constexpr auto& cls_doc = doc.PandaStatusReceiver; + py::class_>(m, "PandaStatusReceiver", cls_doc.doc) + .def(py::init(), py::arg("num_joints") = kPandaArmNumJoints, + cls_doc.ctor.doc) + .def("get_position_commanded_output_port", + &Class::get_position_commanded_output_port, + py_rvp::reference_internal, + cls_doc.get_position_commanded_output_port.doc) + .def("get_position_measured_output_port", + &Class::get_position_measured_output_port, + py_rvp::reference_internal, + cls_doc.get_position_measured_output_port.doc) + .def("get_velocity_commanded_output_port", + &Class::get_velocity_commanded_output_port, + py_rvp::reference_internal, + cls_doc.get_velocity_commanded_output_port.doc) + .def("get_velocity_measured_output_port", + &Class::get_velocity_measured_output_port, + py_rvp::reference_internal, + cls_doc.get_velocity_measured_output_port.doc) + .def("get_acceleration_commanded_output_port", + &Class::get_acceleration_commanded_output_port, + py_rvp::reference_internal, + cls_doc.get_acceleration_commanded_output_port.doc) + .def("get_torque_commanded_output_port", + &Class::get_torque_commanded_output_port, + py_rvp::reference_internal, + cls_doc.get_torque_commanded_output_port.doc) + .def("get_torque_measured_output_port", + &Class::get_torque_measured_output_port, py_rvp::reference_internal, + cls_doc.get_torque_measured_output_port.doc) + .def("get_torque_external_output_port", + &Class::get_torque_external_output_port, py_rvp::reference_internal, + cls_doc.get_torque_external_output_port.doc); + } + + { + using Class = PandaStatusSender; + constexpr auto& cls_doc = doc.PandaStatusSender; + py::class_>(m, "PandaStatusSender", cls_doc.doc) + .def(py::init(), py::arg("num_joints") = kPandaArmNumJoints, + cls_doc.ctor.doc) + .def("get_position_commanded_input_port", + &Class::get_position_commanded_input_port, + py_rvp::reference_internal, + cls_doc.get_position_commanded_input_port.doc) + .def("get_position_measured_input_port", + &Class::get_position_measured_input_port, + py_rvp::reference_internal, + cls_doc.get_position_measured_input_port.doc) + .def("get_velocity_commanded_input_port", + &Class::get_velocity_commanded_input_port, + py_rvp::reference_internal, + cls_doc.get_velocity_commanded_input_port.doc) + .def("get_velocity_measured_input_port", + &Class::get_velocity_measured_input_port, + py_rvp::reference_internal, + cls_doc.get_velocity_measured_input_port.doc) + .def("get_acceleration_commanded_input_port", + &Class::get_acceleration_commanded_input_port, + py_rvp::reference_internal, + cls_doc.get_acceleration_commanded_input_port.doc) + .def("get_torque_commanded_input_port", + &Class::get_torque_commanded_input_port, py_rvp::reference_internal, + cls_doc.get_torque_commanded_input_port.doc) + .def("get_torque_measured_input_port", + &Class::get_torque_measured_input_port, py_rvp::reference_internal, + cls_doc.get_torque_measured_input_port.doc) + .def("get_torque_external_input_port", + &Class::get_torque_external_input_port, py_rvp::reference_internal, + cls_doc.get_torque_external_input_port.doc); + } +} + +} // namespace internal +} // namespace pydrake +} // namespace drake \ No newline at end of file diff --git a/bindings/pydrake/manipulation/test/franka_panda_test.py b/bindings/pydrake/manipulation/test/franka_panda_test.py new file mode 100644 index 000000000000..7656220dcdb4 --- /dev/null +++ b/bindings/pydrake/manipulation/test/franka_panda_test.py @@ -0,0 +1,110 @@ +# -*- coding: utf-8 -*- + +import pydrake.manipulation as mut + +import unittest +import numpy as np + +from pydrake.lcmt_panda_command import lcmt_panda_command +from pydrake.lcmt_panda_status import lcmt_panda_status +from pydrake.systems.framework import ( + InputPort, + OutputPort, +) + + +class TestFrankaPanda(unittest.TestCase): + def test_constants(self): + self.assertEqual(mut.kPandaArmNumJoints, 7) + + def test_franka_panda_command_receiver(self): + # Test with position and torque control mode + control_mode = (lcmt_panda_status.CONTROL_MODE_POSITION | + lcmt_panda_status.CONTROL_MODE_TORQUE) + command_rec = mut.PandaCommandReceiver( + num_joints=mut.kPandaArmNumJoints, + control_mode=control_mode) + self.assertIsInstance( + command_rec.get_message_input_port(), InputPort) + self.assertIsInstance( + command_rec.get_position_measured_input_port(), InputPort) + self.assertIsInstance( + command_rec.get_commanded_position_output_port(), OutputPort) + self.assertIsInstance( + command_rec.get_commanded_torque_output_port(), OutputPort) + + # Test with position, velocity, and torque control mode + control_mode_all = (lcmt_panda_status.CONTROL_MODE_POSITION | + lcmt_panda_status.CONTROL_MODE_VELOCITY | + lcmt_panda_status.CONTROL_MODE_TORQUE) + command_rec_all = mut.PandaCommandReceiver( + num_joints=mut.kPandaArmNumJoints, + control_mode=control_mode_all) + self.assertIsInstance( + command_rec_all.get_commanded_velocity_output_port(), OutputPort) + + def test_franka_panda_command_sender(self): + # Test with position and torque control mode + control_mode = (lcmt_panda_status.CONTROL_MODE_POSITION | + lcmt_panda_status.CONTROL_MODE_TORQUE) + command_send = mut.PandaCommandSender( + num_joints=mut.kPandaArmNumJoints, + control_mode=control_mode) + self.assertIsInstance( + command_send.get_position_input_port(), InputPort) + self.assertIsInstance( + command_send.get_torque_input_port(), InputPort) + + # Test with position, velocity, and torque control mode + control_mode_all = (lcmt_panda_status.CONTROL_MODE_POSITION | + lcmt_panda_status.CONTROL_MODE_VELOCITY | + lcmt_panda_status.CONTROL_MODE_TORQUE) + command_send_all = mut.PandaCommandSender( + num_joints=mut.kPandaArmNumJoints, + control_mode=control_mode_all) + self.assertIsInstance( + command_send_all.get_velocity_input_port(), InputPort) + + def test_franka_panda_status_receiver(self): + status_rec = mut.PandaStatusReceiver() + self.assertIsInstance( + status_rec.get_position_commanded_output_port(), OutputPort) + self.assertIsInstance( + status_rec.get_position_measured_output_port(), OutputPort) + self.assertIsInstance( + status_rec.get_velocity_commanded_output_port(), OutputPort) + self.assertIsInstance( + status_rec.get_velocity_measured_output_port(), OutputPort) + self.assertIsInstance( + status_rec.get_acceleration_commanded_output_port(), OutputPort) + self.assertIsInstance( + status_rec.get_torque_commanded_output_port(), OutputPort) + self.assertIsInstance( + status_rec.get_torque_measured_output_port(), OutputPort) + self.assertIsInstance( + status_rec.get_torque_external_output_port(), OutputPort) + + # Constructor variant. + mut.PandaStatusReceiver(num_joints=mut.kPandaArmNumJoints) + + def test_franka_panda_status_sender(self): + status_send = mut.PandaStatusSender() + self.assertIsInstance( + status_send.get_position_commanded_input_port(), InputPort) + self.assertIsInstance( + status_send.get_position_measured_input_port(), InputPort) + self.assertIsInstance( + status_send.get_velocity_commanded_input_port(), InputPort) + self.assertIsInstance( + status_send.get_velocity_measured_input_port(), InputPort) + self.assertIsInstance( + status_send.get_acceleration_commanded_input_port(), InputPort) + self.assertIsInstance( + status_send.get_torque_commanded_input_port(), InputPort) + self.assertIsInstance( + status_send.get_torque_measured_input_port(), InputPort) + self.assertIsInstance( + status_send.get_torque_external_input_port(), InputPort) + + # Constructor variant. + mut.PandaStatusSender(num_joints=mut.kPandaArmNumJoints) \ No newline at end of file diff --git a/manipulation/franka_panda/BUILD.bazel b/manipulation/franka_panda/BUILD.bazel new file mode 100644 index 000000000000..909b96a58802 --- /dev/null +++ b/manipulation/franka_panda/BUILD.bazel @@ -0,0 +1,117 @@ +load("//tools/lint:lint.bzl", "add_lint_tests") +load( + "//tools/skylark:drake_cc.bzl", + "drake_cc_googletest", + "drake_cc_library", + "drake_cc_package_library", +) + +package( + default_visibility = ["//visibility:public"], +) + +drake_cc_package_library( + name = "franka_panda", + visibility = ["//visibility:public"], + deps = [ + ":panda_command_receiver", + ":panda_command_sender", + ":panda_constants", + ":panda_status_receiver", + ":panda_status_sender", + ], +) + +drake_cc_library( + name = "panda_constants", + hdrs = ["panda_constants.h"], +) + +drake_cc_library( + name = "panda_command_receiver", + srcs = ["panda_command_receiver.cc"], + hdrs = ["panda_command_receiver.h"], + deps = [ + ":panda_constants", + "//common:essential", + "//lcmtypes:lcmtypes_drake_cc", + "//systems/framework:leaf_system", + "//systems/lcm:lcm_pubsub_system", + ], +) + +drake_cc_library( + name = "panda_command_sender", + srcs = ["panda_command_sender.cc"], + hdrs = ["panda_command_sender.h"], + deps = [ + ":panda_constants", + "//common:essential", + "//lcmtypes:lcmtypes_drake_cc", + "//systems/framework:leaf_system", + ], +) + +drake_cc_library( + name = "panda_status_receiver", + srcs = ["panda_status_receiver.cc"], + hdrs = ["panda_status_receiver.h"], + deps = [ + ":panda_constants", + "//common:essential", + "//lcmtypes:lcmtypes_drake_cc", + "//systems/framework:leaf_system", + ], +) + +drake_cc_library( + name = "panda_status_sender", + srcs = ["panda_status_sender.cc"], + hdrs = ["panda_status_sender.h"], + deps = [ + ":panda_constants", + "//common:essential", + "//lcmtypes:lcmtypes_drake_cc", + "//systems/framework:leaf_system", + ], +) + +# === test/ === + +drake_cc_googletest( + name = "panda_command_receiver_test", + deps = [ + ":panda_command_receiver", + "//common/test_utilities:eigen_matrix_compare", + "//common/test_utilities:limit_malloc", + ], +) + +drake_cc_googletest( + name = "panda_command_sender_test", + deps = [ + ":panda_command_sender", + "//common/test_utilities:eigen_matrix_compare", + "//common/test_utilities:limit_malloc", + ], +) + +drake_cc_googletest( + name = "panda_status_receiver_test", + deps = [ + ":panda_status_receiver", + "//common/test_utilities:eigen_matrix_compare", + "//common/test_utilities:limit_malloc", + ], +) + +drake_cc_googletest( + name = "panda_status_sender_test", + deps = [ + ":panda_status_sender", + "//common/test_utilities:eigen_matrix_compare", + "//common/test_utilities:limit_malloc", + ], +) + +add_lint_tests() diff --git a/manipulation/franka_panda/panda_command_receiver.cc b/manipulation/franka_panda/panda_command_receiver.cc new file mode 100644 index 000000000000..d6d478e88f99 --- /dev/null +++ b/manipulation/franka_panda/panda_command_receiver.cc @@ -0,0 +1,269 @@ +#include "manipulation/franka_panda/panda_command_receiver.h" + +#include +#include + +#include "drake/common/drake_throw.h" +#include "drake/lcm/lcm_messages.h" +#include "drake/lcmt_panda_status.hpp" + +namespace drake { +namespace manipulation { + +using drake::lcmt_panda_command; +using drake::lcmt_panda_status; +using drake::Value; +using drake::lcm::AreLcmMessagesEqual; +using drake::systems::BasicVector; +using drake::systems::CompositeEventCollection; +using drake::systems::Context; +using drake::systems::DiscreteUpdateEvent; +using drake::systems::DiscreteValues; +using drake::systems::kVectorValued; +using Eigen::VectorXd; + +PandaCommandReceiver::PandaCommandReceiver(int num_joints, int control_mode) + : num_joints_(num_joints), control_mode_(control_mode) { + DRAKE_THROW_UNLESS(num_joints > 0); + + message_input_ = &DeclareAbstractInputPort("lcmt_panda_command", + Value()); + position_measured_input_ = + &DeclareInputPort("position_measured", kVectorValued, num_joints); + + // Even if we have not yet received a message, we still need to provide + // default values for our output ports. We use a cache entry to compute + // those values, by populating a default lcmt_panda_control message. + if (control_mode_ & lcmt_panda_status::CONTROL_MODE_POSITION) { + // When in position control mode, the default is derived from the + // "position_measured" input. + latched_position_measured_is_set_ = DeclareDiscreteState(VectorXd::Zero(1)); + latched_position_measured_ = + DeclareDiscreteState(VectorXd::Zero(num_joints)); + default_command_ = &DeclareCacheEntry( + "default_command", &PandaCommandReceiver::CalcDefaultCommand, + {discrete_state_ticket(latched_position_measured_is_set_), + discrete_state_ticket(latched_position_measured_), + position_measured_input_->ticket()}); + } else { + // When not in position control mode, the default is just zeros. + default_command_ = &DeclareCacheEntry( + "default_command", &PandaCommandReceiver::CalcDefaultCommand, + {nothing_ticket()}); + } + + // For the convenience of our output calc functions, this cache entry + // provides either the "message_input" (iff a message has been received) + // or else the "default_command". + message_input_or_default_ = &DeclareCacheEntry( + "message_input_or_default", + &PandaCommandReceiver::CalcMessageInputOrDefault, + {message_input_->ticket(), default_command_->ticket()}); + + int remaining = control_mode_; + if (control_mode_ & lcmt_panda_status::CONTROL_MODE_POSITION) { + remaining &= ~int{lcmt_panda_status::CONTROL_MODE_POSITION}; + commanded_position_output_ = + &DeclareVectorOutputPort("position", BasicVector(num_joints), + &PandaCommandReceiver::CalcPositionOutput, + {message_input_or_default_->ticket()}); + } + + if (control_mode_ & lcmt_panda_status::CONTROL_MODE_VELOCITY) { + remaining &= ~int{lcmt_panda_status::CONTROL_MODE_VELOCITY}; + commanded_velocity_output_ = + &DeclareVectorOutputPort("velocity", BasicVector(num_joints), + &PandaCommandReceiver::CalcVelocityOutput, + {message_input_or_default_->ticket()}); + } + + if (control_mode_ & lcmt_panda_status::CONTROL_MODE_TORQUE) { + remaining &= ~int{lcmt_panda_status::CONTROL_MODE_TORQUE}; + commanded_torque_output_ = + &DeclareVectorOutputPort("torque", BasicVector(num_joints), + &PandaCommandReceiver::CalcTorqueOutput, + {message_input_or_default_->ticket()}); + } + + if (remaining != 0) { + throw std::logic_error( + fmt::format("Invalid control_mode bits set: 0x{:x}", remaining)); + } +} + +PandaCommandReceiver::~PandaCommandReceiver() = default; + +const drake::systems::OutputPort& +PandaCommandReceiver::get_commanded_position_output_port() const { + if (commanded_position_output_ == nullptr) { + throw std::runtime_error( + "Invalid call to PandaCommandReciver::get_command_position_output_port" + " when control_mode does not include position"); + } + return *commanded_position_output_; +} + +const drake::systems::OutputPort& +PandaCommandReceiver::get_commanded_velocity_output_port() const { + if (commanded_velocity_output_ == nullptr) { + throw std::runtime_error( + "Invalid call to PandaCommandReciver::get_command_velocity_output_port" + " when control_mode does not include velocity"); + } + return *commanded_velocity_output_; +} + +const drake::systems::OutputPort& +PandaCommandReceiver::get_commanded_torque_output_port() const { + if (commanded_torque_output_ == nullptr) { + throw std::runtime_error( + "Invalid call to PandaCommandReciver::get_command_torque_output_port" + " when control_mode does not include torque"); + } + return *commanded_torque_output_; +} + +void PandaCommandReceiver::LatchInitialPosition( + const Context& context, DiscreteValues* result) const { + DRAKE_DEMAND(control_mode_ & lcmt_panda_status::CONTROL_MODE_POSITION); + 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_input_->Eval>(context)); +} + +void PandaCommandReceiver::LatchInitialPosition( + Context* context) const { + DRAKE_THROW_UNLESS(context != nullptr); + DRAKE_THROW_UNLESS(control_mode_ & lcmt_panda_status::CONTROL_MODE_POSITION); + LatchInitialPosition(*context, &context->get_mutable_discrete_state()); +} + +// 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 PandaCommandReceiver::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're not using position control, then we have no state to latch. + if (!(control_mode_ & lcmt_panda_status::CONTROL_MODE_POSITION)) { + return; + } + + // 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 System&, const Context& event_context, + const DiscreteUpdateEvent&, + DiscreteValues* next_values) { + LatchInitialPosition(event_context, next_values); + return drake::systems::EventStatus::Succeeded(); + })); +} + +void PandaCommandReceiver::CalcDefaultCommand( + const Context& context, lcmt_panda_command* result) const { + *result = {}; + result->control_mode_expected = control_mode_; + if (control_mode_ & lcmt_panda_status::CONTROL_MODE_POSITION) { + 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_input_->Eval>(context); + const VectorXd vec = default_position.CopyToVector(); + DRAKE_DEMAND(vec.size() == num_joints_); + result->num_joint_position = num_joints_; + result->joint_position = {vec.data(), vec.data() + num_joints_}; + } + if (control_mode_ & lcmt_panda_status::CONTROL_MODE_VELOCITY) { + result->num_joint_velocity = num_joints_; + result->joint_velocity = std::vector(num_joints_, 0.0); + } + if (control_mode_ & lcmt_panda_status::CONTROL_MODE_TORQUE) { + result->num_joint_torque = num_joints_; + result->joint_torque = std::vector(num_joints_, 0.0); + } +} + +void PandaCommandReceiver::CalcMessageInputOrDefault( + const Context& context, lcmt_panda_command* result) const { + // Copy the input value into our tentative result. + *result = message_input_->Eval(context); + if (AreLcmMessagesEqual(*result, lcmt_panda_command{})) { + // We haven't received a message yet; fall back to the default outputs. + *result = default_command_->Eval(context); + } else { + // We have received a message; validate it. + // TODO(jeremy.nimmer) Here is where we should check control_mode_ vs + // result->control_mode_expected. + } +} + +namespace { +void CheckNumJoints(const std::string& system_name, const char* vector_name, + int actual, int expected) { + if (actual != expected) { + throw std::runtime_error(fmt::format( + "PandaCommandReceiver {} expected {} {} joints, but received {}", + system_name, expected, vector_name, actual)); + } +} +} // namespace + +void PandaCommandReceiver::CalcPositionOutput( + const Context& context, BasicVector* output) const { + DRAKE_DEMAND(control_mode_ & lcmt_panda_status::CONTROL_MODE_POSITION); + const auto& message = + message_input_or_default_->Eval(context); + DRAKE_DEMAND(message.control_mode_expected == control_mode_); + CheckNumJoints(get_name(), "position", message.num_joint_position, + num_joints_); + CheckNumJoints(get_name(), "position", message.joint_position.size(), + num_joints_); + output->SetFromVector(Eigen::Map( + message.joint_position.data(), message.joint_position.size())); +} + +void PandaCommandReceiver::CalcVelocityOutput( + const Context& context, BasicVector* output) const { + DRAKE_DEMAND(control_mode_ & lcmt_panda_status::CONTROL_MODE_VELOCITY); + const auto& message = + message_input_or_default_->Eval(context); + DRAKE_DEMAND(message.control_mode_expected == control_mode_); + CheckNumJoints(get_name(), "velocity", message.num_joint_velocity, + num_joints_); + CheckNumJoints(get_name(), "velocity", message.joint_velocity.size(), + num_joints_); + output->SetFromVector(Eigen::Map( + message.joint_velocity.data(), message.joint_velocity.size())); +} + +void PandaCommandReceiver::CalcTorqueOutput(const Context& context, + BasicVector* output) const { + DRAKE_DEMAND(control_mode_ & lcmt_panda_status::CONTROL_MODE_TORQUE); + const auto& message = + message_input_or_default_->Eval(context); + DRAKE_DEMAND(message.control_mode_expected == control_mode_); + CheckNumJoints(get_name(), "torque", message.num_joint_torque, num_joints_); + CheckNumJoints(get_name(), "torque", message.joint_torque.size(), + num_joints_); + output->SetFromVector(Eigen::Map( + message.joint_torque.data(), message.joint_torque.size())); +} + +} // namespace manipulation +} // namespace drake diff --git a/manipulation/franka_panda/panda_command_receiver.h b/manipulation/franka_panda/panda_command_receiver.h new file mode 100644 index 000000000000..828fa37153a1 --- /dev/null +++ b/manipulation/franka_panda/panda_command_receiver.h @@ -0,0 +1,121 @@ +#pragma once + +#include "manipulation/franka_panda/panda_constants.h" + +#include "drake/common/drake_copyable.h" +#include "drake/common/drake_throw.h" +#include "drake/lcmt_panda_command.hpp" +#include "drake/systems/framework/leaf_system.h" + +namespace drake { +namespace manipulation { + +/// Handles lcmt_panda_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(). +/// +/// It has one required input port, "lcmt_panda_command". +/// It has several output ports, each one of size num_joints. +/// +/// @system +/// name: PandaCommandReceiver +/// input_ports: +/// - lcmt_panda_command +/// - position_measured +/// output_ports: +/// - position (*) +/// - velocity (*) +/// - torque (*) +/// @endsystem +/// +/// (*) Each output port is present iff the control_mode passed to the +/// constructor set the corresponding CONTROL_MODE bit. +/// +/// Prior to receiving a valid lcmt_panda_command message, the "position" output +/// (if present) initially feeds through from the "position_measured" input, and +/// both the "velocity" and "torque" outputs (if present) are zero. +/// +/// If discrete update events are enabled (e.g., during simulation), the system +/// latches the "position_measured" input into state during the first event, +/// and the "position" output (if present) comes from the latched state, not the +/// input. +/// +/// The lcmt_panda_command input must match the num_joints and control_mode that +/// were passed to the constructor: the message's control_mode_expected must be +/// set to the same value as the constructor's control_mode, and the message's +/// position, velocity, torque vectors must be sized to match the constructor's +/// num_joint iff the corresponding bit of the control_mode is set or must be +/// zero-sized otherwise. +/// TODO(jeremy.nimmer) The control_mode_expected is not actually validated yet +/// at runtime, but will be in the future (once we fix our code to stop setting +/// it incorrectly). +class PandaCommandReceiver final : public drake::systems::LeafSystem { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(PandaCommandReceiver); + + /// @param control_mode is a bitset of one or more control mode constants + /// defined in lcmt_panda_status::CONTROL_MODE_{POSITION,VELOCITY,TORQUE}. + PandaCommandReceiver(int num_joints, int control_mode); + ~PandaCommandReceiver() final; + + /// (Advanced.) Copies the current "position_measured" input + /// 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. + /// @pre the control_mode has the POSITION bit set + void LatchInitialPosition(drake::systems::Context*) const; + + const drake::systems::InputPort& get_message_input_port() const { + return *message_input_; + } + + const drake::systems::InputPort& get_position_measured_input_port() + const { + return *position_measured_input_; + } + + const drake::systems::OutputPort& get_commanded_position_output_port() + const; + + const drake::systems::OutputPort& get_commanded_velocity_output_port() + const; + + const drake::systems::OutputPort& get_commanded_torque_output_port() + const; + + private: + void LatchInitialPosition(const drake::systems::Context&, + drake::systems::DiscreteValues*) const; + void DoCalcNextUpdateTime(const drake::systems::Context&, + drake::systems::CompositeEventCollection*, + double*) const; + void CalcDefaultCommand(const drake::systems::Context&, + drake::lcmt_panda_command*) const; + void CalcMessageInputOrDefault(const drake::systems::Context&, + drake::lcmt_panda_command*) const; + void CalcPositionOutput(const drake::systems::Context&, + drake::systems::BasicVector*) const; + void CalcVelocityOutput(const drake::systems::Context&, + drake::systems::BasicVector*) const; + void CalcTorqueOutput(const drake::systems::Context&, + drake::systems::BasicVector*) const; + + const int num_joints_; + const int control_mode_; + const drake::systems::InputPort* message_input_{}; + const drake::systems::InputPort* position_measured_input_{}; + drake::systems::DiscreteStateIndex latched_position_measured_is_set_; + drake::systems::DiscreteStateIndex latched_position_measured_; + const drake::systems::CacheEntry* default_command_{}; + const drake::systems::CacheEntry* message_input_or_default_{}; + const drake::systems::OutputPort* commanded_position_output_{}; + const drake::systems::OutputPort* commanded_velocity_output_{}; + const drake::systems::OutputPort* commanded_torque_output_{}; +}; + +} // namespace manipulation +} // namespace drake diff --git a/manipulation/franka_panda/panda_command_sender.cc b/manipulation/franka_panda/panda_command_sender.cc new file mode 100644 index 000000000000..037ced7c3dd1 --- /dev/null +++ b/manipulation/franka_panda/panda_command_sender.cc @@ -0,0 +1,109 @@ +#include "manipulation/franka_panda/panda_command_sender.h" + +#include + +#include + +#include "drake/lcmt_panda_status.hpp" + +namespace drake { +namespace manipulation { + +using drake::lcmt_panda_command; +using drake::lcmt_panda_status; +using drake::systems::Context; +using drake::systems::kVectorValued; + +PandaCommandSender::PandaCommandSender(int num_joints, int control_mode) + : num_joints_(num_joints), control_mode_(control_mode) { + int remaining = control_mode_; + if (control_mode_ & lcmt_panda_status::CONTROL_MODE_POSITION) { + remaining &= ~int{lcmt_panda_status::CONTROL_MODE_POSITION}; + position_input_port_ = + &this->DeclareInputPort("position", kVectorValued, num_joints_); + } + if (control_mode_ & lcmt_panda_status::CONTROL_MODE_VELOCITY) { + remaining &= ~int{lcmt_panda_status::CONTROL_MODE_VELOCITY}; + velocity_input_port_ = + &this->DeclareInputPort("velocity", kVectorValued, num_joints_); + } + if (control_mode_ & lcmt_panda_status::CONTROL_MODE_TORQUE) { + remaining &= ~int{lcmt_panda_status::CONTROL_MODE_TORQUE}; + torque_input_port_ = + &this->DeclareInputPort("torque", kVectorValued, num_joints_); + } + if (remaining != 0) { + throw std::logic_error( + fmt::format("Invalid control_mode bits set: 0x{:x}", remaining)); + } + this->DeclareAbstractOutputPort("lcmt_panda_command", + &PandaCommandSender::CalcOutput); +} + +PandaCommandSender::~PandaCommandSender() = default; + +using InPort = drake::systems::InputPort; + +const InPort& PandaCommandSender::get_position_input_port() const { + if (position_input_port_ == nullptr) { + throw std::logic_error( + "Invalid call to PandaCommandSender::get_position_input_port when" + " control_mode does not involve position"); + } + return *position_input_port_; +} + +const InPort& PandaCommandSender::get_velocity_input_port() const { + if (velocity_input_port_ == nullptr) { + throw std::logic_error( + "Invalid call to PandaCommandSender::get_velocity_input_port when" + " control_mode does not involve velocity"); + } + return *velocity_input_port_; +} + +const InPort& PandaCommandSender::get_torque_input_port() const { + if (torque_input_port_ == nullptr) { + throw std::logic_error( + "Invalid call to PandaCommandSender::get_torque_input_port when" + " control_mode does not involve torque"); + } + return *torque_input_port_; +} + +namespace { +void CopyInputPortToMessage(const Context& context, const InPort* port, + int32_t* size_out, + std::vector* values_out) { + DRAKE_DEMAND(size_out != nullptr); + DRAKE_DEMAND(values_out != nullptr); + if (port == nullptr) { + *size_out = 0; + values_out->clear(); + } else { + const auto& values_in = port->Eval(context); + const int size_in = values_in.size(); + *size_out = size_in; + values_out->resize(size_in); + for (int i = 0; i < size_in; ++i) { + (*values_out)[i] = values_in[i]; + } + } +} +} // namespace + +void PandaCommandSender::CalcOutput(const Context& context, + lcmt_panda_command* output) const { + lcmt_panda_command& command = *output; + command.utime = context.get_time() * 1e6; + command.control_mode_expected = control_mode_; + CopyInputPortToMessage(context, position_input_port_, + &command.num_joint_position, &command.joint_position); + CopyInputPortToMessage(context, velocity_input_port_, + &command.num_joint_velocity, &command.joint_velocity); + CopyInputPortToMessage(context, torque_input_port_, &command.num_joint_torque, + &command.joint_torque); +} + +} // namespace manipulation +} // namespace drake diff --git a/manipulation/franka_panda/panda_command_sender.h b/manipulation/franka_panda/panda_command_sender.h new file mode 100644 index 000000000000..c7225b7fe0ab --- /dev/null +++ b/manipulation/franka_panda/panda_command_sender.h @@ -0,0 +1,65 @@ +#pragma once + +#include "manipulation/franka_panda/panda_constants.h" + +#include "drake/common/drake_copyable.h" +#include "drake/lcmt_panda_command.hpp" +#include "drake/systems/framework/leaf_system.h" + +namespace drake { +namespace manipulation { + +/// Creates and outputs lcmt_panda_command messages. +/// +/// Note that this system does not actually send the message an LCM channel. To +/// send the message, the output of this system should be connected to a +/// LcmPublisherSystem::Make(). +/// +/// This system has vector-valued input ports, each one of size num_joints. +/// +/// This system has one abstract-valued output port of type lcmt_panda_command. +/// +/// @system +/// name: PandaCommandSender +/// input_ports: +/// - position (*) +/// - velocity (*) +/// - torque (*) +/// output_ports: +/// - lcmt_panda_command +/// @endsystem +/// +/// (*) Each input port is present if it's relevant to the control_mode passed +/// to the constructor. +/// +/// @see `lcmt_panda_command.lcm` for additional documentation. +class PandaCommandSender final : public drake::systems::LeafSystem { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(PandaCommandSender); + + /// @param control_mode is a bitset of one or more control mode constants + /// defined in lcmt_panda_status::CONTROL_MODE_{POSITION,VELOCITY,TORQUE}. + PandaCommandSender(int num_joints, int control_mode); + ~PandaCommandSender() final; + + /// @name Named accessors for this System's input and output ports. + //@{ + const drake::systems::InputPort& get_position_input_port() const; + const drake::systems::InputPort& get_velocity_input_port() const; + const drake::systems::InputPort& get_torque_input_port() const; + //@} + + private: + void CalcOutput(const drake::systems::Context&, + drake::lcmt_panda_command*) const; + + const int num_joints_; + const int control_mode_; + + const drake::systems::InputPort* position_input_port_{}; + const drake::systems::InputPort* velocity_input_port_{}; + const drake::systems::InputPort* torque_input_port_{}; +}; + +} // namespace manipulation +} // namespace drake diff --git a/manipulation/franka_panda/panda_constants.h b/manipulation/franka_panda/panda_constants.h new file mode 100644 index 000000000000..ccb0462677e5 --- /dev/null +++ b/manipulation/franka_panda/panda_constants.h @@ -0,0 +1,9 @@ +#pragma once + +namespace drake { +namespace manipulation { + +const int kPandaArmNumJoints = 7; + +} // namespace manipulation +} // namespace drake diff --git a/manipulation/franka_panda/panda_status_receiver.cc b/manipulation/franka_panda/panda_status_receiver.cc new file mode 100644 index 000000000000..84a2ded24d57 --- /dev/null +++ b/manipulation/franka_panda/panda_status_receiver.cc @@ -0,0 +1,96 @@ +#include "manipulation/franka_panda/panda_status_receiver.h" + +#include "drake/common/drake_throw.h" + +namespace drake { +namespace manipulation { + +using drake::lcmt_panda_status; +using drake::Value; +using drake::systems::BasicVector; +using drake::systems::Context; + +PandaStatusReceiver::PandaStatusReceiver(int num_joints) + : num_joints_(num_joints) { + this->DeclareAbstractInputPort("lcmt_panda_status", + Value{}); + this->DeclareVectorOutputPort( + "position_commanded", BasicVector(num_joints_), + &PandaStatusReceiver::CalcLcmOutput< + &lcmt_panda_status::joint_position_desired>); + this->DeclareVectorOutputPort( + "position_measured", BasicVector(num_joints_), + &PandaStatusReceiver::CalcLcmOutput<&lcmt_panda_status::joint_position>); + this->DeclareVectorOutputPort( + "velocity_commanded", BasicVector(num_joints_), + &PandaStatusReceiver::CalcLcmOutput< + &lcmt_panda_status::joint_velocity_desired>); + this->DeclareVectorOutputPort( + "velocity_measured", BasicVector(num_joints_), + &PandaStatusReceiver::CalcLcmOutput<&lcmt_panda_status::joint_velocity>); + this->DeclareVectorOutputPort( + "acceleration_commanded", BasicVector(num_joints_), + &PandaStatusReceiver::CalcLcmOutput< + &lcmt_panda_status::joint_acceleration_desired>); + this->DeclareVectorOutputPort("torque_commanded", + BasicVector(num_joints_), + &PandaStatusReceiver::CalcLcmOutput< + &lcmt_panda_status::joint_torque_desired>); + this->DeclareVectorOutputPort( + "torque_measured", BasicVector(num_joints_), + &PandaStatusReceiver::CalcLcmOutput<&lcmt_panda_status::joint_torque>); + this->DeclareVectorOutputPort("torque_external", + BasicVector(num_joints_), + &PandaStatusReceiver::CalcLcmOutput< + &lcmt_panda_status::joint_torque_external>); +} + +PandaStatusReceiver::~PandaStatusReceiver() = default; + +using OutPort = drake::systems::OutputPort; +const OutPort& PandaStatusReceiver::get_position_commanded_output_port() const { + return LeafSystem::get_output_port(0); +} +const OutPort& PandaStatusReceiver::get_position_measured_output_port() const { + return LeafSystem::get_output_port(1); +} +const OutPort& PandaStatusReceiver::get_velocity_commanded_output_port() const { + return LeafSystem::get_output_port(2); +} +const OutPort& PandaStatusReceiver::get_velocity_measured_output_port() const { + return LeafSystem::get_output_port(3); +} +const OutPort& PandaStatusReceiver::get_acceleration_commanded_output_port() + const { + return LeafSystem::get_output_port(4); +} +const OutPort& PandaStatusReceiver::get_torque_commanded_output_port() const { + return LeafSystem::get_output_port(5); +} +const OutPort& PandaStatusReceiver::get_torque_measured_output_port() const { + return LeafSystem::get_output_port(6); +} +const OutPort& PandaStatusReceiver::get_torque_external_output_port() const { + return LeafSystem::get_output_port(7); +} + +template drake::lcmt_panda_status::* field_ptr> +void PandaStatusReceiver::CalcLcmOutput(const Context& context, + BasicVector* output) const { + const auto& status = get_input_port().Eval(context); + + // If we're using a default constructed message (i.e., we haven't received + // any status message yet), output zero. + if (status.num_joints == 0) { + output->get_mutable_value().setZero(); + } else { + const auto& status_field = status.*field_ptr; + DRAKE_THROW_UNLESS(status.num_joints == num_joints_); + DRAKE_THROW_UNLESS(static_cast(status_field.size()) == num_joints_); + output->get_mutable_value() = + Eigen::Map(status_field.data(), num_joints_); + } +} + +} // namespace manipulation +} // namespace drake diff --git a/manipulation/franka_panda/panda_status_receiver.h b/manipulation/franka_panda/panda_status_receiver.h new file mode 100644 index 000000000000..ba0568c4a7fd --- /dev/null +++ b/manipulation/franka_panda/panda_status_receiver.h @@ -0,0 +1,68 @@ +#pragma once + +#include + +#include "manipulation/franka_panda/panda_constants.h" + +#include "drake/common/drake_copyable.h" +#include "drake/lcmt_panda_status.hpp" +#include "drake/systems/framework/leaf_system.h" + +namespace drake { +namespace manipulation { + +/// Handles lcmt_panda_status messages 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(). +/// +/// This system has one abstract-valued input port of type lcmt_panda_status. +/// +/// This system has many vector-valued output ports, each of which has exactly +/// num_joints elements. The ports will output zeros until an input message is +/// received. +// +/// @system +/// name: PandaStatusReceiver +/// input_ports: +/// - lcmt_panda_status +/// output_ports: +/// - position_commanded +/// - position_measured +/// - velocity_commanded +/// - velocity_measured +/// - acceleration_commanded +/// - torque_commanded +/// - torque_measured +/// - torque_external +/// @endsystem +/// +/// @see `lcmt_panda_status.lcm` for additional documentation. +class PandaStatusReceiver final : public drake::systems::LeafSystem { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(PandaStatusReceiver); + + explicit PandaStatusReceiver(int num_joints = kPandaArmNumJoints); + ~PandaStatusReceiver() final; + + using OutputPort = drake::systems::OutputPort; + const OutputPort& get_position_commanded_output_port() const; + const OutputPort& get_position_measured_output_port() const; + const OutputPort& get_velocity_commanded_output_port() const; + const OutputPort& get_velocity_measured_output_port() const; + const OutputPort& get_acceleration_commanded_output_port() const; + const OutputPort& get_torque_commanded_output_port() const; + const OutputPort& get_torque_measured_output_port() const; + const OutputPort& get_torque_external_output_port() const; + + private: + template drake::lcmt_panda_status::*> + void CalcLcmOutput(const drake::systems::Context&, + drake::systems::BasicVector*) const; + + const int num_joints_; +}; + +} // namespace manipulation +} // namespace drake diff --git a/manipulation/franka_panda/panda_status_sender.cc b/manipulation/franka_panda/panda_status_sender.cc new file mode 100644 index 000000000000..f1cc92329094 --- /dev/null +++ b/manipulation/franka_panda/panda_status_sender.cc @@ -0,0 +1,145 @@ +#include "manipulation/franka_panda/panda_status_sender.h" + +namespace drake { +namespace manipulation { + +using drake::lcmt_panda_status; +using drake::systems::Context; +using drake::systems::InputPort; +using drake::systems::kVectorValued; + +PandaStatusSender::PandaStatusSender(int num_joints) + : num_joints_(num_joints), zero_vector_(Eigen::VectorXd::Zero(num_joints)) { + DeclareInputPort("position_commanded", kVectorValued, num_joints_); + DeclareInputPort("position_measured", kVectorValued, num_joints_); + DeclareInputPort("velocity_commanded", kVectorValued, num_joints_); + DeclareInputPort("velocity_measured", kVectorValued, num_joints_); + DeclareInputPort("acceleration_commanded", kVectorValued, num_joints_); + DeclareInputPort("torque_commanded", kVectorValued, num_joints_); + DeclareInputPort("torque_measured", kVectorValued, num_joints_); + DeclareInputPort("torque_external", kVectorValued, num_joints_); + DeclareAbstractOutputPort("lcmt_panda_status", + &PandaStatusSender::CalcOutput); +} + +PandaStatusSender::~PandaStatusSender() = default; + +// using drake::systems::LeafSystem::get_input_port; +using InPort = InputPort; +const InPort& PandaStatusSender::get_position_commanded_input_port() const { + return get_input_port(0); +} +const InPort& PandaStatusSender::get_position_measured_input_port() const { + return get_input_port(1); +} +const InPort& PandaStatusSender::get_velocity_commanded_input_port() const { + return get_input_port(2); +} +const InPort& PandaStatusSender::get_velocity_measured_input_port() const { + return get_input_port(3); +} +const InPort& PandaStatusSender::get_acceleration_commanded_input_port() const { + return get_input_port(4); +} +const InPort& PandaStatusSender::get_torque_commanded_input_port() const { + return get_input_port(5); +} +const InPort& PandaStatusSender::get_torque_measured_input_port() const { + return get_input_port(6); +} +const InPort& PandaStatusSender::get_torque_external_input_port() const { + return get_input_port(7); +} + +namespace { +// Returns the first one of port1.Eval or port2.Eval that has a value. +// If min_num_connected is zero and both ports are empty, return zeros. +// If less than min_num_connected of (port1,port2) are connected, throws. +// If more than max_num_connected of (port1,port2) are connected, throws. +// If port2_tail is provided, a suffix of port2's value is returned. +Eigen::Ref EvalFirstConnected( + const Context& context, int min_num_connected, + int max_num_connected, const Eigen::VectorXd& zeros, + const InputPort& port1, const InputPort& port2, + const int port2_tail = -1) { + const int total_connected = + (port1.HasValue(context) ? 1 : 0) + (port2.HasValue(context) ? 1 : 0); + if (total_connected > max_num_connected) { + throw std::logic_error( + fmt::format("Both {} and {} cannot both be connected at the same time.", + port1.GetFullDescription(), port2.GetFullDescription())); + } + if (total_connected < min_num_connected) { + throw std::logic_error(fmt::format( + "At least {} of {} or {} must be connected.", min_num_connected, + port1.GetFullDescription(), port2.GetFullDescription())); + } + if (port1.HasValue(context)) { + return port1.Eval(context); + } + if (port2.HasValue(context)) { + if (port2_tail < 0) { + return port2.Eval(context); + } else { + return port2.Eval(context).tail(port2_tail); + } + } + return zeros; +} + +} // namespace + +void PandaStatusSender::CalcOutput(const Context& context, + lcmt_panda_status* output) const { + const auto& position_commanded = + get_position_commanded_input_port().HasValue(context) + ? get_position_commanded_input_port().Eval(context) + : zero_vector_.head(num_joints_); + const auto& position_measured = + get_position_measured_input_port().Eval(context); + const auto& velocity_commanded = + get_velocity_commanded_input_port().HasValue(context) + ? get_velocity_commanded_input_port().Eval(context) + : zero_vector_.head(num_joints_); + const auto& velocity_measured = + get_velocity_measured_input_port().HasValue(context) + ? get_velocity_measured_input_port().Eval(context) + : zero_vector_.head(num_joints_); + const auto& acceleration_commanded = + get_acceleration_commanded_input_port().HasValue(context) + ? get_acceleration_commanded_input_port().Eval(context) + : zero_vector_.head(num_joints_); + const auto& torque_commanded = + get_torque_commanded_input_port().Eval(context); + const auto& torque_measured = EvalFirstConnected( + context, 1, 2, zero_vector_, get_torque_measured_input_port(), + get_torque_commanded_input_port()); + const auto& torque_external = EvalFirstConnected( + context, 0, 2, zero_vector_, get_torque_external_input_port(), + get_torque_external_input_port()); + + lcmt_panda_status& status = *output; + status.utime = context.get_time() * 1e6; + status.num_joints = num_joints_; + status.joint_position.resize(num_joints_, 0); + status.joint_position_desired.resize(num_joints_, 0); + status.joint_velocity.resize(num_joints_, 0); + status.joint_velocity_desired.resize(num_joints_, 0); + status.joint_acceleration_desired.resize(num_joints_, 0); + status.joint_torque.resize(num_joints_, 0); + status.joint_torque_desired.resize(num_joints_, 0); + status.joint_torque_external.resize(num_joints_, 0); + for (int i = 0; i < num_joints_; ++i) { + status.joint_position[i] = position_measured[i]; + status.joint_position_desired[i] = position_commanded[i]; + status.joint_velocity[i] = velocity_measured[i]; + status.joint_velocity_desired[i] = velocity_commanded[i]; + status.joint_acceleration_desired[i] = acceleration_commanded[i]; + status.joint_torque[i] = torque_measured[i]; + status.joint_torque_desired[i] = torque_commanded[i]; + status.joint_torque_external[i] = torque_external[i]; + } +} + +} // namespace manipulation +} // namespace drake diff --git a/manipulation/franka_panda/panda_status_sender.h b/manipulation/franka_panda/panda_status_sender.h new file mode 100644 index 000000000000..6a915f48f0c1 --- /dev/null +++ b/manipulation/franka_panda/panda_status_sender.h @@ -0,0 +1,68 @@ +#pragma once + +#include "manipulation/franka_panda/panda_constants.h" + +#include "drake/common/drake_copyable.h" +#include "drake/lcmt_panda_status.hpp" +#include "drake/systems/framework/leaf_system.h" + +namespace drake { +namespace manipulation { + +/// Creates and outputs lcmt_panda_status messages. +/// +/// Note that this system does not actually send the message an LCM channel. To +/// send the message, the output of this system should be connected to a +/// LcmPublisherSystem::Make(). +/// +/// This system has many vector-valued input ports, each of which has exactly +/// num_joints elements. +/// +/// This system has one abstract-valued output port of type lcmt_panda_status. +/// +/// 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 +/// name: PandaStatusSender +/// input_ports: +/// - position_commanded (optional) +/// - position_measured +/// - velocity_commanded (optional) +/// - velocity_measured (optional) +/// - acceleration_commanded (optional) +/// - torque_commanded +/// - torque_measured (optional) +/// - torque_external (optional) +/// output_ports: +/// - lcmt_panda_status +/// @endsystem +/// +/// @see `lcmt_panda_status.lcm` for additional documentation. +class PandaStatusSender final : public drake::systems::LeafSystem { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(PandaStatusSender); + + explicit PandaStatusSender(int num_joints = kPandaArmNumJoints); + ~PandaStatusSender() final; + + using InputPort = drake::systems::InputPort; + const InputPort& get_position_commanded_input_port() const; + const InputPort& get_position_measured_input_port() const; + const InputPort& get_velocity_commanded_input_port() const; + const InputPort& get_velocity_measured_input_port() const; + const InputPort& get_acceleration_commanded_input_port() const; + const InputPort& get_torque_commanded_input_port() const; + const InputPort& get_torque_measured_input_port() const; + const InputPort& get_torque_external_input_port() const; + + private: + void CalcOutput(const drake::systems::Context&, + drake::lcmt_panda_status*) const; + + const int num_joints_; + const Eigen::VectorXd zero_vector_; +}; + +} // namespace manipulation +} // namespace drake diff --git a/manipulation/franka_panda/test/panda_command_receiver_test.cc b/manipulation/franka_panda/test/panda_command_receiver_test.cc new file mode 100644 index 000000000000..09947859d6ab --- /dev/null +++ b/manipulation/franka_panda/test/panda_command_receiver_test.cc @@ -0,0 +1,168 @@ +#include "manipulation/franka_panda/panda_command_receiver.h" + +#include + +#include +#include + +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/lcmt_panda_status.hpp" + +namespace drake { +namespace manipulation { +namespace { + +using drake::CompareMatrices; +using drake::lcmt_panda_command; +using drake::lcmt_panda_status; +using drake::systems::Context; +using drake::systems::FixedInputPortValue; +using Eigen::VectorXd; +constexpr int N = kPandaArmNumJoints; + +class PandaCommandReceiverTest : public testing::Test { + public: + PandaCommandReceiverTest() + : dut_(kPandaArmNumJoints, lcmt_panda_status::CONTROL_MODE_POSITION | + lcmt_panda_status::CONTROL_MODE_TORQUE), + context_ptr_(dut_.CreateDefaultContext()), + context_(*context_ptr_), + fixed_input_(FixInput()) {} + + // For use only by our constructor. + FixedInputPortValue& FixInput() { + return dut_.get_message_input_port().FixValue(&context_, + lcmt_panda_command{}); + } + + // Test cases should call this to set the DUT's input value. + void SetInput(lcmt_panda_command message) { + // TODO(jwnimmer-tri) This systems framework API is not very ergonomic. + fixed_input_.GetMutableData() + ->template get_mutable_value() = message; + } + + VectorXd position() const { + const auto& port = dut_.get_commanded_position_output_port(); + const VectorXd result = port.Eval(context_); + return result; + } + + VectorXd velocity() const { + return dut_.get_commanded_velocity_output_port().Eval(context_); + } + + VectorXd torque() const { + return dut_.get_commanded_torque_output_port().Eval(context_); + } + + protected: + PandaCommandReceiver dut_; + std::unique_ptr> context_ptr_; + Context& context_; + FixedInputPortValue& fixed_input_; +}; + +TEST_F(PandaCommandReceiverTest, AcceptanceTestWithMeasuredPositionInput) { + const VectorXd zero = VectorXd::Zero(N); + + // We cannot query commanded velocity. + EXPECT_THROW(velocity(), std::runtime_error); + + // 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.2, 0.3); + dut_.get_position_measured_input_port().FixValue(&context_, q0); + EXPECT_TRUE(CompareMatrices(position(), q0)); + EXPECT_TRUE(CompareMatrices(torque(), zero)); + + // Check that a real command trumps the initial position. + const VectorXd q1 = VectorXd::LinSpaced(N, 0.3, 0.4); + const VectorXd t1 = VectorXd::LinSpaced(N, 0.5, 0.6); + lcmt_panda_command command{}; + command.control_mode_expected = lcmt_panda_status::CONTROL_MODE_POSITION | + lcmt_panda_status::CONTROL_MODE_TORQUE; + command.utime = 0; + command.num_joint_position = N; + command.joint_position = {q1.data(), q1.data() + q1.size()}; + command.num_joint_torque = N; + command.joint_torque = {t1.data(), t1.data() + t1.size()}; + SetInput(command); + EXPECT_TRUE(CompareMatrices(position(), q1)); + EXPECT_THROW(velocity(), std::runtime_error); + EXPECT_TRUE(CompareMatrices(torque(), t1)); +} + +TEST_F(PandaCommandReceiverTest, AcceptanceTestWithLatching) { + const VectorXd zero = VectorXd::Zero(N); + + // We cannot query commanded velocity. + EXPECT_THROW(velocity(), std::runtime_error); + + // 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 v3 = VectorXd::LinSpaced(N, 0.5, 0.6); + const VectorXd t3 = VectorXd::LinSpaced(N, 0.6, 0.7); + lcmt_panda_command command{}; + command.control_mode_expected = lcmt_panda_status::CONTROL_MODE_POSITION | + lcmt_panda_status::CONTROL_MODE_TORQUE; + command.utime = 0; + command.num_joint_position = N; + command.joint_position = {q3.data(), q3.data() + q3.size()}; + command.num_joint_torque = N; + command.joint_torque = {t3.data(), t3.data() + t3.size()}; + SetInput(command); + EXPECT_TRUE(CompareMatrices(position(), q3)); + EXPECT_TRUE(CompareMatrices(torque(), t3)); +} + +GTEST_TEST(PandaCommandReceiverTestVelocity, VelocityControl) { + PandaCommandReceiver receiver(kPandaArmNumJoints, + lcmt_panda_status::CONTROL_MODE_VELOCITY); + EXPECT_EQ(receiver.num_discrete_state_groups(), 0); + + EXPECT_THROW(receiver.get_commanded_position_output_port(), + std::runtime_error); + EXPECT_THROW(receiver.get_commanded_torque_output_port(), std::runtime_error); + + lcmt_panda_command command{}; + command.control_mode_expected = lcmt_panda_status::CONTROL_MODE_VELOCITY; + command.num_joint_velocity = kPandaArmNumJoints; + command.joint_velocity.resize(kPandaArmNumJoints, 0.1); + + auto context = receiver.CreateDefaultContext(); + receiver.get_message_input_port().FixValue(context.get(), command); + const VectorXd velocity = + receiver.get_commanded_velocity_output_port().Eval(*context); + EXPECT_TRUE(CompareMatrices(velocity, VectorXd::Constant(7, 0.1))); +} + +TEST_F(PandaCommandReceiverTest, BadControlMode) { + EXPECT_THROW(PandaCommandReceiver(7, 0x9999), std::exception); +} + +} // namespace +} // namespace manipulation +} // namespace drake diff --git a/manipulation/franka_panda/test/panda_command_sender_test.cc b/manipulation/franka_panda/test/panda_command_sender_test.cc new file mode 100644 index 000000000000..24dfcd1a739b --- /dev/null +++ b/manipulation/franka_panda/test/panda_command_sender_test.cc @@ -0,0 +1,142 @@ +#include "manipulation/franka_panda/panda_command_sender.h" + +#include +#include + +#include +#include + +#include "drake/common/test_utilities/limit_malloc.h" +#include "drake/lcmt_panda_status.hpp" + +namespace drake { +namespace manipulation { +namespace { + +using drake::lcmt_panda_command; +using drake::lcmt_panda_status; +using drake::systems::Context; +using drake::test::LimitMalloc; +using Eigen::VectorXd; + +constexpr int N = kPandaArmNumJoints; + +class PandaCommandSenderTest : public testing::Test { + public: + PandaCommandSenderTest() + : dut_(N, lcmt_panda_status::CONTROL_MODE_POSITION | + lcmt_panda_status::CONTROL_MODE_TORQUE), + context_ptr_(dut_.CreateDefaultContext()), + context_(*context_ptr_) {} + + const lcmt_panda_command& output() const { + return dut_.get_output_port().Eval(context_); + } + + protected: + PandaCommandSender dut_; + std::unique_ptr> context_ptr_; + Context& context_; + + const VectorXd q0_{VectorXd::LinSpaced(N, 0.1, 0.2)}; + const std::vector std_q0_{q0_.data(), q0_.data() + q0_.size()}; + + const VectorXd v0_{VectorXd::LinSpaced(N, 0.2, 0.3)}; + const std::vector std_v0_{v0_.data(), v0_.data() + v0_.size()}; + + const VectorXd t0_{VectorXd::LinSpaced(N, 0.3, 0.4)}; + const std::vector std_t0_{t0_.data(), t0_.data() + t0_.size()}; +}; + +TEST_F(PandaCommandSenderTest, PositionAndTorque) { + dut_.get_position_input_port().FixValue(&context_, q0_); + dut_.get_torque_input_port().FixValue(&context_, t0_); + EXPECT_EQ(output().control_mode_expected, + lcmt_panda_status::CONTROL_MODE_POSITION | + lcmt_panda_status::CONTROL_MODE_TORQUE); + EXPECT_EQ(output().num_joint_position, N); + EXPECT_EQ(output().joint_position, std_q0_); + EXPECT_EQ(output().num_joint_velocity, 0); + EXPECT_EQ(output().joint_velocity.size(), 0); + EXPECT_EQ(output().num_joint_torque, N); + EXPECT_EQ(output().joint_torque, std_t0_); +} + +TEST_F(PandaCommandSenderTest, PositionOnly) { + const PandaCommandSender sender(N, lcmt_panda_status::CONTROL_MODE_POSITION); + auto context = sender.CreateDefaultContext(); + sender.get_position_input_port().FixValue(context.get(), q0_); + const lcmt_panda_command output = + sender.get_output_port().Eval(*context); + EXPECT_EQ(output.control_mode_expected, + lcmt_panda_status::CONTROL_MODE_POSITION); + EXPECT_EQ(output.num_joint_position, N); + EXPECT_EQ(output.joint_position, std_q0_); + EXPECT_EQ(output.num_joint_velocity, 0); + EXPECT_EQ(output.joint_velocity.size(), 0); + EXPECT_EQ(output.num_joint_torque, 0); + EXPECT_EQ(output.joint_torque.size(), 0); +} + +TEST_F(PandaCommandSenderTest, VelocityOnly) { + const PandaCommandSender sender(N, lcmt_panda_status::CONTROL_MODE_VELOCITY); + auto context = sender.CreateDefaultContext(); + sender.get_velocity_input_port().FixValue(context.get(), v0_); + const lcmt_panda_command output = + sender.get_output_port().Eval(*context); + EXPECT_EQ(output.control_mode_expected, + lcmt_panda_status::CONTROL_MODE_VELOCITY); + EXPECT_EQ(output.num_joint_position, 0); + EXPECT_EQ(output.joint_position.size(), 0); + EXPECT_EQ(output.num_joint_velocity, N); + EXPECT_EQ(output.joint_velocity, std_v0_); + EXPECT_EQ(output.num_joint_torque, 0); + EXPECT_EQ(output.joint_torque.size(), 0); +} + +TEST_F(PandaCommandSenderTest, TorqueOnly) { + const PandaCommandSender sender(N, lcmt_panda_status::CONTROL_MODE_TORQUE); + auto context = sender.CreateDefaultContext(); + sender.get_torque_input_port().FixValue(context.get(), t0_); + const lcmt_panda_command output = + sender.get_output_port().Eval(*context); + EXPECT_EQ(output.control_mode_expected, + lcmt_panda_status::CONTROL_MODE_TORQUE); + EXPECT_EQ(output.num_joint_position, 0); + EXPECT_EQ(output.joint_position.size(), 0); + EXPECT_EQ(output.num_joint_velocity, 0); + EXPECT_EQ(output.joint_velocity.size(), 0); + EXPECT_EQ(output.num_joint_torque, N); + EXPECT_EQ(output.joint_torque, std_t0_); +} + +// This class is likely to be used on the critical path for robot control, so +// we insist that it must not perform heap operations while in steady-state. +TEST_F(PandaCommandSenderTest, MallocTest) { + // Compute the output once; this will allocate all required storage. + auto& q = dut_.get_position_input_port().FixValue(&context_, q0_); + auto& tau = dut_.get_torque_input_port().FixValue(&context_, t0_); + output(); + + // Change q and recompute; no heap changes are allowed. + { + LimitMalloc guard; + q.GetMutableVectorData(); + output(); + } + + // Change tau and recompute; no heap changes are allowed. + { + LimitMalloc guard; + tau.GetMutableVectorData(); + output(); + } +} + +TEST_F(PandaCommandSenderTest, BadControlMode) { + EXPECT_THROW(PandaCommandSender(7, 0x9999), std::exception); +} + +} // namespace +} // namespace manipulation +} // namespace drake diff --git a/manipulation/franka_panda/test/panda_status_receiver_test.cc b/manipulation/franka_panda/test/panda_status_receiver_test.cc new file mode 100644 index 000000000000..02d4a0728091 --- /dev/null +++ b/manipulation/franka_panda/test/panda_status_receiver_test.cc @@ -0,0 +1,137 @@ +#include "manipulation/franka_panda/panda_status_receiver.h" + +#include +#include + +#include +#include + +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/common/test_utilities/limit_malloc.h" + +namespace drake { +namespace manipulation { +namespace { + +using drake::CompareMatrices; +using drake::lcmt_panda_status; +using drake::systems::Context; +using drake::systems::FixedInputPortValue; +using drake::systems::LeafSystem; +using drake::test::LimitMalloc; +using Eigen::VectorXd; + +constexpr int N = kPandaArmNumJoints; + +class PandaStatusReceiverTest : public testing::Test { + public: + PandaStatusReceiverTest() + : dut_(), + context_ptr_(dut_.CreateDefaultContext()), + context_(*context_ptr_), + fixed_input_( + dut_.get_input_port().FixValue(&context_, lcmt_panda_status{})) {} + + void Copy(const Eigen::VectorXd& from, std::vector* to) { + *to = {from.data(), from.data() + from.size()}; + } + + protected: + PandaStatusReceiver dut_; + std::unique_ptr> context_ptr_; + Context& context_; + FixedInputPortValue& fixed_input_; + lcmt_panda_status status_{}; +}; + +TEST_F(PandaStatusReceiverTest, AcceptanceTest) { + // Confirm that output is zero for uninitialized lcm input. + const int num_output_ports = dut_.num_output_ports(); + EXPECT_EQ(num_output_ports, 8); + for (int i = 0; i < num_output_ports; ++i) { + const LeafSystem& leaf = dut_; + const auto& port = leaf.get_output_port(i); + EXPECT_TRUE(CompareMatrices(port.Eval(context_), VectorXd::Zero(N))); + } + + // Populate the status message with distinct values. + const VectorXd position_commanded = VectorXd::LinSpaced(N, 0.0, 1.0); + const VectorXd position_measured = VectorXd::LinSpaced(N, 1.0, 2.0); + const VectorXd velocity_commanded = VectorXd::LinSpaced(N, 2.0, 3.0); + const VectorXd velocity_measured = VectorXd::LinSpaced(N, 3.0, 4.0); + const VectorXd acceleration_commanded = VectorXd::LinSpaced(N, 4.0, 5.0); + const VectorXd torque_commanded = VectorXd::LinSpaced(N, 6.0, 7.0); + const VectorXd torque_measured = VectorXd::LinSpaced(N, 8.0, 9.0); + const VectorXd torque_external = VectorXd::LinSpaced(N, 10.0, 11.0); + status_.utime = 1; + status_.num_joints = N; + Copy(position_commanded, &status_.joint_position_desired); + Copy(position_measured, &status_.joint_position); + Copy(velocity_commanded, &status_.joint_velocity_desired); + Copy(velocity_measured, &status_.joint_velocity); + Copy(acceleration_commanded, &status_.joint_acceleration_desired); + Copy(torque_commanded, &status_.joint_torque_desired); + Copy(torque_measured, &status_.joint_torque); + Copy(torque_external, &status_.joint_torque_external); + // TODO(jwnimmer-tri) This systems framework API is not very ergonomic. + fixed_input_.GetMutableData() + ->template get_mutable_value() = status_; + + // Confirm that real message values are output correctly. + EXPECT_TRUE( + CompareMatrices(dut_.get_position_commanded_output_port().Eval(context_), + position_commanded)); + EXPECT_TRUE( + CompareMatrices(dut_.get_position_measured_output_port().Eval(context_), + position_measured)); + EXPECT_TRUE( + CompareMatrices(dut_.get_velocity_commanded_output_port().Eval(context_), + velocity_commanded)); + EXPECT_TRUE( + CompareMatrices(dut_.get_velocity_measured_output_port().Eval(context_), + velocity_measured)); + EXPECT_TRUE(CompareMatrices( + dut_.get_acceleration_commanded_output_port().Eval(context_), + acceleration_commanded)); + EXPECT_TRUE( + CompareMatrices(dut_.get_torque_commanded_output_port().Eval(context_), + torque_commanded)); + EXPECT_TRUE(CompareMatrices( + dut_.get_torque_measured_output_port().Eval(context_), torque_measured)); + EXPECT_TRUE(CompareMatrices( + dut_.get_torque_external_output_port().Eval(context_), torque_external)); +} + +// This class is likely to be used on the critical path for robot control, so +// we insist that it must not perform heap operations while in steady-state. +TEST_F(PandaStatusReceiverTest, MallocTest) { + // Set as input a non-trivial message. + status_.utime = 1; + status_.num_joints = N; + status_.joint_position_desired.resize(N); + status_.joint_position.resize(N); + status_.joint_velocity_desired.resize(N); + status_.joint_velocity.resize(N); + status_.joint_acceleration_desired.resize(N); + status_.joint_torque_desired.resize(N); + status_.joint_torque.resize(N); + status_.joint_torque_external.resize(N); + // TODO(jwnimmer-tri) This systems framework API is not very ergonomic. + fixed_input_.GetMutableData() + ->template get_mutable_value() = status_; + + // Compute the output. No heap changes are allowed. + { + LimitMalloc guard; + const LeafSystem& leaf = dut_; + const int num_output_ports = leaf.num_output_ports(); + for (int i = 0; i < num_output_ports; ++i) { + const auto& port = leaf.get_output_port(i); + port.Eval(context_); + } + } +} + +} // namespace +} // namespace manipulation +} // namespace drake diff --git a/manipulation/franka_panda/test/panda_status_sender_test.cc b/manipulation/franka_panda/test/panda_status_sender_test.cc new file mode 100644 index 000000000000..6e68f64ace86 --- /dev/null +++ b/manipulation/franka_panda/test/panda_status_sender_test.cc @@ -0,0 +1,99 @@ +#include "manipulation/franka_panda/panda_status_sender.h" + +#include +#include + +#include +#include + +namespace drake { +namespace manipulation { +namespace { + +using drake::lcmt_panda_status; +using drake::systems::Context; +using drake::systems::InputPort; +using Eigen::VectorXd; +constexpr int N = kPandaArmNumJoints; + +class PandaStatusSenderTest : public testing::Test { + public: + PandaStatusSenderTest() + : dut_(), + context_ptr_(dut_.CreateDefaultContext()), + context_(*context_ptr_) {} + + const lcmt_panda_status& output() const { + const lcmt_panda_status& result = + dut_.get_output_port().Eval(context_); + DRAKE_DEMAND(result.num_joints == N); + DRAKE_DEMAND(result.joint_position.size() == N); + DRAKE_DEMAND(result.joint_position_desired.size() == N); + DRAKE_DEMAND(result.joint_velocity.size() == N); + DRAKE_DEMAND(result.joint_velocity_desired.size() == N); + DRAKE_DEMAND(result.joint_acceleration_desired.size() == N); + DRAKE_DEMAND(result.joint_torque.size() == N); + DRAKE_DEMAND(result.joint_torque_desired.size() == N); + DRAKE_DEMAND(result.joint_torque_external.size() == N); + return result; + } + + static std::vector as_vector(const Eigen::VectorXd& v) { + return {v.data(), v.data() + v.size()}; + } + + void Fix(const InputPort& port, const Eigen::VectorXd& v) { + port.FixValue(&context_, v); + } + + protected: + PandaStatusSender dut_; + std::unique_ptr> context_ptr_; + Context& context_; +}; + +TEST_F(PandaStatusSenderTest, AcceptanceTest) { + const VectorXd q0_commanded = VectorXd::LinSpaced(N, 0.1, 0.2); + const VectorXd q0_measured = VectorXd::LinSpaced(N, 0.11, 0.22); + const VectorXd v0_commanded = VectorXd::LinSpaced(N, 0.2, 0.3); + const VectorXd v0_measured = VectorXd::LinSpaced(N, 0.22, 0.33); + const VectorXd a0_commanded = VectorXd::LinSpaced(N, 0.3, 0.4); + const VectorXd t0_commanded = VectorXd::LinSpaced(N, 0.4, 0.5); + const VectorXd t0_measured = VectorXd::LinSpaced(N, 0.44, 0.55); + const VectorXd t0_external = VectorXd::LinSpaced(N, 0.6, 0.7); + const std::vector zeros(N, 0.0); + + // Fix only the required inputs ... + Fix(dut_.get_position_measured_input_port(), q0_measured); + Fix(dut_.get_torque_commanded_input_port(), t0_commanded); + // ... so that some outputs have passthrough values ... + EXPECT_EQ(output().joint_position, as_vector(q0_measured)); + EXPECT_EQ(output().joint_torque_desired, as_vector(t0_commanded)); + // ... and some outputs have default values. + EXPECT_EQ(output().joint_position_desired, zeros); + EXPECT_EQ(output().joint_velocity_desired, zeros); + EXPECT_EQ(output().joint_velocity, zeros); + EXPECT_EQ(output().joint_acceleration_desired, zeros); + EXPECT_EQ(output().joint_torque, as_vector(t0_commanded)); + EXPECT_EQ(output().joint_torque_external, zeros); + + // Fix all of the inputs ... + Fix(dut_.get_position_commanded_input_port(), q0_commanded); + Fix(dut_.get_velocity_commanded_input_port(), v0_commanded); + Fix(dut_.get_velocity_measured_input_port(), v0_measured); + Fix(dut_.get_acceleration_commanded_input_port(), a0_commanded); + Fix(dut_.get_torque_measured_input_port(), t0_measured); + Fix(dut_.get_torque_external_input_port(), t0_external); + // ... so all outputs have values. + EXPECT_EQ(output().joint_position_desired, as_vector(q0_commanded)); + EXPECT_EQ(output().joint_position, as_vector(q0_measured)); + EXPECT_EQ(output().joint_velocity_desired, as_vector(v0_commanded)); + EXPECT_EQ(output().joint_velocity, as_vector(v0_measured)); + EXPECT_EQ(output().joint_torque_desired, as_vector(t0_commanded)); + EXPECT_EQ(output().joint_torque, as_vector(t0_measured)); + EXPECT_EQ(output().joint_torque_external, as_vector(t0_external)); +} + +} // namespace +} // namespace manipulation +} // namespace drake From a06df7cdeefe8484133ee8dab04666c948e704a6 Mon Sep 17 00:00:00 2001 From: Peter Date: Thu, 9 Oct 2025 15:51:08 -0400 Subject: [PATCH 02/12] cpp portion passing tests --- bindings/pydrake/manipulation/manipulation_py_franka_panda.cc | 4 ++-- manipulation/franka_panda/panda_command_receiver.cc | 2 ++ manipulation/franka_panda/panda_command_receiver.h | 2 ++ manipulation/franka_panda/panda_command_sender.cc | 2 ++ manipulation/franka_panda/panda_command_sender.h | 2 ++ manipulation/franka_panda/panda_constants.h | 2 ++ manipulation/franka_panda/panda_status_receiver.cc | 2 ++ manipulation/franka_panda/panda_status_receiver.h | 2 ++ manipulation/franka_panda/panda_status_sender.cc | 2 ++ manipulation/franka_panda/panda_status_sender.h | 2 ++ manipulation/franka_panda/test/panda_command_receiver_test.cc | 4 ++-- manipulation/franka_panda/test/panda_command_sender_test.cc | 4 ++-- manipulation/franka_panda/test/panda_status_receiver_test.cc | 4 ++-- manipulation/franka_panda/test/panda_status_sender_test.cc | 4 ++-- 14 files changed, 28 insertions(+), 10 deletions(-) diff --git a/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc b/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc index 18cabb7645fa..c06cf9c8f625 100644 --- a/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc +++ b/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc @@ -16,9 +16,9 @@ using systems::LeafSystem; void DefineManipulationFrankaPanda(py::module m) { // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. - using namespace drake::manipulation; + using namespace drake::manipulation::franka_panda; constexpr auto& doc = - pydrake_doc_manipulation_franka_panda.drake.manipulation; + pydrake_doc_manipulation_franka_panda.drake.manipulation.franka_panda; // Constants. m.attr("kPandaArmNumJoints") = kPandaArmNumJoints; diff --git a/manipulation/franka_panda/panda_command_receiver.cc b/manipulation/franka_panda/panda_command_receiver.cc index d6d478e88f99..aec42bd811af 100644 --- a/manipulation/franka_panda/panda_command_receiver.cc +++ b/manipulation/franka_panda/panda_command_receiver.cc @@ -9,6 +9,7 @@ namespace drake { namespace manipulation { +namespace franka_panda { using drake::lcmt_panda_command; using drake::lcmt_panda_status; @@ -265,5 +266,6 @@ void PandaCommandReceiver::CalcTorqueOutput(const Context& context, message.joint_torque.data(), message.joint_torque.size())); } +} // namespace franka_panda } // namespace manipulation } // namespace drake diff --git a/manipulation/franka_panda/panda_command_receiver.h b/manipulation/franka_panda/panda_command_receiver.h index 828fa37153a1..68abde718a6b 100644 --- a/manipulation/franka_panda/panda_command_receiver.h +++ b/manipulation/franka_panda/panda_command_receiver.h @@ -9,6 +9,7 @@ namespace drake { namespace manipulation { +namespace franka_panda { /// Handles lcmt_panda_command message from a LcmSubscriberSystem. /// @@ -117,5 +118,6 @@ class PandaCommandReceiver final : public drake::systems::LeafSystem { const drake::systems::OutputPort* commanded_torque_output_{}; }; +} // namespace franka_panda } // namespace manipulation } // namespace drake diff --git a/manipulation/franka_panda/panda_command_sender.cc b/manipulation/franka_panda/panda_command_sender.cc index 037ced7c3dd1..30926cec10a5 100644 --- a/manipulation/franka_panda/panda_command_sender.cc +++ b/manipulation/franka_panda/panda_command_sender.cc @@ -8,6 +8,7 @@ namespace drake { namespace manipulation { +namespace franka_panda { using drake::lcmt_panda_command; using drake::lcmt_panda_status; @@ -105,5 +106,6 @@ void PandaCommandSender::CalcOutput(const Context& context, &command.joint_torque); } +} // namespace franka_panda } // namespace manipulation } // namespace drake diff --git a/manipulation/franka_panda/panda_command_sender.h b/manipulation/franka_panda/panda_command_sender.h index c7225b7fe0ab..c945196b3eee 100644 --- a/manipulation/franka_panda/panda_command_sender.h +++ b/manipulation/franka_panda/panda_command_sender.h @@ -8,6 +8,7 @@ namespace drake { namespace manipulation { +namespace franka_panda { /// Creates and outputs lcmt_panda_command messages. /// @@ -61,5 +62,6 @@ class PandaCommandSender final : public drake::systems::LeafSystem { const drake::systems::InputPort* torque_input_port_{}; }; +} // namespace franka_panda } // namespace manipulation } // namespace drake diff --git a/manipulation/franka_panda/panda_constants.h b/manipulation/franka_panda/panda_constants.h index ccb0462677e5..1fda944bd782 100644 --- a/manipulation/franka_panda/panda_constants.h +++ b/manipulation/franka_panda/panda_constants.h @@ -2,8 +2,10 @@ namespace drake { namespace manipulation { +namespace franka_panda { const int kPandaArmNumJoints = 7; +} // namespace franka_panda } // namespace manipulation } // namespace drake diff --git a/manipulation/franka_panda/panda_status_receiver.cc b/manipulation/franka_panda/panda_status_receiver.cc index 84a2ded24d57..4c3d09a5c4c4 100644 --- a/manipulation/franka_panda/panda_status_receiver.cc +++ b/manipulation/franka_panda/panda_status_receiver.cc @@ -4,6 +4,7 @@ namespace drake { namespace manipulation { +namespace franka_panda { using drake::lcmt_panda_status; using drake::Value; @@ -92,5 +93,6 @@ void PandaStatusReceiver::CalcLcmOutput(const Context& context, } } +} // namespace franka_panda } // namespace manipulation } // namespace drake diff --git a/manipulation/franka_panda/panda_status_receiver.h b/manipulation/franka_panda/panda_status_receiver.h index ba0568c4a7fd..77e2d249e7ae 100644 --- a/manipulation/franka_panda/panda_status_receiver.h +++ b/manipulation/franka_panda/panda_status_receiver.h @@ -10,6 +10,7 @@ namespace drake { namespace manipulation { +namespace franka_panda { /// Handles lcmt_panda_status messages from a LcmSubscriberSystem. /// @@ -64,5 +65,6 @@ class PandaStatusReceiver final : public drake::systems::LeafSystem { const int num_joints_; }; +} // namespace franka_panda } // namespace manipulation } // namespace drake diff --git a/manipulation/franka_panda/panda_status_sender.cc b/manipulation/franka_panda/panda_status_sender.cc index f1cc92329094..37b68fdfc590 100644 --- a/manipulation/franka_panda/panda_status_sender.cc +++ b/manipulation/franka_panda/panda_status_sender.cc @@ -2,6 +2,7 @@ namespace drake { namespace manipulation { +namespace franka_panda { using drake::lcmt_panda_status; using drake::systems::Context; @@ -141,5 +142,6 @@ void PandaStatusSender::CalcOutput(const Context& context, } } +} // namespace franka_panda } // namespace manipulation } // namespace drake diff --git a/manipulation/franka_panda/panda_status_sender.h b/manipulation/franka_panda/panda_status_sender.h index 6a915f48f0c1..19fee28d8d22 100644 --- a/manipulation/franka_panda/panda_status_sender.h +++ b/manipulation/franka_panda/panda_status_sender.h @@ -8,6 +8,7 @@ namespace drake { namespace manipulation { +namespace franka_panda { /// Creates and outputs lcmt_panda_status messages. /// @@ -64,5 +65,6 @@ class PandaStatusSender final : public drake::systems::LeafSystem { const Eigen::VectorXd zero_vector_; }; +} // namespace franka_panda } // namespace manipulation } // namespace drake diff --git a/manipulation/franka_panda/test/panda_command_receiver_test.cc b/manipulation/franka_panda/test/panda_command_receiver_test.cc index 09947859d6ab..5bc5a5554c8d 100644 --- a/manipulation/franka_panda/test/panda_command_receiver_test.cc +++ b/manipulation/franka_panda/test/panda_command_receiver_test.cc @@ -10,7 +10,7 @@ namespace drake { namespace manipulation { -namespace { +namespace franka_panda { using drake::CompareMatrices; using drake::lcmt_panda_command; @@ -163,6 +163,6 @@ TEST_F(PandaCommandReceiverTest, BadControlMode) { EXPECT_THROW(PandaCommandReceiver(7, 0x9999), std::exception); } -} // namespace +} // namespace franka_panda } // namespace manipulation } // namespace drake diff --git a/manipulation/franka_panda/test/panda_command_sender_test.cc b/manipulation/franka_panda/test/panda_command_sender_test.cc index 24dfcd1a739b..8b57d9b17c1f 100644 --- a/manipulation/franka_panda/test/panda_command_sender_test.cc +++ b/manipulation/franka_panda/test/panda_command_sender_test.cc @@ -11,7 +11,7 @@ namespace drake { namespace manipulation { -namespace { +namespace franka_panda { using drake::lcmt_panda_command; using drake::lcmt_panda_status; @@ -137,6 +137,6 @@ TEST_F(PandaCommandSenderTest, BadControlMode) { EXPECT_THROW(PandaCommandSender(7, 0x9999), std::exception); } -} // namespace +} // namespace franka_panda } // namespace manipulation } // namespace drake diff --git a/manipulation/franka_panda/test/panda_status_receiver_test.cc b/manipulation/franka_panda/test/panda_status_receiver_test.cc index 02d4a0728091..334d89d86b4a 100644 --- a/manipulation/franka_panda/test/panda_status_receiver_test.cc +++ b/manipulation/franka_panda/test/panda_status_receiver_test.cc @@ -11,7 +11,7 @@ namespace drake { namespace manipulation { -namespace { +namespace franka_panda { using drake::CompareMatrices; using drake::lcmt_panda_status; @@ -132,6 +132,6 @@ TEST_F(PandaStatusReceiverTest, MallocTest) { } } -} // namespace +} // namespace franka_panda } // namespace manipulation } // namespace drake diff --git a/manipulation/franka_panda/test/panda_status_sender_test.cc b/manipulation/franka_panda/test/panda_status_sender_test.cc index 6e68f64ace86..f5c51a63925b 100644 --- a/manipulation/franka_panda/test/panda_status_sender_test.cc +++ b/manipulation/franka_panda/test/panda_status_sender_test.cc @@ -8,7 +8,7 @@ namespace drake { namespace manipulation { -namespace { +namespace franka_panda { using drake::lcmt_panda_status; using drake::systems::Context; @@ -94,6 +94,6 @@ TEST_F(PandaStatusSenderTest, AcceptanceTest) { EXPECT_EQ(output().joint_torque_external, as_vector(t0_external)); } -} // namespace +} // namespace franka_panda } // namespace manipulation } // namespace drake From 52308a079cc1c1a0d2ba2d78304ca9be67b8fa4b Mon Sep 17 00:00:00 2001 From: Peter Date: Thu, 9 Oct 2025 20:35:43 -0400 Subject: [PATCH 03/12] first draft complete --- .../manipulation_franka_panda.h | 334 ++++++++++++++++++ bindings/pydrake/manipulation/BUILD.bazel | 2 +- .../manipulation_py_franka_panda.cc | 13 +- .../manipulation/test/franka_panda_test.py | 23 +- tools/install/libdrake/build_components.bzl | 1 + 5 files changed, 357 insertions(+), 16 deletions(-) create mode 100644 bindings/generated_docstrings/manipulation_franka_panda.h diff --git a/bindings/generated_docstrings/manipulation_franka_panda.h b/bindings/generated_docstrings/manipulation_franka_panda.h new file mode 100644 index 000000000000..0c93a778714c --- /dev/null +++ b/bindings/generated_docstrings/manipulation_franka_panda.h @@ -0,0 +1,334 @@ +#pragma once + +// GENERATED FILE DO NOT EDIT +// This file contains docstrings for the Python bindings that were +// automatically extracted by mkdoc.py. + +#include +#include + +#if defined(__GNUG__) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" +#endif + +// #include "drake/manipulation/franka_panda/panda_command_receiver.h" +// #include "drake/manipulation/franka_panda/panda_command_sender.h" +// #include "drake/manipulation/franka_panda/panda_constants.h" +// #include "drake/manipulation/franka_panda/panda_status_receiver.h" +// #include "drake/manipulation/franka_panda/panda_status_sender.h" + +// Symbol: pydrake_doc_manipulation_franka_panda +constexpr struct /* pydrake_doc_manipulation_franka_panda */ { + // Symbol: drake + struct /* drake */ { + // Symbol: drake::manipulation + struct /* manipulation */ { + // Symbol: drake::manipulation::franka_panda + struct /* franka_panda */ { + // Symbol: drake::manipulation::franka_panda::PandaCommandReceiver + struct /* PandaCommandReceiver */ { + // Source: drake/manipulation/franka_panda/panda_command_receiver.h + const char* doc = +R"""(Handles lcmt_panda_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(). + +It has one required input port, "lcmt_panda_command". It has several +output ports, each one of size num_joints. + +.. pydrake_system:: + + name: PandaCommandReceiver + input_ports: + - lcmt_panda_command + - position_measured + output_ports: + - position (*) + - velocity (*) + - torque (*) + +(*) Each output port is present iff the control_mode passed to the +constructor set the corresponding CONTROL_MODE bit. + +Prior to receiving a valid lcmt_panda_command message, the "position" +output (if present) initially feeds through from the +"position_measured" input, and both the "velocity" and "torque" +outputs (if present) are zero. + +If discrete update events are enabled (e.g., during simulation), the +system latches the "position_measured" input into state during the +first event, and the "position" output (if present) comes from the +latched state, not the input. + +The lcmt_panda_command input must match the num_joints and +control_mode that were passed to the constructor: the message's +control_mode_expected must be set to the same value as the +constructor's control_mode, and the message's position, velocity, +torque vectors must be sized to match the constructor's num_joint iff +the corresponding bit of the control_mode is set or must be zero-sized +otherwise. TODO(jeremy.nimmer) The control_mode_expected is not +actually validated yet at runtime, but will be in the future (once we +fix our code to stop setting it incorrectly).)"""; + // Symbol: drake::manipulation::franka_panda::PandaCommandReceiver::LatchInitialPosition + struct /* LatchInitialPosition */ { + // Source: drake/manipulation/franka_panda/panda_command_receiver.h + const char* doc = +R"""((Advanced.) Copies the current "position_measured" input 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. + +Precondition: + the control_mode has the POSITION bit set)"""; + } LatchInitialPosition; + // Symbol: drake::manipulation::franka_panda::PandaCommandReceiver::PandaCommandReceiver + struct /* ctor */ { + // Source: drake/manipulation/franka_panda/panda_command_receiver.h + const char* doc = +R"""(Parameter ``control_mode``: + is a bitset of one or more control mode constants defined in + lcmt_panda_status::CONTROL_MODE_{POSITION,VELOCITY,TORQUE}.)"""; + } ctor; + // Symbol: drake::manipulation::franka_panda::PandaCommandReceiver::get_commanded_position_output_port + struct /* get_commanded_position_output_port */ { + // Source: drake/manipulation/franka_panda/panda_command_receiver.h + const char* doc = R"""()"""; + } get_commanded_position_output_port; + // Symbol: drake::manipulation::franka_panda::PandaCommandReceiver::get_commanded_torque_output_port + struct /* get_commanded_torque_output_port */ { + // Source: drake/manipulation/franka_panda/panda_command_receiver.h + const char* doc = R"""()"""; + } get_commanded_torque_output_port; + // Symbol: drake::manipulation::franka_panda::PandaCommandReceiver::get_commanded_velocity_output_port + struct /* get_commanded_velocity_output_port */ { + // Source: drake/manipulation/franka_panda/panda_command_receiver.h + const char* doc = R"""()"""; + } get_commanded_velocity_output_port; + // Symbol: drake::manipulation::franka_panda::PandaCommandReceiver::get_message_input_port + struct /* get_message_input_port */ { + // Source: drake/manipulation/franka_panda/panda_command_receiver.h + const char* doc = R"""()"""; + } get_message_input_port; + // Symbol: drake::manipulation::franka_panda::PandaCommandReceiver::get_position_measured_input_port + struct /* get_position_measured_input_port */ { + // Source: drake/manipulation/franka_panda/panda_command_receiver.h + const char* doc = R"""()"""; + } get_position_measured_input_port; + } PandaCommandReceiver; + // Symbol: drake::manipulation::franka_panda::PandaCommandSender + struct /* PandaCommandSender */ { + // Source: drake/manipulation/franka_panda/panda_command_sender.h + const char* doc = +R"""(Creates and outputs lcmt_panda_command messages. + +Note that this system does not actually send the message an LCM +channel. To send the message, the output of this system should be +connected to a LcmPublisherSystem::Make(). + +This system has vector-valued input ports, each one of size +num_joints. + +This system has one abstract-valued output port of type +lcmt_panda_command. + +.. pydrake_system:: + + name: PandaCommandSender + input_ports: + - position (*) + - velocity (*) + - torque (*) + output_ports: + - lcmt_panda_command + +(*) Each input port is present if it's relevant to the control_mode +passed to the constructor. + +See also: + ``lcmt_panda_command.lcm`` for additional documentation.)"""; + // Symbol: drake::manipulation::franka_panda::PandaCommandSender::PandaCommandSender + struct /* ctor */ { + // Source: drake/manipulation/franka_panda/panda_command_sender.h + const char* doc = +R"""(Parameter ``control_mode``: + is a bitset of one or more control mode constants defined in + lcmt_panda_status::CONTROL_MODE_{POSITION,VELOCITY,TORQUE}.)"""; + } ctor; + // Symbol: drake::manipulation::franka_panda::PandaCommandSender::get_position_input_port + struct /* get_position_input_port */ { + // Source: drake/manipulation/franka_panda/panda_command_sender.h + const char* doc = R"""()"""; + } get_position_input_port; + // Symbol: drake::manipulation::franka_panda::PandaCommandSender::get_torque_input_port + struct /* get_torque_input_port */ { + // Source: drake/manipulation/franka_panda/panda_command_sender.h + const char* doc = R"""()"""; + } get_torque_input_port; + // Symbol: drake::manipulation::franka_panda::PandaCommandSender::get_velocity_input_port + struct /* get_velocity_input_port */ { + // Source: drake/manipulation/franka_panda/panda_command_sender.h + const char* doc = R"""()"""; + } get_velocity_input_port; + } PandaCommandSender; + // Symbol: drake::manipulation::franka_panda::PandaStatusReceiver + struct /* PandaStatusReceiver */ { + // Source: drake/manipulation/franka_panda/panda_status_receiver.h + const char* doc = +R"""(@system name: PandaStatusReceiver input_ports: - lcmt_panda_status +output_ports: - position_commanded - position_measured - +velocity_commanded - velocity_measured - acceleration_commanded - +torque_commanded - torque_measured - torque_external @endsystem + +See also: + ``lcmt_panda_status.lcm`` for additional documentation.)"""; + // Symbol: drake::manipulation::franka_panda::PandaStatusReceiver::OutputPort + struct /* OutputPort */ { + // Source: drake/manipulation/franka_panda/panda_status_receiver.h + const char* doc = R"""()"""; + } OutputPort; + // Symbol: drake::manipulation::franka_panda::PandaStatusReceiver::PandaStatusReceiver + struct /* ctor */ { + // Source: drake/manipulation/franka_panda/panda_status_receiver.h + const char* doc = R"""()"""; + } ctor; + // Symbol: drake::manipulation::franka_panda::PandaStatusReceiver::get_acceleration_commanded_output_port + struct /* get_acceleration_commanded_output_port */ { + // Source: drake/manipulation/franka_panda/panda_status_receiver.h + const char* doc = R"""()"""; + } get_acceleration_commanded_output_port; + // Symbol: drake::manipulation::franka_panda::PandaStatusReceiver::get_position_commanded_output_port + struct /* get_position_commanded_output_port */ { + // Source: drake/manipulation/franka_panda/panda_status_receiver.h + const char* doc = R"""()"""; + } get_position_commanded_output_port; + // Symbol: drake::manipulation::franka_panda::PandaStatusReceiver::get_position_measured_output_port + struct /* get_position_measured_output_port */ { + // Source: drake/manipulation/franka_panda/panda_status_receiver.h + const char* doc = R"""()"""; + } get_position_measured_output_port; + // Symbol: drake::manipulation::franka_panda::PandaStatusReceiver::get_torque_commanded_output_port + struct /* get_torque_commanded_output_port */ { + // Source: drake/manipulation/franka_panda/panda_status_receiver.h + const char* doc = R"""()"""; + } get_torque_commanded_output_port; + // Symbol: drake::manipulation::franka_panda::PandaStatusReceiver::get_torque_external_output_port + struct /* get_torque_external_output_port */ { + // Source: drake/manipulation/franka_panda/panda_status_receiver.h + const char* doc = R"""()"""; + } get_torque_external_output_port; + // Symbol: drake::manipulation::franka_panda::PandaStatusReceiver::get_torque_measured_output_port + struct /* get_torque_measured_output_port */ { + // Source: drake/manipulation/franka_panda/panda_status_receiver.h + const char* doc = R"""()"""; + } get_torque_measured_output_port; + // Symbol: drake::manipulation::franka_panda::PandaStatusReceiver::get_velocity_commanded_output_port + struct /* get_velocity_commanded_output_port */ { + // Source: drake/manipulation/franka_panda/panda_status_receiver.h + const char* doc = R"""()"""; + } get_velocity_commanded_output_port; + // Symbol: drake::manipulation::franka_panda::PandaStatusReceiver::get_velocity_measured_output_port + struct /* get_velocity_measured_output_port */ { + // Source: drake/manipulation/franka_panda/panda_status_receiver.h + const char* doc = R"""()"""; + } get_velocity_measured_output_port; + } PandaStatusReceiver; + // Symbol: drake::manipulation::franka_panda::PandaStatusSender + struct /* PandaStatusSender */ { + // Source: drake/manipulation/franka_panda/panda_status_sender.h + const char* doc = +R"""(Creates and outputs lcmt_panda_status messages. + +Note that this system does not actually send the message an LCM +channel. To send the message, the output of this system should be +connected to a LcmPublisherSystem::Make(). + +This system has many vector-valued input ports, each of which has +exactly num_joints elements. + +This system has one abstract-valued output port of type +lcmt_panda_status. + +This system is presently only used in simulation. The robot hardware +drivers publish directly to LCM and do not make use of this system. + +.. pydrake_system:: + + name: PandaStatusSender + input_ports: + - position_commanded (optional) + - position_measured + - velocity_commanded (optional) + - velocity_measured (optional) + - acceleration_commanded (optional) + - torque_commanded + - torque_measured (optional) + - torque_external (optional) + output_ports: + - lcmt_panda_status + +See also: + ``lcmt_panda_status.lcm`` for additional documentation.)"""; + // Symbol: drake::manipulation::franka_panda::PandaStatusSender::InputPort + struct /* InputPort */ { + // Source: drake/manipulation/franka_panda/panda_status_sender.h + const char* doc = R"""()"""; + } InputPort; + // Symbol: drake::manipulation::franka_panda::PandaStatusSender::PandaStatusSender + struct /* ctor */ { + // Source: drake/manipulation/franka_panda/panda_status_sender.h + const char* doc = R"""()"""; + } ctor; + // Symbol: drake::manipulation::franka_panda::PandaStatusSender::get_acceleration_commanded_input_port + struct /* get_acceleration_commanded_input_port */ { + // Source: drake/manipulation/franka_panda/panda_status_sender.h + const char* doc = R"""()"""; + } get_acceleration_commanded_input_port; + // Symbol: drake::manipulation::franka_panda::PandaStatusSender::get_position_commanded_input_port + struct /* get_position_commanded_input_port */ { + // Source: drake/manipulation/franka_panda/panda_status_sender.h + const char* doc = R"""()"""; + } get_position_commanded_input_port; + // Symbol: drake::manipulation::franka_panda::PandaStatusSender::get_position_measured_input_port + struct /* get_position_measured_input_port */ { + // Source: drake/manipulation/franka_panda/panda_status_sender.h + const char* doc = R"""()"""; + } get_position_measured_input_port; + // Symbol: drake::manipulation::franka_panda::PandaStatusSender::get_torque_commanded_input_port + struct /* get_torque_commanded_input_port */ { + // Source: drake/manipulation/franka_panda/panda_status_sender.h + const char* doc = R"""()"""; + } get_torque_commanded_input_port; + // Symbol: drake::manipulation::franka_panda::PandaStatusSender::get_torque_external_input_port + struct /* get_torque_external_input_port */ { + // Source: drake/manipulation/franka_panda/panda_status_sender.h + const char* doc = R"""()"""; + } get_torque_external_input_port; + // Symbol: drake::manipulation::franka_panda::PandaStatusSender::get_torque_measured_input_port + struct /* get_torque_measured_input_port */ { + // Source: drake/manipulation/franka_panda/panda_status_sender.h + const char* doc = R"""()"""; + } get_torque_measured_input_port; + // Symbol: drake::manipulation::franka_panda::PandaStatusSender::get_velocity_commanded_input_port + struct /* get_velocity_commanded_input_port */ { + // Source: drake/manipulation/franka_panda/panda_status_sender.h + const char* doc = R"""()"""; + } get_velocity_commanded_input_port; + // Symbol: drake::manipulation::franka_panda::PandaStatusSender::get_velocity_measured_input_port + struct /* get_velocity_measured_input_port */ { + // Source: drake/manipulation/franka_panda/panda_status_sender.h + const char* doc = R"""()"""; + } get_velocity_measured_input_port; + } PandaStatusSender; + } franka_panda; + } manipulation; + } drake; +} pydrake_doc_manipulation_franka_panda; + +#if defined(__GNUG__) +#pragma GCC diagnostic pop +#endif diff --git a/bindings/pydrake/manipulation/BUILD.bazel b/bindings/pydrake/manipulation/BUILD.bazel index 9ce4b31ab823..e4b05dd23b4b 100644 --- a/bindings/pydrake/manipulation/BUILD.bazel +++ b/bindings/pydrake/manipulation/BUILD.bazel @@ -23,8 +23,8 @@ PACKAGE_INFO = get_pybind_package_info("//bindings") drake_pybind_library( name = "manipulation", cc_deps = [ - "//bindings/generated_docstrings:manipulation_kuka_iiwa", "//bindings/generated_docstrings:manipulation_franka_panda", + "//bindings/generated_docstrings:manipulation_kuka_iiwa", "//bindings/generated_docstrings:manipulation_schunk_wsg", "//bindings/generated_docstrings:manipulation_util", "//bindings/pydrake/common:eigen_pybind", diff --git a/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc b/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc index c06cf9c8f625..8d70c4328426 100644 --- a/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc +++ b/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc @@ -2,6 +2,7 @@ #include "drake/bindings/pydrake/common/serialize_pybind.h" #include "drake/bindings/pydrake/manipulation/manipulation_py.h" #include "drake/bindings/pydrake/pydrake_pybind.h" +#include "drake/lcmt_panda_status.hpp" #include "drake/manipulation/franka_panda/panda_command_receiver.h" #include "drake/manipulation/franka_panda/panda_command_sender.h" #include "drake/manipulation/franka_panda/panda_constants.h" @@ -23,10 +24,18 @@ void DefineManipulationFrankaPanda(py::module m) { // Constants. m.attr("kPandaArmNumJoints") = kPandaArmNumJoints; + // Control mode constants from lcmt_panda_status + m.attr("CONTROL_MODE_POSITION") = + drake::lcmt_panda_status::CONTROL_MODE_POSITION; + m.attr("CONTROL_MODE_VELOCITY") = + drake::lcmt_panda_status::CONTROL_MODE_VELOCITY; + m.attr("CONTROL_MODE_TORQUE") = drake::lcmt_panda_status::CONTROL_MODE_TORQUE; + { using Class = PandaCommandReceiver; constexpr auto& cls_doc = doc.PandaCommandReceiver; - py::class_>(m, "PandaCommandReceiver", cls_doc.doc) + py::class_>( + m, "PandaCommandReceiver", cls_doc.doc) .def(py::init(), py::arg("num_joints") = kPandaArmNumJoints, py::arg("control_mode"), cls_doc.ctor.doc) .def("LatchInitialPosition", @@ -145,4 +154,4 @@ void DefineManipulationFrankaPanda(py::module m) { } // namespace internal } // namespace pydrake -} // namespace drake \ No newline at end of file +} // namespace drake diff --git a/bindings/pydrake/manipulation/test/franka_panda_test.py b/bindings/pydrake/manipulation/test/franka_panda_test.py index 7656220dcdb4..f4d04415bb5d 100644 --- a/bindings/pydrake/manipulation/test/franka_panda_test.py +++ b/bindings/pydrake/manipulation/test/franka_panda_test.py @@ -3,10 +3,7 @@ import pydrake.manipulation as mut import unittest -import numpy as np -from pydrake.lcmt_panda_command import lcmt_panda_command -from pydrake.lcmt_panda_status import lcmt_panda_status from pydrake.systems.framework import ( InputPort, OutputPort, @@ -19,8 +16,8 @@ def test_constants(self): def test_franka_panda_command_receiver(self): # Test with position and torque control mode - control_mode = (lcmt_panda_status.CONTROL_MODE_POSITION | - lcmt_panda_status.CONTROL_MODE_TORQUE) + control_mode = (mut.CONTROL_MODE_POSITION | + mut.CONTROL_MODE_TORQUE) command_rec = mut.PandaCommandReceiver( num_joints=mut.kPandaArmNumJoints, control_mode=control_mode) @@ -34,9 +31,9 @@ def test_franka_panda_command_receiver(self): command_rec.get_commanded_torque_output_port(), OutputPort) # Test with position, velocity, and torque control mode - control_mode_all = (lcmt_panda_status.CONTROL_MODE_POSITION | - lcmt_panda_status.CONTROL_MODE_VELOCITY | - lcmt_panda_status.CONTROL_MODE_TORQUE) + control_mode_all = (mut.CONTROL_MODE_POSITION | + mut.CONTROL_MODE_VELOCITY | + mut.CONTROL_MODE_TORQUE) command_rec_all = mut.PandaCommandReceiver( num_joints=mut.kPandaArmNumJoints, control_mode=control_mode_all) @@ -45,8 +42,8 @@ def test_franka_panda_command_receiver(self): def test_franka_panda_command_sender(self): # Test with position and torque control mode - control_mode = (lcmt_panda_status.CONTROL_MODE_POSITION | - lcmt_panda_status.CONTROL_MODE_TORQUE) + control_mode = (mut.CONTROL_MODE_POSITION | + mut.CONTROL_MODE_TORQUE) command_send = mut.PandaCommandSender( num_joints=mut.kPandaArmNumJoints, control_mode=control_mode) @@ -56,9 +53,9 @@ def test_franka_panda_command_sender(self): command_send.get_torque_input_port(), InputPort) # Test with position, velocity, and torque control mode - control_mode_all = (lcmt_panda_status.CONTROL_MODE_POSITION | - lcmt_panda_status.CONTROL_MODE_VELOCITY | - lcmt_panda_status.CONTROL_MODE_TORQUE) + control_mode_all = (mut.CONTROL_MODE_POSITION | + mut.CONTROL_MODE_VELOCITY | + mut.CONTROL_MODE_TORQUE) command_send_all = mut.PandaCommandSender( num_joints=mut.kPandaArmNumJoints, control_mode=control_mode_all) diff --git a/tools/install/libdrake/build_components.bzl b/tools/install/libdrake/build_components.bzl index 253f30212e06..078a36cf9194 100644 --- a/tools/install/libdrake/build_components.bzl +++ b/tools/install/libdrake/build_components.bzl @@ -44,6 +44,7 @@ LIBDRAKE_COMPONENTS = [ "//geometry/render_gltf_client", "//geometry/render_vtk", "//lcm", + "//manipulation/franka_panda", "//manipulation/kinova_jaco", "//manipulation/kuka_iiwa", "//manipulation/schunk_wsg", From fdaf893f62655f2860cde86eefddd070d7a58def Mon Sep 17 00:00:00 2001 From: Peter Date: Fri, 10 Oct 2025 11:16:16 -0400 Subject: [PATCH 04/12] add panda control mode enum to panda constants --- .../manipulation_py_franka_panda.cc | 14 ++++---- .../manipulation/test/franka_panda_test.py | 20 +++++------ .../franka_panda/panda_command_receiver.cc | 34 ++++++++++--------- .../franka_panda/panda_command_sender.cc | 13 +++---- manipulation/franka_panda/panda_constants.h | 22 ++++++++++++ .../test/panda_command_receiver_test.cc | 16 ++++----- .../test/panda_command_sender_test.cc | 21 +++++------- 7 files changed, 81 insertions(+), 59 deletions(-) diff --git a/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc b/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc index 8d70c4328426..c0db42b2035b 100644 --- a/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc +++ b/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc @@ -24,12 +24,14 @@ void DefineManipulationFrankaPanda(py::module m) { // Constants. m.attr("kPandaArmNumJoints") = kPandaArmNumJoints; - // Control mode constants from lcmt_panda_status - m.attr("CONTROL_MODE_POSITION") = - drake::lcmt_panda_status::CONTROL_MODE_POSITION; - m.attr("CONTROL_MODE_VELOCITY") = - drake::lcmt_panda_status::CONTROL_MODE_VELOCITY; - m.attr("CONTROL_MODE_TORQUE") = drake::lcmt_panda_status::CONTROL_MODE_TORQUE; + // Control mode constants + auto control_mode = m.def_submodule("PandaControlMode"); + control_mode.doc() = + "Control modes for the Panda robot. " + "These can be bitwise OR'd together."; + control_mode.attr("kPosition") = PandaControlMode::kPosition; + control_mode.attr("kVelocity") = PandaControlMode::kVelocity; + control_mode.attr("kTorque") = PandaControlMode::kTorque; { using Class = PandaCommandReceiver; diff --git a/bindings/pydrake/manipulation/test/franka_panda_test.py b/bindings/pydrake/manipulation/test/franka_panda_test.py index f4d04415bb5d..89cc741a485b 100644 --- a/bindings/pydrake/manipulation/test/franka_panda_test.py +++ b/bindings/pydrake/manipulation/test/franka_panda_test.py @@ -16,8 +16,8 @@ def test_constants(self): def test_franka_panda_command_receiver(self): # Test with position and torque control mode - control_mode = (mut.CONTROL_MODE_POSITION | - mut.CONTROL_MODE_TORQUE) + control_mode = (mut.PandaControlMode.kPosition | + mut.PandaControlMode.kTorque) command_rec = mut.PandaCommandReceiver( num_joints=mut.kPandaArmNumJoints, control_mode=control_mode) @@ -31,9 +31,9 @@ def test_franka_panda_command_receiver(self): command_rec.get_commanded_torque_output_port(), OutputPort) # Test with position, velocity, and torque control mode - control_mode_all = (mut.CONTROL_MODE_POSITION | - mut.CONTROL_MODE_VELOCITY | - mut.CONTROL_MODE_TORQUE) + control_mode_all = (mut.PandaControlMode.kPosition | + mut.PandaControlMode.kVelocity | + mut.PandaControlMode.kTorque) command_rec_all = mut.PandaCommandReceiver( num_joints=mut.kPandaArmNumJoints, control_mode=control_mode_all) @@ -42,8 +42,8 @@ def test_franka_panda_command_receiver(self): def test_franka_panda_command_sender(self): # Test with position and torque control mode - control_mode = (mut.CONTROL_MODE_POSITION | - mut.CONTROL_MODE_TORQUE) + control_mode = (mut.PandaControlMode.kPosition | + mut.PandaControlMode.kTorque) command_send = mut.PandaCommandSender( num_joints=mut.kPandaArmNumJoints, control_mode=control_mode) @@ -53,9 +53,9 @@ def test_franka_panda_command_sender(self): command_send.get_torque_input_port(), InputPort) # Test with position, velocity, and torque control mode - control_mode_all = (mut.CONTROL_MODE_POSITION | - mut.CONTROL_MODE_VELOCITY | - mut.CONTROL_MODE_TORQUE) + control_mode_all = (mut.PandaControlMode.kPosition | + mut.PandaControlMode.kVelocity | + mut.PandaControlMode.kTorque) command_send_all = mut.PandaCommandSender( num_joints=mut.kPandaArmNumJoints, control_mode=control_mode_all) diff --git a/manipulation/franka_panda/panda_command_receiver.cc b/manipulation/franka_panda/panda_command_receiver.cc index aec42bd811af..c169ca84b057 100644 --- a/manipulation/franka_panda/panda_command_receiver.cc +++ b/manipulation/franka_panda/panda_command_receiver.cc @@ -3,6 +3,8 @@ #include #include +#include "manipulation/franka_panda/panda_constants.h" + #include "drake/common/drake_throw.h" #include "drake/lcm/lcm_messages.h" #include "drake/lcmt_panda_status.hpp" @@ -35,7 +37,7 @@ PandaCommandReceiver::PandaCommandReceiver(int num_joints, int control_mode) // Even if we have not yet received a message, we still need to provide // default values for our output ports. We use a cache entry to compute // those values, by populating a default lcmt_panda_control message. - if (control_mode_ & lcmt_panda_status::CONTROL_MODE_POSITION) { + if (control_mode_ & PandaControlMode::kPosition) { // When in position control mode, the default is derived from the // "position_measured" input. latched_position_measured_is_set_ = DeclareDiscreteState(VectorXd::Zero(1)); @@ -62,24 +64,24 @@ PandaCommandReceiver::PandaCommandReceiver(int num_joints, int control_mode) {message_input_->ticket(), default_command_->ticket()}); int remaining = control_mode_; - if (control_mode_ & lcmt_panda_status::CONTROL_MODE_POSITION) { - remaining &= ~int{lcmt_panda_status::CONTROL_MODE_POSITION}; + if (control_mode_ & PandaControlMode::kPosition) { + remaining &= ~int{PandaControlMode::kPosition}; commanded_position_output_ = &DeclareVectorOutputPort("position", BasicVector(num_joints), &PandaCommandReceiver::CalcPositionOutput, {message_input_or_default_->ticket()}); } - if (control_mode_ & lcmt_panda_status::CONTROL_MODE_VELOCITY) { - remaining &= ~int{lcmt_panda_status::CONTROL_MODE_VELOCITY}; + if (control_mode_ & PandaControlMode::kVelocity) { + remaining &= ~int{PandaControlMode::kVelocity}; commanded_velocity_output_ = &DeclareVectorOutputPort("velocity", BasicVector(num_joints), &PandaCommandReceiver::CalcVelocityOutput, {message_input_or_default_->ticket()}); } - if (control_mode_ & lcmt_panda_status::CONTROL_MODE_TORQUE) { - remaining &= ~int{lcmt_panda_status::CONTROL_MODE_TORQUE}; + if (control_mode_ & PandaControlMode::kTorque) { + remaining &= ~int{PandaControlMode::kTorque}; commanded_torque_output_ = &DeclareVectorOutputPort("torque", BasicVector(num_joints), &PandaCommandReceiver::CalcTorqueOutput, @@ -126,7 +128,7 @@ PandaCommandReceiver::get_commanded_torque_output_port() const { void PandaCommandReceiver::LatchInitialPosition( const Context& context, DiscreteValues* result) const { - DRAKE_DEMAND(control_mode_ & lcmt_panda_status::CONTROL_MODE_POSITION); + DRAKE_DEMAND(control_mode_ & PandaControlMode::kPosition); 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; @@ -137,7 +139,7 @@ void PandaCommandReceiver::LatchInitialPosition( void PandaCommandReceiver::LatchInitialPosition( Context* context) const { DRAKE_THROW_UNLESS(context != nullptr); - DRAKE_THROW_UNLESS(control_mode_ & lcmt_panda_status::CONTROL_MODE_POSITION); + DRAKE_THROW_UNLESS(control_mode_ & PandaControlMode::kPosition); LatchInitialPosition(*context, &context->get_mutable_discrete_state()); } @@ -153,7 +155,7 @@ void PandaCommandReceiver::DoCalcNextUpdateTime( DRAKE_THROW_UNLESS(std::isinf(*time)); // If we're not using position control, then we have no state to latch. - if (!(control_mode_ & lcmt_panda_status::CONTROL_MODE_POSITION)) { + if (!(control_mode_ & PandaControlMode::kPosition)) { return; } @@ -178,7 +180,7 @@ void PandaCommandReceiver::CalcDefaultCommand( const Context& context, lcmt_panda_command* result) const { *result = {}; result->control_mode_expected = control_mode_; - if (control_mode_ & lcmt_panda_status::CONTROL_MODE_POSITION) { + if (control_mode_ & PandaControlMode::kPosition) { const BasicVector& latch_is_set = context.get_discrete_state(latched_position_measured_is_set_); const BasicVector& default_position = @@ -190,11 +192,11 @@ void PandaCommandReceiver::CalcDefaultCommand( result->num_joint_position = num_joints_; result->joint_position = {vec.data(), vec.data() + num_joints_}; } - if (control_mode_ & lcmt_panda_status::CONTROL_MODE_VELOCITY) { + if (control_mode_ & PandaControlMode::kVelocity) { result->num_joint_velocity = num_joints_; result->joint_velocity = std::vector(num_joints_, 0.0); } - if (control_mode_ & lcmt_panda_status::CONTROL_MODE_TORQUE) { + if (control_mode_ & PandaControlMode::kTorque) { result->num_joint_torque = num_joints_; result->joint_torque = std::vector(num_joints_, 0.0); } @@ -227,7 +229,7 @@ void CheckNumJoints(const std::string& system_name, const char* vector_name, void PandaCommandReceiver::CalcPositionOutput( const Context& context, BasicVector* output) const { - DRAKE_DEMAND(control_mode_ & lcmt_panda_status::CONTROL_MODE_POSITION); + DRAKE_DEMAND(control_mode_ & PandaControlMode::kPosition); const auto& message = message_input_or_default_->Eval(context); DRAKE_DEMAND(message.control_mode_expected == control_mode_); @@ -241,7 +243,7 @@ void PandaCommandReceiver::CalcPositionOutput( void PandaCommandReceiver::CalcVelocityOutput( const Context& context, BasicVector* output) const { - DRAKE_DEMAND(control_mode_ & lcmt_panda_status::CONTROL_MODE_VELOCITY); + DRAKE_DEMAND(control_mode_ & PandaControlMode::kVelocity); const auto& message = message_input_or_default_->Eval(context); DRAKE_DEMAND(message.control_mode_expected == control_mode_); @@ -255,7 +257,7 @@ void PandaCommandReceiver::CalcVelocityOutput( void PandaCommandReceiver::CalcTorqueOutput(const Context& context, BasicVector* output) const { - DRAKE_DEMAND(control_mode_ & lcmt_panda_status::CONTROL_MODE_TORQUE); + DRAKE_DEMAND(control_mode_ & PandaControlMode::kTorque); const auto& message = message_input_or_default_->Eval(context); DRAKE_DEMAND(message.control_mode_expected == control_mode_); diff --git a/manipulation/franka_panda/panda_command_sender.cc b/manipulation/franka_panda/panda_command_sender.cc index 30926cec10a5..1d008a7a5798 100644 --- a/manipulation/franka_panda/panda_command_sender.cc +++ b/manipulation/franka_panda/panda_command_sender.cc @@ -2,6 +2,7 @@ #include +#include "manipulation/franka_panda/panda_constants.h" #include #include "drake/lcmt_panda_status.hpp" @@ -18,18 +19,18 @@ using drake::systems::kVectorValued; PandaCommandSender::PandaCommandSender(int num_joints, int control_mode) : num_joints_(num_joints), control_mode_(control_mode) { int remaining = control_mode_; - if (control_mode_ & lcmt_panda_status::CONTROL_MODE_POSITION) { - remaining &= ~int{lcmt_panda_status::CONTROL_MODE_POSITION}; + if (control_mode_ & PandaControlMode::kPosition) { + remaining &= ~int{PandaControlMode::kPosition}; position_input_port_ = &this->DeclareInputPort("position", kVectorValued, num_joints_); } - if (control_mode_ & lcmt_panda_status::CONTROL_MODE_VELOCITY) { - remaining &= ~int{lcmt_panda_status::CONTROL_MODE_VELOCITY}; + if (control_mode_ & PandaControlMode::kVelocity) { + remaining &= ~int{PandaControlMode::kVelocity}; velocity_input_port_ = &this->DeclareInputPort("velocity", kVectorValued, num_joints_); } - if (control_mode_ & lcmt_panda_status::CONTROL_MODE_TORQUE) { - remaining &= ~int{lcmt_panda_status::CONTROL_MODE_TORQUE}; + if (control_mode_ & PandaControlMode::kTorque) { + remaining &= ~int{PandaControlMode::kTorque}; torque_input_port_ = &this->DeclareInputPort("torque", kVectorValued, num_joints_); } diff --git a/manipulation/franka_panda/panda_constants.h b/manipulation/franka_panda/panda_constants.h index 1fda944bd782..5f80279b6f4b 100644 --- a/manipulation/franka_panda/panda_constants.h +++ b/manipulation/franka_panda/panda_constants.h @@ -1,11 +1,33 @@ #pragma once +#include "drake/lcmt_panda_status.hpp" + namespace drake { namespace manipulation { namespace franka_panda { const int kPandaArmNumJoints = 7; +/** Control modes for the Panda robot. These can be bitwise OR'd together + * to enable multiple control modes simultaneously. + * Values match lcmt_panda_status::CONTROL_MODE_* constants. */ +namespace PandaControlMode { +constexpr int kPosition = 1; +constexpr int kVelocity = 2; +constexpr int kTorque = 4; +} // namespace PandaControlMode + +// Ensure our constants match the LCM message definition +static_assert(PandaControlMode::kPosition == + drake::lcmt_panda_status::CONTROL_MODE_POSITION, + "PandaControlMode::kPosition must match LCM definition"); +static_assert(PandaControlMode::kVelocity == + drake::lcmt_panda_status::CONTROL_MODE_VELOCITY, + "PandaControlMode::kVelocity must match LCM definition"); +static_assert(PandaControlMode::kTorque == + drake::lcmt_panda_status::CONTROL_MODE_TORQUE, + "PandaControlMode::kTorque must match LCM definition"); + } // namespace franka_panda } // namespace manipulation } // namespace drake diff --git a/manipulation/franka_panda/test/panda_command_receiver_test.cc b/manipulation/franka_panda/test/panda_command_receiver_test.cc index 5bc5a5554c8d..b568f65b11b7 100644 --- a/manipulation/franka_panda/test/panda_command_receiver_test.cc +++ b/manipulation/franka_panda/test/panda_command_receiver_test.cc @@ -23,8 +23,8 @@ constexpr int N = kPandaArmNumJoints; class PandaCommandReceiverTest : public testing::Test { public: PandaCommandReceiverTest() - : dut_(kPandaArmNumJoints, lcmt_panda_status::CONTROL_MODE_POSITION | - lcmt_panda_status::CONTROL_MODE_TORQUE), + : dut_(kPandaArmNumJoints, + PandaControlMode::kPosition | PandaControlMode::kTorque), context_ptr_(dut_.CreateDefaultContext()), context_(*context_ptr_), fixed_input_(FixInput()) {} @@ -80,8 +80,8 @@ TEST_F(PandaCommandReceiverTest, AcceptanceTestWithMeasuredPositionInput) { const VectorXd q1 = VectorXd::LinSpaced(N, 0.3, 0.4); const VectorXd t1 = VectorXd::LinSpaced(N, 0.5, 0.6); lcmt_panda_command command{}; - command.control_mode_expected = lcmt_panda_status::CONTROL_MODE_POSITION | - lcmt_panda_status::CONTROL_MODE_TORQUE; + command.control_mode_expected = + PandaControlMode::kPosition | PandaControlMode::kTorque; command.utime = 0; command.num_joint_position = N; command.joint_position = {q1.data(), q1.data() + q1.size()}; @@ -126,8 +126,8 @@ TEST_F(PandaCommandReceiverTest, AcceptanceTestWithLatching) { const VectorXd v3 = VectorXd::LinSpaced(N, 0.5, 0.6); const VectorXd t3 = VectorXd::LinSpaced(N, 0.6, 0.7); lcmt_panda_command command{}; - command.control_mode_expected = lcmt_panda_status::CONTROL_MODE_POSITION | - lcmt_panda_status::CONTROL_MODE_TORQUE; + command.control_mode_expected = + PandaControlMode::kPosition | PandaControlMode::kTorque; command.utime = 0; command.num_joint_position = N; command.joint_position = {q3.data(), q3.data() + q3.size()}; @@ -140,7 +140,7 @@ TEST_F(PandaCommandReceiverTest, AcceptanceTestWithLatching) { GTEST_TEST(PandaCommandReceiverTestVelocity, VelocityControl) { PandaCommandReceiver receiver(kPandaArmNumJoints, - lcmt_panda_status::CONTROL_MODE_VELOCITY); + PandaControlMode::kVelocity); EXPECT_EQ(receiver.num_discrete_state_groups(), 0); EXPECT_THROW(receiver.get_commanded_position_output_port(), @@ -148,7 +148,7 @@ GTEST_TEST(PandaCommandReceiverTestVelocity, VelocityControl) { EXPECT_THROW(receiver.get_commanded_torque_output_port(), std::runtime_error); lcmt_panda_command command{}; - command.control_mode_expected = lcmt_panda_status::CONTROL_MODE_VELOCITY; + command.control_mode_expected = PandaControlMode::kVelocity; command.num_joint_velocity = kPandaArmNumJoints; command.joint_velocity.resize(kPandaArmNumJoints, 0.1); diff --git a/manipulation/franka_panda/test/panda_command_sender_test.cc b/manipulation/franka_panda/test/panda_command_sender_test.cc index 8b57d9b17c1f..f4e01573ede3 100644 --- a/manipulation/franka_panda/test/panda_command_sender_test.cc +++ b/manipulation/franka_panda/test/panda_command_sender_test.cc @@ -24,8 +24,7 @@ constexpr int N = kPandaArmNumJoints; class PandaCommandSenderTest : public testing::Test { public: PandaCommandSenderTest() - : dut_(N, lcmt_panda_status::CONTROL_MODE_POSITION | - lcmt_panda_status::CONTROL_MODE_TORQUE), + : dut_(N, PandaControlMode::kPosition | PandaControlMode::kTorque), context_ptr_(dut_.CreateDefaultContext()), context_(*context_ptr_) {} @@ -52,8 +51,7 @@ TEST_F(PandaCommandSenderTest, PositionAndTorque) { dut_.get_position_input_port().FixValue(&context_, q0_); dut_.get_torque_input_port().FixValue(&context_, t0_); EXPECT_EQ(output().control_mode_expected, - lcmt_panda_status::CONTROL_MODE_POSITION | - lcmt_panda_status::CONTROL_MODE_TORQUE); + PandaControlMode::kPosition | PandaControlMode::kTorque); EXPECT_EQ(output().num_joint_position, N); EXPECT_EQ(output().joint_position, std_q0_); EXPECT_EQ(output().num_joint_velocity, 0); @@ -63,13 +61,12 @@ TEST_F(PandaCommandSenderTest, PositionAndTorque) { } TEST_F(PandaCommandSenderTest, PositionOnly) { - const PandaCommandSender sender(N, lcmt_panda_status::CONTROL_MODE_POSITION); + const PandaCommandSender sender(N, PandaControlMode::kPosition); auto context = sender.CreateDefaultContext(); sender.get_position_input_port().FixValue(context.get(), q0_); const lcmt_panda_command output = sender.get_output_port().Eval(*context); - EXPECT_EQ(output.control_mode_expected, - lcmt_panda_status::CONTROL_MODE_POSITION); + EXPECT_EQ(output.control_mode_expected, PandaControlMode::kPosition); EXPECT_EQ(output.num_joint_position, N); EXPECT_EQ(output.joint_position, std_q0_); EXPECT_EQ(output.num_joint_velocity, 0); @@ -79,13 +76,12 @@ TEST_F(PandaCommandSenderTest, PositionOnly) { } TEST_F(PandaCommandSenderTest, VelocityOnly) { - const PandaCommandSender sender(N, lcmt_panda_status::CONTROL_MODE_VELOCITY); + const PandaCommandSender sender(N, PandaControlMode::kVelocity); auto context = sender.CreateDefaultContext(); sender.get_velocity_input_port().FixValue(context.get(), v0_); const lcmt_panda_command output = sender.get_output_port().Eval(*context); - EXPECT_EQ(output.control_mode_expected, - lcmt_panda_status::CONTROL_MODE_VELOCITY); + EXPECT_EQ(output.control_mode_expected, PandaControlMode::kVelocity); EXPECT_EQ(output.num_joint_position, 0); EXPECT_EQ(output.joint_position.size(), 0); EXPECT_EQ(output.num_joint_velocity, N); @@ -95,13 +91,12 @@ TEST_F(PandaCommandSenderTest, VelocityOnly) { } TEST_F(PandaCommandSenderTest, TorqueOnly) { - const PandaCommandSender sender(N, lcmt_panda_status::CONTROL_MODE_TORQUE); + const PandaCommandSender sender(N, PandaControlMode::kTorque); auto context = sender.CreateDefaultContext(); sender.get_torque_input_port().FixValue(context.get(), t0_); const lcmt_panda_command output = sender.get_output_port().Eval(*context); - EXPECT_EQ(output.control_mode_expected, - lcmt_panda_status::CONTROL_MODE_TORQUE); + EXPECT_EQ(output.control_mode_expected, PandaControlMode::kTorque); EXPECT_EQ(output.num_joint_position, 0); EXPECT_EQ(output.joint_position.size(), 0); EXPECT_EQ(output.num_joint_velocity, 0); From 1b839799ca7a3c16e5977e3a2fbb60241b437678 Mon Sep 17 00:00:00 2001 From: Peter Date: Fri, 10 Oct 2025 16:25:24 -0400 Subject: [PATCH 05/12] fixed build errors --- .../manipulation_py_franka_panda.cc | 23 ++++--- .../franka_panda/panda_command_receiver.cc | 67 +++++++++++-------- .../franka_panda/panda_command_receiver.h | 9 ++- .../franka_panda/panda_command_sender.cc | 31 +++++---- .../franka_panda/panda_command_sender.h | 9 ++- manipulation/franka_panda/panda_constants.h | 47 ++++++++++--- .../franka_panda/panda_status_receiver.cc | 4 +- .../franka_panda/panda_status_receiver.h | 3 +- .../franka_panda/panda_status_sender.cc | 2 +- .../franka_panda/panda_status_sender.h | 3 +- .../test/panda_command_receiver_test.cc | 9 +-- .../test/panda_command_sender_test.cc | 11 +-- 12 files changed, 132 insertions(+), 86 deletions(-) diff --git a/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc b/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc index c0db42b2035b..7c6a3d488f88 100644 --- a/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc +++ b/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc @@ -24,21 +24,24 @@ void DefineManipulationFrankaPanda(py::module m) { // Constants. m.attr("kPandaArmNumJoints") = kPandaArmNumJoints; - // Control mode constants - auto control_mode = m.def_submodule("PandaControlMode"); - control_mode.doc() = + // Control mode enum + py::enum_(m, "PandaControlMode", py::arithmetic(), "Control modes for the Panda robot. " - "These can be bitwise OR'd together."; - control_mode.attr("kPosition") = PandaControlMode::kPosition; - control_mode.attr("kVelocity") = PandaControlMode::kVelocity; - control_mode.attr("kTorque") = PandaControlMode::kTorque; - + "These can be bitwise OR'd together.") + .value("kNone", PandaControlMode::kNone) + .value("kPosition", PandaControlMode::kPosition) + .value("kVelocity", PandaControlMode::kVelocity) + .value("kTorque", PandaControlMode::kTorque) + .def("__or__", + [](PandaControlMode a, PandaControlMode b) { return a | b; }) + .def("__and__", + [](PandaControlMode a, PandaControlMode b) { return a & b; }); { using Class = PandaCommandReceiver; constexpr auto& cls_doc = doc.PandaCommandReceiver; py::class_>( m, "PandaCommandReceiver", cls_doc.doc) - .def(py::init(), py::arg("num_joints") = kPandaArmNumJoints, + .def(py::init(), py::arg("num_joints"), py::arg("control_mode"), cls_doc.ctor.doc) .def("LatchInitialPosition", overload_cast_explicit*>( @@ -68,7 +71,7 @@ void DefineManipulationFrankaPanda(py::module m) { using Class = PandaCommandSender; constexpr auto& cls_doc = doc.PandaCommandSender; py::class_>(m, "PandaCommandSender", cls_doc.doc) - .def(py::init(), py::arg("num_joints") = kPandaArmNumJoints, + .def(py::init(), py::arg("num_joints"), py::arg("control_mode"), cls_doc.ctor.doc) .def("get_position_input_port", &Class::get_position_input_port, py_rvp::reference_internal, cls_doc.get_position_input_port.doc) diff --git a/manipulation/franka_panda/panda_command_receiver.cc b/manipulation/franka_panda/panda_command_receiver.cc index c169ca84b057..313dd8b93209 100644 --- a/manipulation/franka_panda/panda_command_receiver.cc +++ b/manipulation/franka_panda/panda_command_receiver.cc @@ -1,13 +1,12 @@ -#include "manipulation/franka_panda/panda_command_receiver.h" +#include "drake/manipulation/franka_panda/panda_command_receiver.h" #include #include -#include "manipulation/franka_panda/panda_constants.h" - #include "drake/common/drake_throw.h" #include "drake/lcm/lcm_messages.h" #include "drake/lcmt_panda_status.hpp" +#include "drake/manipulation/franka_panda/panda_constants.h" namespace drake { namespace manipulation { @@ -25,7 +24,8 @@ using drake::systems::DiscreteValues; using drake::systems::kVectorValued; using Eigen::VectorXd; -PandaCommandReceiver::PandaCommandReceiver(int num_joints, int control_mode) +PandaCommandReceiver::PandaCommandReceiver(int num_joints, + PandaControlMode control_mode) : num_joints_(num_joints), control_mode_(control_mode) { DRAKE_THROW_UNLESS(num_joints > 0); @@ -37,7 +37,8 @@ PandaCommandReceiver::PandaCommandReceiver(int num_joints, int control_mode) // Even if we have not yet received a message, we still need to provide // default values for our output ports. We use a cache entry to compute // those values, by populating a default lcmt_panda_control message. - if (control_mode_ & PandaControlMode::kPosition) { + if ((control_mode_ & PandaControlMode::kPosition) != + PandaControlMode::kNone) { // When in position control mode, the default is derived from the // "position_measured" input. latched_position_measured_is_set_ = DeclareDiscreteState(VectorXd::Zero(1)); @@ -63,34 +64,36 @@ PandaCommandReceiver::PandaCommandReceiver(int num_joints, int control_mode) &PandaCommandReceiver::CalcMessageInputOrDefault, {message_input_->ticket(), default_command_->ticket()}); - int remaining = control_mode_; - if (control_mode_ & PandaControlMode::kPosition) { - remaining &= ~int{PandaControlMode::kPosition}; + PandaControlMode remaining = control_mode_; + if ((control_mode_ & PandaControlMode::kPosition) != + PandaControlMode::kNone) { + remaining &= ~PandaControlMode::kPosition; commanded_position_output_ = &DeclareVectorOutputPort("position", BasicVector(num_joints), &PandaCommandReceiver::CalcPositionOutput, {message_input_or_default_->ticket()}); } - if (control_mode_ & PandaControlMode::kVelocity) { - remaining &= ~int{PandaControlMode::kVelocity}; + if ((control_mode_ & PandaControlMode::kVelocity) != + PandaControlMode::kNone) { + remaining &= ~PandaControlMode::kVelocity; commanded_velocity_output_ = &DeclareVectorOutputPort("velocity", BasicVector(num_joints), &PandaCommandReceiver::CalcVelocityOutput, {message_input_or_default_->ticket()}); } - if (control_mode_ & PandaControlMode::kTorque) { - remaining &= ~int{PandaControlMode::kTorque}; + if ((control_mode_ & PandaControlMode::kTorque) != PandaControlMode::kNone) { + remaining &= ~PandaControlMode::kTorque; commanded_torque_output_ = &DeclareVectorOutputPort("torque", BasicVector(num_joints), &PandaCommandReceiver::CalcTorqueOutput, {message_input_or_default_->ticket()}); } - if (remaining != 0) { - throw std::logic_error( - fmt::format("Invalid control_mode bits set: 0x{:x}", remaining)); + if (remaining != PandaControlMode::kNone) { + throw std::logic_error(fmt::format("Invalid control_mode bits set: 0x{:x}", + to_int(remaining))); } } @@ -128,7 +131,8 @@ PandaCommandReceiver::get_commanded_torque_output_port() const { void PandaCommandReceiver::LatchInitialPosition( const Context& context, DiscreteValues* result) const { - DRAKE_DEMAND(control_mode_ & PandaControlMode::kPosition); + DRAKE_DEMAND((control_mode_ & PandaControlMode::kPosition) != + PandaControlMode::kNone); 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; @@ -139,7 +143,8 @@ void PandaCommandReceiver::LatchInitialPosition( void PandaCommandReceiver::LatchInitialPosition( Context* context) const { DRAKE_THROW_UNLESS(context != nullptr); - DRAKE_THROW_UNLESS(control_mode_ & PandaControlMode::kPosition); + DRAKE_THROW_UNLESS((control_mode_ & PandaControlMode::kPosition) != + PandaControlMode::kNone); LatchInitialPosition(*context, &context->get_mutable_discrete_state()); } @@ -155,7 +160,8 @@ void PandaCommandReceiver::DoCalcNextUpdateTime( DRAKE_THROW_UNLESS(std::isinf(*time)); // If we're not using position control, then we have no state to latch. - if (!(control_mode_ & PandaControlMode::kPosition)) { + if ((control_mode_ & PandaControlMode::kPosition) == + PandaControlMode::kNone) { return; } @@ -179,8 +185,9 @@ void PandaCommandReceiver::DoCalcNextUpdateTime( void PandaCommandReceiver::CalcDefaultCommand( const Context& context, lcmt_panda_command* result) const { *result = {}; - result->control_mode_expected = control_mode_; - if (control_mode_ & PandaControlMode::kPosition) { + result->control_mode_expected = to_int(control_mode_); + if ((control_mode_ & PandaControlMode::kPosition) != + PandaControlMode::kNone) { const BasicVector& latch_is_set = context.get_discrete_state(latched_position_measured_is_set_); const BasicVector& default_position = @@ -192,11 +199,12 @@ void PandaCommandReceiver::CalcDefaultCommand( result->num_joint_position = num_joints_; result->joint_position = {vec.data(), vec.data() + num_joints_}; } - if (control_mode_ & PandaControlMode::kVelocity) { + if ((control_mode_ & PandaControlMode::kVelocity) != + PandaControlMode::kNone) { result->num_joint_velocity = num_joints_; result->joint_velocity = std::vector(num_joints_, 0.0); } - if (control_mode_ & PandaControlMode::kTorque) { + if ((control_mode_ & PandaControlMode::kTorque) != PandaControlMode::kNone) { result->num_joint_torque = num_joints_; result->joint_torque = std::vector(num_joints_, 0.0); } @@ -229,10 +237,11 @@ void CheckNumJoints(const std::string& system_name, const char* vector_name, void PandaCommandReceiver::CalcPositionOutput( const Context& context, BasicVector* output) const { - DRAKE_DEMAND(control_mode_ & PandaControlMode::kPosition); + DRAKE_DEMAND((control_mode_ & PandaControlMode::kPosition) != + PandaControlMode::kNone); const auto& message = message_input_or_default_->Eval(context); - DRAKE_DEMAND(message.control_mode_expected == control_mode_); + DRAKE_DEMAND(message.control_mode_expected == to_int(control_mode_)); CheckNumJoints(get_name(), "position", message.num_joint_position, num_joints_); CheckNumJoints(get_name(), "position", message.joint_position.size(), @@ -243,10 +252,11 @@ void PandaCommandReceiver::CalcPositionOutput( void PandaCommandReceiver::CalcVelocityOutput( const Context& context, BasicVector* output) const { - DRAKE_DEMAND(control_mode_ & PandaControlMode::kVelocity); + DRAKE_DEMAND((control_mode_ & PandaControlMode::kVelocity) != + PandaControlMode::kNone); const auto& message = message_input_or_default_->Eval(context); - DRAKE_DEMAND(message.control_mode_expected == control_mode_); + DRAKE_DEMAND(message.control_mode_expected == to_int(control_mode_)); CheckNumJoints(get_name(), "velocity", message.num_joint_velocity, num_joints_); CheckNumJoints(get_name(), "velocity", message.joint_velocity.size(), @@ -257,10 +267,11 @@ void PandaCommandReceiver::CalcVelocityOutput( void PandaCommandReceiver::CalcTorqueOutput(const Context& context, BasicVector* output) const { - DRAKE_DEMAND(control_mode_ & PandaControlMode::kTorque); + DRAKE_DEMAND((control_mode_ & PandaControlMode::kTorque) != + PandaControlMode::kNone); const auto& message = message_input_or_default_->Eval(context); - DRAKE_DEMAND(message.control_mode_expected == control_mode_); + DRAKE_DEMAND(message.control_mode_expected == to_int(control_mode_)); CheckNumJoints(get_name(), "torque", message.num_joint_torque, num_joints_); CheckNumJoints(get_name(), "torque", message.joint_torque.size(), num_joints_); diff --git a/manipulation/franka_panda/panda_command_receiver.h b/manipulation/franka_panda/panda_command_receiver.h index 68abde718a6b..c499346c380f 100644 --- a/manipulation/franka_panda/panda_command_receiver.h +++ b/manipulation/franka_panda/panda_command_receiver.h @@ -1,10 +1,9 @@ #pragma once -#include "manipulation/franka_panda/panda_constants.h" - #include "drake/common/drake_copyable.h" #include "drake/common/drake_throw.h" #include "drake/lcmt_panda_command.hpp" +#include "drake/manipulation/franka_panda/panda_constants.h" #include "drake/systems/framework/leaf_system.h" namespace drake { @@ -57,8 +56,8 @@ class PandaCommandReceiver final : public drake::systems::LeafSystem { DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(PandaCommandReceiver); /// @param control_mode is a bitset of one or more control mode constants - /// defined in lcmt_panda_status::CONTROL_MODE_{POSITION,VELOCITY,TORQUE}. - PandaCommandReceiver(int num_joints, int control_mode); + /// defined in PandaControlMode enum. Use bitwise OR to combine modes. + PandaCommandReceiver(int num_joints, PandaControlMode control_mode); ~PandaCommandReceiver() final; /// (Advanced.) Copies the current "position_measured" input @@ -106,7 +105,7 @@ class PandaCommandReceiver final : public drake::systems::LeafSystem { drake::systems::BasicVector*) const; const int num_joints_; - const int control_mode_; + const PandaControlMode control_mode_; const drake::systems::InputPort* message_input_{}; const drake::systems::InputPort* position_measured_input_{}; drake::systems::DiscreteStateIndex latched_position_measured_is_set_; diff --git a/manipulation/franka_panda/panda_command_sender.cc b/manipulation/franka_panda/panda_command_sender.cc index 1d008a7a5798..23392d3aad54 100644 --- a/manipulation/franka_panda/panda_command_sender.cc +++ b/manipulation/franka_panda/panda_command_sender.cc @@ -1,11 +1,11 @@ -#include "manipulation/franka_panda/panda_command_sender.h" +#include "drake/manipulation/franka_panda/panda_command_sender.h" #include -#include "manipulation/franka_panda/panda_constants.h" #include #include "drake/lcmt_panda_status.hpp" +#include "drake/manipulation/franka_panda/panda_constants.h" namespace drake { namespace manipulation { @@ -16,27 +16,30 @@ using drake::lcmt_panda_status; using drake::systems::Context; using drake::systems::kVectorValued; -PandaCommandSender::PandaCommandSender(int num_joints, int control_mode) +PandaCommandSender::PandaCommandSender(int num_joints, + PandaControlMode control_mode) : num_joints_(num_joints), control_mode_(control_mode) { - int remaining = control_mode_; - if (control_mode_ & PandaControlMode::kPosition) { - remaining &= ~int{PandaControlMode::kPosition}; + PandaControlMode remaining = control_mode_; + if ((control_mode_ & PandaControlMode::kPosition) != + PandaControlMode::kNone) { + remaining &= ~PandaControlMode::kPosition; position_input_port_ = &this->DeclareInputPort("position", kVectorValued, num_joints_); } - if (control_mode_ & PandaControlMode::kVelocity) { - remaining &= ~int{PandaControlMode::kVelocity}; + if ((control_mode_ & PandaControlMode::kVelocity) != + PandaControlMode::kNone) { + remaining &= ~PandaControlMode::kVelocity; velocity_input_port_ = &this->DeclareInputPort("velocity", kVectorValued, num_joints_); } - if (control_mode_ & PandaControlMode::kTorque) { - remaining &= ~int{PandaControlMode::kTorque}; + if ((control_mode_ & PandaControlMode::kTorque) != PandaControlMode::kNone) { + remaining &= ~PandaControlMode::kTorque; torque_input_port_ = &this->DeclareInputPort("torque", kVectorValued, num_joints_); } - if (remaining != 0) { - throw std::logic_error( - fmt::format("Invalid control_mode bits set: 0x{:x}", remaining)); + if (remaining != PandaControlMode::kNone) { + throw std::logic_error(fmt::format("Invalid control_mode bits set: 0x{:x}", + to_int(remaining))); } this->DeclareAbstractOutputPort("lcmt_panda_command", &PandaCommandSender::CalcOutput); @@ -98,7 +101,7 @@ void PandaCommandSender::CalcOutput(const Context& context, lcmt_panda_command* output) const { lcmt_panda_command& command = *output; command.utime = context.get_time() * 1e6; - command.control_mode_expected = control_mode_; + command.control_mode_expected = to_int(control_mode_); CopyInputPortToMessage(context, position_input_port_, &command.num_joint_position, &command.joint_position); CopyInputPortToMessage(context, velocity_input_port_, diff --git a/manipulation/franka_panda/panda_command_sender.h b/manipulation/franka_panda/panda_command_sender.h index c945196b3eee..c4b5cbe1c5e8 100644 --- a/manipulation/franka_panda/panda_command_sender.h +++ b/manipulation/franka_panda/panda_command_sender.h @@ -1,9 +1,8 @@ #pragma once -#include "manipulation/franka_panda/panda_constants.h" - #include "drake/common/drake_copyable.h" #include "drake/lcmt_panda_command.hpp" +#include "drake/manipulation/franka_panda/panda_constants.h" #include "drake/systems/framework/leaf_system.h" namespace drake { @@ -39,8 +38,8 @@ class PandaCommandSender final : public drake::systems::LeafSystem { DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(PandaCommandSender); /// @param control_mode is a bitset of one or more control mode constants - /// defined in lcmt_panda_status::CONTROL_MODE_{POSITION,VELOCITY,TORQUE}. - PandaCommandSender(int num_joints, int control_mode); + /// defined in PandaControlMode enum. Use bitwise OR to combine modes. + PandaCommandSender(int num_joints, PandaControlMode control_mode); ~PandaCommandSender() final; /// @name Named accessors for this System's input and output ports. @@ -55,7 +54,7 @@ class PandaCommandSender final : public drake::systems::LeafSystem { drake::lcmt_panda_command*) const; const int num_joints_; - const int control_mode_; + const PandaControlMode control_mode_; const drake::systems::InputPort* position_input_port_{}; const drake::systems::InputPort* velocity_input_port_{}; diff --git a/manipulation/franka_panda/panda_constants.h b/manipulation/franka_panda/panda_constants.h index 5f80279b6f4b..6b0faee0c89c 100644 --- a/manipulation/franka_panda/panda_constants.h +++ b/manipulation/franka_panda/panda_constants.h @@ -11,20 +11,51 @@ const int kPandaArmNumJoints = 7; /** Control modes for the Panda robot. These can be bitwise OR'd together * to enable multiple control modes simultaneously. * Values match lcmt_panda_status::CONTROL_MODE_* constants. */ -namespace PandaControlMode { -constexpr int kPosition = 1; -constexpr int kVelocity = 2; -constexpr int kTorque = 4; -} // namespace PandaControlMode +enum class PandaControlMode : int { + kNone = 0, + kPosition = 1, + kVelocity = 2, + kTorque = 4, +}; + +// Enable bitwise operations for PandaControlMode +constexpr PandaControlMode operator|(PandaControlMode a, PandaControlMode b) { + return static_cast(static_cast(a) | + static_cast(b)); +} + +constexpr PandaControlMode operator&(PandaControlMode a, PandaControlMode b) { + return static_cast(static_cast(a) & + static_cast(b)); +} + +constexpr PandaControlMode operator~(PandaControlMode a) { + return static_cast(~static_cast(a)); +} + +constexpr PandaControlMode& operator|=(PandaControlMode& a, + PandaControlMode b) { + return a = a | b; +} + +constexpr PandaControlMode& operator&=(PandaControlMode& a, + PandaControlMode b) { + return a = a & b; +} + +// Helper to convert to int for use in comparisons and LCM messages +constexpr int to_int(PandaControlMode mode) { + return static_cast(mode); +} // Ensure our constants match the LCM message definition -static_assert(PandaControlMode::kPosition == +static_assert(to_int(PandaControlMode::kPosition) == drake::lcmt_panda_status::CONTROL_MODE_POSITION, "PandaControlMode::kPosition must match LCM definition"); -static_assert(PandaControlMode::kVelocity == +static_assert(to_int(PandaControlMode::kVelocity) == drake::lcmt_panda_status::CONTROL_MODE_VELOCITY, "PandaControlMode::kVelocity must match LCM definition"); -static_assert(PandaControlMode::kTorque == +static_assert(to_int(PandaControlMode::kTorque) == drake::lcmt_panda_status::CONTROL_MODE_TORQUE, "PandaControlMode::kTorque must match LCM definition"); diff --git a/manipulation/franka_panda/panda_status_receiver.cc b/manipulation/franka_panda/panda_status_receiver.cc index 4c3d09a5c4c4..c7642fbc3d6b 100644 --- a/manipulation/franka_panda/panda_status_receiver.cc +++ b/manipulation/franka_panda/panda_status_receiver.cc @@ -1,4 +1,4 @@ -#include "manipulation/franka_panda/panda_status_receiver.h" +#include "drake/manipulation/franka_panda/panda_status_receiver.h" #include "drake/common/drake_throw.h" @@ -75,7 +75,7 @@ const OutPort& PandaStatusReceiver::get_torque_external_output_port() const { return LeafSystem::get_output_port(7); } -template drake::lcmt_panda_status::* field_ptr> +template drake::lcmt_panda_status::*field_ptr> void PandaStatusReceiver::CalcLcmOutput(const Context& context, BasicVector* output) const { const auto& status = get_input_port().Eval(context); diff --git a/manipulation/franka_panda/panda_status_receiver.h b/manipulation/franka_panda/panda_status_receiver.h index 77e2d249e7ae..dc1431b9bf95 100644 --- a/manipulation/franka_panda/panda_status_receiver.h +++ b/manipulation/franka_panda/panda_status_receiver.h @@ -2,10 +2,9 @@ #include -#include "manipulation/franka_panda/panda_constants.h" - #include "drake/common/drake_copyable.h" #include "drake/lcmt_panda_status.hpp" +#include "drake/manipulation/franka_panda/panda_constants.h" #include "drake/systems/framework/leaf_system.h" namespace drake { diff --git a/manipulation/franka_panda/panda_status_sender.cc b/manipulation/franka_panda/panda_status_sender.cc index 37b68fdfc590..457fb7db51e2 100644 --- a/manipulation/franka_panda/panda_status_sender.cc +++ b/manipulation/franka_panda/panda_status_sender.cc @@ -1,4 +1,4 @@ -#include "manipulation/franka_panda/panda_status_sender.h" +#include "drake/manipulation/franka_panda/panda_status_sender.h" namespace drake { namespace manipulation { diff --git a/manipulation/franka_panda/panda_status_sender.h b/manipulation/franka_panda/panda_status_sender.h index 19fee28d8d22..ca79a84f8834 100644 --- a/manipulation/franka_panda/panda_status_sender.h +++ b/manipulation/franka_panda/panda_status_sender.h @@ -1,9 +1,8 @@ #pragma once -#include "manipulation/franka_panda/panda_constants.h" - #include "drake/common/drake_copyable.h" #include "drake/lcmt_panda_status.hpp" +#include "drake/manipulation/franka_panda/panda_constants.h" #include "drake/systems/framework/leaf_system.h" namespace drake { diff --git a/manipulation/franka_panda/test/panda_command_receiver_test.cc b/manipulation/franka_panda/test/panda_command_receiver_test.cc index b568f65b11b7..967bda07d731 100644 --- a/manipulation/franka_panda/test/panda_command_receiver_test.cc +++ b/manipulation/franka_panda/test/panda_command_receiver_test.cc @@ -81,7 +81,7 @@ TEST_F(PandaCommandReceiverTest, AcceptanceTestWithMeasuredPositionInput) { const VectorXd t1 = VectorXd::LinSpaced(N, 0.5, 0.6); lcmt_panda_command command{}; command.control_mode_expected = - PandaControlMode::kPosition | PandaControlMode::kTorque; + to_int(PandaControlMode::kPosition | PandaControlMode::kTorque); command.utime = 0; command.num_joint_position = N; command.joint_position = {q1.data(), q1.data() + q1.size()}; @@ -127,7 +127,7 @@ TEST_F(PandaCommandReceiverTest, AcceptanceTestWithLatching) { const VectorXd t3 = VectorXd::LinSpaced(N, 0.6, 0.7); lcmt_panda_command command{}; command.control_mode_expected = - PandaControlMode::kPosition | PandaControlMode::kTorque; + to_int(PandaControlMode::kPosition | PandaControlMode::kTorque); command.utime = 0; command.num_joint_position = N; command.joint_position = {q3.data(), q3.data() + q3.size()}; @@ -148,7 +148,7 @@ GTEST_TEST(PandaCommandReceiverTestVelocity, VelocityControl) { EXPECT_THROW(receiver.get_commanded_torque_output_port(), std::runtime_error); lcmt_panda_command command{}; - command.control_mode_expected = PandaControlMode::kVelocity; + command.control_mode_expected = to_int(PandaControlMode::kVelocity); command.num_joint_velocity = kPandaArmNumJoints; command.joint_velocity.resize(kPandaArmNumJoints, 0.1); @@ -160,7 +160,8 @@ GTEST_TEST(PandaCommandReceiverTestVelocity, VelocityControl) { } TEST_F(PandaCommandReceiverTest, BadControlMode) { - EXPECT_THROW(PandaCommandReceiver(7, 0x9999), std::exception); + EXPECT_THROW(PandaCommandReceiver(7, static_cast(0x9999)), + std::exception); } } // namespace franka_panda diff --git a/manipulation/franka_panda/test/panda_command_sender_test.cc b/manipulation/franka_panda/test/panda_command_sender_test.cc index f4e01573ede3..1ac9ac38c293 100644 --- a/manipulation/franka_panda/test/panda_command_sender_test.cc +++ b/manipulation/franka_panda/test/panda_command_sender_test.cc @@ -51,7 +51,7 @@ TEST_F(PandaCommandSenderTest, PositionAndTorque) { dut_.get_position_input_port().FixValue(&context_, q0_); dut_.get_torque_input_port().FixValue(&context_, t0_); EXPECT_EQ(output().control_mode_expected, - PandaControlMode::kPosition | PandaControlMode::kTorque); + to_int(PandaControlMode::kPosition | PandaControlMode::kTorque)); EXPECT_EQ(output().num_joint_position, N); EXPECT_EQ(output().joint_position, std_q0_); EXPECT_EQ(output().num_joint_velocity, 0); @@ -66,7 +66,7 @@ TEST_F(PandaCommandSenderTest, PositionOnly) { sender.get_position_input_port().FixValue(context.get(), q0_); const lcmt_panda_command output = sender.get_output_port().Eval(*context); - EXPECT_EQ(output.control_mode_expected, PandaControlMode::kPosition); + EXPECT_EQ(output.control_mode_expected, to_int(PandaControlMode::kPosition)); EXPECT_EQ(output.num_joint_position, N); EXPECT_EQ(output.joint_position, std_q0_); EXPECT_EQ(output.num_joint_velocity, 0); @@ -81,7 +81,7 @@ TEST_F(PandaCommandSenderTest, VelocityOnly) { sender.get_velocity_input_port().FixValue(context.get(), v0_); const lcmt_panda_command output = sender.get_output_port().Eval(*context); - EXPECT_EQ(output.control_mode_expected, PandaControlMode::kVelocity); + EXPECT_EQ(output.control_mode_expected, to_int(PandaControlMode::kVelocity)); EXPECT_EQ(output.num_joint_position, 0); EXPECT_EQ(output.joint_position.size(), 0); EXPECT_EQ(output.num_joint_velocity, N); @@ -96,7 +96,7 @@ TEST_F(PandaCommandSenderTest, TorqueOnly) { sender.get_torque_input_port().FixValue(context.get(), t0_); const lcmt_panda_command output = sender.get_output_port().Eval(*context); - EXPECT_EQ(output.control_mode_expected, PandaControlMode::kTorque); + EXPECT_EQ(output.control_mode_expected, to_int(PandaControlMode::kTorque)); EXPECT_EQ(output.num_joint_position, 0); EXPECT_EQ(output.joint_position.size(), 0); EXPECT_EQ(output.num_joint_velocity, 0); @@ -129,7 +129,8 @@ TEST_F(PandaCommandSenderTest, MallocTest) { } TEST_F(PandaCommandSenderTest, BadControlMode) { - EXPECT_THROW(PandaCommandSender(7, 0x9999), std::exception); + EXPECT_THROW(PandaCommandSender(7, static_cast(0x9999)), + std::exception); } } // namespace franka_panda From 2edb5b1077069f0a6e6ed7a9f5b78869ae1987e2 Mon Sep 17 00:00:00 2001 From: Peter Date: Fri, 10 Oct 2025 17:32:44 -0400 Subject: [PATCH 06/12] fixes nit --- manipulation/franka_panda/panda_status_receiver.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/manipulation/franka_panda/panda_status_receiver.cc b/manipulation/franka_panda/panda_status_receiver.cc index c7642fbc3d6b..b6f88ada7c1b 100644 --- a/manipulation/franka_panda/panda_status_receiver.cc +++ b/manipulation/franka_panda/panda_status_receiver.cc @@ -75,7 +75,7 @@ const OutPort& PandaStatusReceiver::get_torque_external_output_port() const { return LeafSystem::get_output_port(7); } -template drake::lcmt_panda_status::*field_ptr> +template drake::lcmt_panda_status::* field_ptr> void PandaStatusReceiver::CalcLcmOutput(const Context& context, BasicVector* output) const { const auto& status = get_input_port().Eval(context); From b32a24ce2c4835c9f0e8fd9ee50ea3a9601f3176 Mon Sep 17 00:00:00 2001 From: Peter Date: Wed, 5 Nov 2025 15:59:35 -0500 Subject: [PATCH 07/12] fixing lint test --- .../manipulation/test/franka_panda_test.py | 119 +++++++++++------- 1 file changed, 71 insertions(+), 48 deletions(-) diff --git a/bindings/pydrake/manipulation/test/franka_panda_test.py b/bindings/pydrake/manipulation/test/franka_panda_test.py index 89cc741a485b..9e4c5a320950 100644 --- a/bindings/pydrake/manipulation/test/franka_panda_test.py +++ b/bindings/pydrake/manipulation/test/franka_panda_test.py @@ -1,9 +1,8 @@ # -*- coding: utf-8 -*- -import pydrake.manipulation as mut - import unittest +import pydrake.manipulation as mut from pydrake.systems.framework import ( InputPort, OutputPort, @@ -16,70 +15,86 @@ def test_constants(self): def test_franka_panda_command_receiver(self): # Test with position and torque control mode - control_mode = (mut.PandaControlMode.kPosition | - mut.PandaControlMode.kTorque) + control_mode = ( + mut.PandaControlMode.kPosition | mut.PandaControlMode.kTorque + ) command_rec = mut.PandaCommandReceiver( - num_joints=mut.kPandaArmNumJoints, - control_mode=control_mode) + num_joints=mut.kPandaArmNumJoints, control_mode=control_mode + ) + self.assertIsInstance(command_rec.get_message_input_port(), InputPort) self.assertIsInstance( - command_rec.get_message_input_port(), InputPort) + command_rec.get_position_measured_input_port(), InputPort + ) self.assertIsInstance( - command_rec.get_position_measured_input_port(), InputPort) + command_rec.get_commanded_position_output_port(), OutputPort + ) self.assertIsInstance( - command_rec.get_commanded_position_output_port(), OutputPort) - self.assertIsInstance( - command_rec.get_commanded_torque_output_port(), OutputPort) + command_rec.get_commanded_torque_output_port(), OutputPort + ) # Test with position, velocity, and torque control mode - control_mode_all = (mut.PandaControlMode.kPosition | - mut.PandaControlMode.kVelocity | - mut.PandaControlMode.kTorque) + control_mode_all = ( + mut.PandaControlMode.kPosition + | mut.PandaControlMode.kVelocity + | mut.PandaControlMode.kTorque + ) command_rec_all = mut.PandaCommandReceiver( - num_joints=mut.kPandaArmNumJoints, - control_mode=control_mode_all) + num_joints=mut.kPandaArmNumJoints, control_mode=control_mode_all + ) self.assertIsInstance( - command_rec_all.get_commanded_velocity_output_port(), OutputPort) + command_rec_all.get_commanded_velocity_output_port(), OutputPort + ) def test_franka_panda_command_sender(self): # Test with position and torque control mode - control_mode = (mut.PandaControlMode.kPosition | - mut.PandaControlMode.kTorque) + control_mode = ( + mut.PandaControlMode.kPosition | mut.PandaControlMode.kTorque + ) command_send = mut.PandaCommandSender( - num_joints=mut.kPandaArmNumJoints, - control_mode=control_mode) - self.assertIsInstance( - command_send.get_position_input_port(), InputPort) - self.assertIsInstance( - command_send.get_torque_input_port(), InputPort) + num_joints=mut.kPandaArmNumJoints, control_mode=control_mode + ) + self.assertIsInstance(command_send.get_position_input_port(), InputPort) + self.assertIsInstance(command_send.get_torque_input_port(), InputPort) # Test with position, velocity, and torque control mode - control_mode_all = (mut.PandaControlMode.kPosition | - mut.PandaControlMode.kVelocity | - mut.PandaControlMode.kTorque) + control_mode_all = ( + mut.PandaControlMode.kPosition + | mut.PandaControlMode.kVelocity + | mut.PandaControlMode.kTorque + ) command_send_all = mut.PandaCommandSender( - num_joints=mut.kPandaArmNumJoints, - control_mode=control_mode_all) + num_joints=mut.kPandaArmNumJoints, control_mode=control_mode_all + ) self.assertIsInstance( - command_send_all.get_velocity_input_port(), InputPort) + command_send_all.get_velocity_input_port(), InputPort + ) def test_franka_panda_status_receiver(self): status_rec = mut.PandaStatusReceiver() self.assertIsInstance( - status_rec.get_position_commanded_output_port(), OutputPort) + status_rec.get_position_commanded_output_port(), OutputPort + ) self.assertIsInstance( - status_rec.get_position_measured_output_port(), OutputPort) + status_rec.get_position_measured_output_port(), OutputPort + ) self.assertIsInstance( - status_rec.get_velocity_commanded_output_port(), OutputPort) + status_rec.get_velocity_commanded_output_port(), OutputPort + ) self.assertIsInstance( - status_rec.get_velocity_measured_output_port(), OutputPort) + status_rec.get_velocity_measured_output_port(), OutputPort + ) self.assertIsInstance( - status_rec.get_acceleration_commanded_output_port(), OutputPort) + status_rec.get_acceleration_commanded_output_port(), OutputPort + ) self.assertIsInstance( - status_rec.get_torque_commanded_output_port(), OutputPort) + status_rec.get_torque_commanded_output_port(), OutputPort + ) self.assertIsInstance( - status_rec.get_torque_measured_output_port(), OutputPort) + status_rec.get_torque_measured_output_port(), OutputPort + ) self.assertIsInstance( - status_rec.get_torque_external_output_port(), OutputPort) + status_rec.get_torque_external_output_port(), OutputPort + ) # Constructor variant. mut.PandaStatusReceiver(num_joints=mut.kPandaArmNumJoints) @@ -87,21 +102,29 @@ def test_franka_panda_status_receiver(self): def test_franka_panda_status_sender(self): status_send = mut.PandaStatusSender() self.assertIsInstance( - status_send.get_position_commanded_input_port(), InputPort) + status_send.get_position_commanded_input_port(), InputPort + ) self.assertIsInstance( - status_send.get_position_measured_input_port(), InputPort) + status_send.get_position_measured_input_port(), InputPort + ) self.assertIsInstance( - status_send.get_velocity_commanded_input_port(), InputPort) + status_send.get_velocity_commanded_input_port(), InputPort + ) self.assertIsInstance( - status_send.get_velocity_measured_input_port(), InputPort) + status_send.get_velocity_measured_input_port(), InputPort + ) self.assertIsInstance( - status_send.get_acceleration_commanded_input_port(), InputPort) + status_send.get_acceleration_commanded_input_port(), InputPort + ) self.assertIsInstance( - status_send.get_torque_commanded_input_port(), InputPort) + status_send.get_torque_commanded_input_port(), InputPort + ) self.assertIsInstance( - status_send.get_torque_measured_input_port(), InputPort) + status_send.get_torque_measured_input_port(), InputPort + ) self.assertIsInstance( - status_send.get_torque_external_input_port(), InputPort) + status_send.get_torque_external_input_port(), InputPort + ) # Constructor variant. - mut.PandaStatusSender(num_joints=mut.kPandaArmNumJoints) \ No newline at end of file + mut.PandaStatusSender(num_joints=mut.kPandaArmNumJoints) From 0347be77792fce2566f416059664ef1105b125b2 Mon Sep 17 00:00:00 2001 From: Peter Date: Thu, 6 Nov 2025 11:56:25 -0500 Subject: [PATCH 08/12] fix typos --- manipulation/franka_panda/panda_command_receiver.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/manipulation/franka_panda/panda_command_receiver.cc b/manipulation/franka_panda/panda_command_receiver.cc index 313dd8b93209..cd3c8b2ededa 100644 --- a/manipulation/franka_panda/panda_command_receiver.cc +++ b/manipulation/franka_panda/panda_command_receiver.cc @@ -103,7 +103,7 @@ const drake::systems::OutputPort& PandaCommandReceiver::get_commanded_position_output_port() const { if (commanded_position_output_ == nullptr) { throw std::runtime_error( - "Invalid call to PandaCommandReciver::get_command_position_output_port" + "Invalid call to PandaCommandReceiver::get_command_position_output_port" " when control_mode does not include position"); } return *commanded_position_output_; @@ -113,7 +113,7 @@ const drake::systems::OutputPort& PandaCommandReceiver::get_commanded_velocity_output_port() const { if (commanded_velocity_output_ == nullptr) { throw std::runtime_error( - "Invalid call to PandaCommandReciver::get_command_velocity_output_port" + "Invalid call to PandaCommandReceiver::get_command_velocity_output_port" " when control_mode does not include velocity"); } return *commanded_velocity_output_; @@ -123,7 +123,7 @@ const drake::systems::OutputPort& PandaCommandReceiver::get_commanded_torque_output_port() const { if (commanded_torque_output_ == nullptr) { throw std::runtime_error( - "Invalid call to PandaCommandReciver::get_command_torque_output_port" + "Invalid call to PandaCommandReceiver::get_command_torque_output_port" " when control_mode does not include torque"); } return *commanded_torque_output_; From 4d7f1220e6426fdce8e91f04b0c71630624437c5 Mon Sep 17 00:00:00 2001 From: Peter Date: Tue, 11 Nov 2025 15:10:05 -0500 Subject: [PATCH 09/12] refactor control mode enum --- .../manipulation_py_franka_panda.cc | 24 ++++---- .../manipulation/test/franka_panda_test.py | 25 ++++++++ .../franka_panda/panda_command_receiver.cc | 58 ++++++++++--------- .../franka_panda/panda_command_receiver.h | 2 +- .../franka_panda/panda_command_sender.cc | 19 +++--- .../franka_panda/panda_command_sender.h | 2 +- manipulation/franka_panda/panda_constants.h | 54 +++++------------ .../test/panda_command_receiver_test.cc | 10 ++-- .../test/panda_command_sender_test.cc | 16 ++--- 9 files changed, 108 insertions(+), 102 deletions(-) diff --git a/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc b/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc index 7c6a3d488f88..f1cf962ad78a 100644 --- a/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc +++ b/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc @@ -24,18 +24,18 @@ void DefineManipulationFrankaPanda(py::module m) { // Constants. m.attr("kPandaArmNumJoints") = kPandaArmNumJoints; - // Control mode enum - py::enum_(m, "PandaControlMode", py::arithmetic(), - "Control modes for the Panda robot. " - "These can be bitwise OR'd together.") - .value("kNone", PandaControlMode::kNone) - .value("kPosition", PandaControlMode::kPosition) - .value("kVelocity", PandaControlMode::kVelocity) - .value("kTorque", PandaControlMode::kTorque) - .def("__or__", - [](PandaControlMode a, PandaControlMode b) { return a | b; }) - .def("__and__", - [](PandaControlMode a, PandaControlMode b) { return a & b; }); + // Control mode constants. PandaControlMode is a uint64_t typedef, so we + // expose the constants as integers. Python's native bitwise operators (| and &) + // work with these integers, and pybind11 automatically converts them to uint64_t + // when calling C++ functions. + py::object control_mode_class = py::module_::import("types").attr("SimpleNamespace")(); + control_mode_class.attr("__doc__") = + "Control modes for the Panda robot. These can be bitwise OR'd together."; + control_mode_class.attr("kNone") = py::int_(PandaControlModes::kNone); + control_mode_class.attr("kPosition") = py::int_(PandaControlModes::kPosition); + control_mode_class.attr("kVelocity") = py::int_(PandaControlModes::kVelocity); + control_mode_class.attr("kTorque") = py::int_(PandaControlModes::kTorque); + m.attr("PandaControlMode") = control_mode_class; { using Class = PandaCommandReceiver; constexpr auto& cls_doc = doc.PandaCommandReceiver; diff --git a/bindings/pydrake/manipulation/test/franka_panda_test.py b/bindings/pydrake/manipulation/test/franka_panda_test.py index 9e4c5a320950..0c0834a0653b 100644 --- a/bindings/pydrake/manipulation/test/franka_panda_test.py +++ b/bindings/pydrake/manipulation/test/franka_panda_test.py @@ -13,6 +13,31 @@ class TestFrankaPanda(unittest.TestCase): def test_constants(self): self.assertEqual(mut.kPandaArmNumJoints, 7) + def test_control_mode_bitwise_or(self): + # Test that OR-ing control modes produces correct bit patterns + # kPosition = 1, kVelocity = 2, kTorque = 4 + self.assertEqual(mut.PandaControlMode.kPosition, 1) + self.assertEqual(mut.PandaControlMode.kVelocity, 2) + self.assertEqual(mut.PandaControlMode.kTorque, 4) + self.assertEqual(mut.PandaControlMode.kNone, 0) + + # Test OR combinations + self.assertEqual( + mut.PandaControlMode.kPosition | mut.PandaControlMode.kVelocity, 3 + ) + self.assertEqual( + mut.PandaControlMode.kPosition | mut.PandaControlMode.kTorque, 5 + ) + self.assertEqual( + mut.PandaControlMode.kVelocity | mut.PandaControlMode.kTorque, 6 + ) + self.assertEqual( + mut.PandaControlMode.kPosition + | mut.PandaControlMode.kVelocity + | mut.PandaControlMode.kTorque, + 7 + ) + def test_franka_panda_command_receiver(self): # Test with position and torque control mode control_mode = ( diff --git a/manipulation/franka_panda/panda_command_receiver.cc b/manipulation/franka_panda/panda_command_receiver.cc index cd3c8b2ededa..eef2d41a4852 100644 --- a/manipulation/franka_panda/panda_command_receiver.cc +++ b/manipulation/franka_panda/panda_command_receiver.cc @@ -37,8 +37,8 @@ PandaCommandReceiver::PandaCommandReceiver(int num_joints, // Even if we have not yet received a message, we still need to provide // default values for our output ports. We use a cache entry to compute // those values, by populating a default lcmt_panda_control message. - if ((control_mode_ & PandaControlMode::kPosition) != - PandaControlMode::kNone) { + if ((control_mode_ & PandaControlModes::kPosition) != + PandaControlModes::kNone) { // When in position control mode, the default is derived from the // "position_measured" input. latched_position_measured_is_set_ = DeclareDiscreteState(VectorXd::Zero(1)); @@ -65,33 +65,34 @@ PandaCommandReceiver::PandaCommandReceiver(int num_joints, {message_input_->ticket(), default_command_->ticket()}); PandaControlMode remaining = control_mode_; - if ((control_mode_ & PandaControlMode::kPosition) != - PandaControlMode::kNone) { - remaining &= ~PandaControlMode::kPosition; + if ((control_mode_ & PandaControlModes::kPosition) != + PandaControlModes::kNone) { + remaining &= ~PandaControlModes::kPosition; commanded_position_output_ = &DeclareVectorOutputPort("position", BasicVector(num_joints), &PandaCommandReceiver::CalcPositionOutput, {message_input_or_default_->ticket()}); } - if ((control_mode_ & PandaControlMode::kVelocity) != - PandaControlMode::kNone) { - remaining &= ~PandaControlMode::kVelocity; + if ((control_mode_ & PandaControlModes::kVelocity) != + PandaControlModes::kNone) { + remaining &= ~PandaControlModes::kVelocity; commanded_velocity_output_ = &DeclareVectorOutputPort("velocity", BasicVector(num_joints), &PandaCommandReceiver::CalcVelocityOutput, {message_input_or_default_->ticket()}); } - if ((control_mode_ & PandaControlMode::kTorque) != PandaControlMode::kNone) { - remaining &= ~PandaControlMode::kTorque; + if ((control_mode_ & PandaControlModes::kTorque) != + PandaControlModes::kNone) { + remaining &= ~PandaControlModes::kTorque; commanded_torque_output_ = &DeclareVectorOutputPort("torque", BasicVector(num_joints), &PandaCommandReceiver::CalcTorqueOutput, {message_input_or_default_->ticket()}); } - if (remaining != PandaControlMode::kNone) { + if (remaining != PandaControlModes::kNone) { throw std::logic_error(fmt::format("Invalid control_mode bits set: 0x{:x}", to_int(remaining))); } @@ -131,8 +132,8 @@ PandaCommandReceiver::get_commanded_torque_output_port() const { void PandaCommandReceiver::LatchInitialPosition( const Context& context, DiscreteValues* result) const { - DRAKE_DEMAND((control_mode_ & PandaControlMode::kPosition) != - PandaControlMode::kNone); + DRAKE_DEMAND((control_mode_ & PandaControlModes::kPosition) != + PandaControlModes::kNone); 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; @@ -143,8 +144,8 @@ void PandaCommandReceiver::LatchInitialPosition( void PandaCommandReceiver::LatchInitialPosition( Context* context) const { DRAKE_THROW_UNLESS(context != nullptr); - DRAKE_THROW_UNLESS((control_mode_ & PandaControlMode::kPosition) != - PandaControlMode::kNone); + DRAKE_THROW_UNLESS((control_mode_ & PandaControlModes::kPosition) != + PandaControlModes::kNone); LatchInitialPosition(*context, &context->get_mutable_discrete_state()); } @@ -160,8 +161,8 @@ void PandaCommandReceiver::DoCalcNextUpdateTime( DRAKE_THROW_UNLESS(std::isinf(*time)); // If we're not using position control, then we have no state to latch. - if ((control_mode_ & PandaControlMode::kPosition) == - PandaControlMode::kNone) { + if ((control_mode_ & PandaControlModes::kPosition) == + PandaControlModes::kNone) { return; } @@ -186,8 +187,8 @@ void PandaCommandReceiver::CalcDefaultCommand( const Context& context, lcmt_panda_command* result) const { *result = {}; result->control_mode_expected = to_int(control_mode_); - if ((control_mode_ & PandaControlMode::kPosition) != - PandaControlMode::kNone) { + if ((control_mode_ & PandaControlModes::kPosition) != + PandaControlModes::kNone) { const BasicVector& latch_is_set = context.get_discrete_state(latched_position_measured_is_set_); const BasicVector& default_position = @@ -199,12 +200,13 @@ void PandaCommandReceiver::CalcDefaultCommand( result->num_joint_position = num_joints_; result->joint_position = {vec.data(), vec.data() + num_joints_}; } - if ((control_mode_ & PandaControlMode::kVelocity) != - PandaControlMode::kNone) { + if ((control_mode_ & PandaControlModes::kVelocity) != + PandaControlModes::kNone) { result->num_joint_velocity = num_joints_; result->joint_velocity = std::vector(num_joints_, 0.0); } - if ((control_mode_ & PandaControlMode::kTorque) != PandaControlMode::kNone) { + if ((control_mode_ & PandaControlModes::kTorque) != + PandaControlModes::kNone) { result->num_joint_torque = num_joints_; result->joint_torque = std::vector(num_joints_, 0.0); } @@ -237,8 +239,8 @@ void CheckNumJoints(const std::string& system_name, const char* vector_name, void PandaCommandReceiver::CalcPositionOutput( const Context& context, BasicVector* output) const { - DRAKE_DEMAND((control_mode_ & PandaControlMode::kPosition) != - PandaControlMode::kNone); + DRAKE_DEMAND((control_mode_ & PandaControlModes::kPosition) != + PandaControlModes::kNone); const auto& message = message_input_or_default_->Eval(context); DRAKE_DEMAND(message.control_mode_expected == to_int(control_mode_)); @@ -252,8 +254,8 @@ void PandaCommandReceiver::CalcPositionOutput( void PandaCommandReceiver::CalcVelocityOutput( const Context& context, BasicVector* output) const { - DRAKE_DEMAND((control_mode_ & PandaControlMode::kVelocity) != - PandaControlMode::kNone); + DRAKE_DEMAND((control_mode_ & PandaControlModes::kVelocity) != + PandaControlModes::kNone); const auto& message = message_input_or_default_->Eval(context); DRAKE_DEMAND(message.control_mode_expected == to_int(control_mode_)); @@ -267,8 +269,8 @@ void PandaCommandReceiver::CalcVelocityOutput( void PandaCommandReceiver::CalcTorqueOutput(const Context& context, BasicVector* output) const { - DRAKE_DEMAND((control_mode_ & PandaControlMode::kTorque) != - PandaControlMode::kNone); + DRAKE_DEMAND((control_mode_ & PandaControlModes::kTorque) != + PandaControlModes::kNone); const auto& message = message_input_or_default_->Eval(context); DRAKE_DEMAND(message.control_mode_expected == to_int(control_mode_)); diff --git a/manipulation/franka_panda/panda_command_receiver.h b/manipulation/franka_panda/panda_command_receiver.h index c499346c380f..6d33571dd6c6 100644 --- a/manipulation/franka_panda/panda_command_receiver.h +++ b/manipulation/franka_panda/panda_command_receiver.h @@ -56,7 +56,7 @@ class PandaCommandReceiver final : public drake::systems::LeafSystem { DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(PandaCommandReceiver); /// @param control_mode is a bitset of one or more control mode constants - /// defined in PandaControlMode enum. Use bitwise OR to combine modes. + /// defined in PandaControlModes namespace. Use bitwise OR to combine modes. PandaCommandReceiver(int num_joints, PandaControlMode control_mode); ~PandaCommandReceiver() final; diff --git a/manipulation/franka_panda/panda_command_sender.cc b/manipulation/franka_panda/panda_command_sender.cc index 23392d3aad54..fcdb9c253ab0 100644 --- a/manipulation/franka_panda/panda_command_sender.cc +++ b/manipulation/franka_panda/panda_command_sender.cc @@ -20,24 +20,25 @@ PandaCommandSender::PandaCommandSender(int num_joints, PandaControlMode control_mode) : num_joints_(num_joints), control_mode_(control_mode) { PandaControlMode remaining = control_mode_; - if ((control_mode_ & PandaControlMode::kPosition) != - PandaControlMode::kNone) { - remaining &= ~PandaControlMode::kPosition; + if ((control_mode_ & PandaControlModes::kPosition) != + PandaControlModes::kNone) { + remaining &= ~PandaControlModes::kPosition; position_input_port_ = &this->DeclareInputPort("position", kVectorValued, num_joints_); } - if ((control_mode_ & PandaControlMode::kVelocity) != - PandaControlMode::kNone) { - remaining &= ~PandaControlMode::kVelocity; + if ((control_mode_ & PandaControlModes::kVelocity) != + PandaControlModes::kNone) { + remaining &= ~PandaControlModes::kVelocity; velocity_input_port_ = &this->DeclareInputPort("velocity", kVectorValued, num_joints_); } - if ((control_mode_ & PandaControlMode::kTorque) != PandaControlMode::kNone) { - remaining &= ~PandaControlMode::kTorque; + if ((control_mode_ & PandaControlModes::kTorque) != + PandaControlModes::kNone) { + remaining &= ~PandaControlModes::kTorque; torque_input_port_ = &this->DeclareInputPort("torque", kVectorValued, num_joints_); } - if (remaining != PandaControlMode::kNone) { + if (remaining != PandaControlModes::kNone) { throw std::logic_error(fmt::format("Invalid control_mode bits set: 0x{:x}", to_int(remaining))); } diff --git a/manipulation/franka_panda/panda_command_sender.h b/manipulation/franka_panda/panda_command_sender.h index c4b5cbe1c5e8..df14d124c888 100644 --- a/manipulation/franka_panda/panda_command_sender.h +++ b/manipulation/franka_panda/panda_command_sender.h @@ -38,7 +38,7 @@ class PandaCommandSender final : public drake::systems::LeafSystem { DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(PandaCommandSender); /// @param control_mode is a bitset of one or more control mode constants - /// defined in PandaControlMode enum. Use bitwise OR to combine modes. + /// defined in PandaControlModes namespace. Use bitwise OR to combine modes. PandaCommandSender(int num_joints, PandaControlMode control_mode); ~PandaCommandSender() final; diff --git a/manipulation/franka_panda/panda_constants.h b/manipulation/franka_panda/panda_constants.h index 6b0faee0c89c..eb4b25f20568 100644 --- a/manipulation/franka_panda/panda_constants.h +++ b/manipulation/franka_panda/panda_constants.h @@ -8,40 +8,18 @@ namespace franka_panda { const int kPandaArmNumJoints = 7; -/** Control modes for the Panda robot. These can be bitwise OR'd together - * to enable multiple control modes simultaneously. +/** Type alias for Panda control mode bitfields. Control modes can be bitwise + * OR'd together to enable multiple control modes simultaneously. * Values match lcmt_panda_status::CONTROL_MODE_* constants. */ -enum class PandaControlMode : int { - kNone = 0, - kPosition = 1, - kVelocity = 2, - kTorque = 4, -}; +using PandaControlMode = uint64_t; -// Enable bitwise operations for PandaControlMode -constexpr PandaControlMode operator|(PandaControlMode a, PandaControlMode b) { - return static_cast(static_cast(a) | - static_cast(b)); -} - -constexpr PandaControlMode operator&(PandaControlMode a, PandaControlMode b) { - return static_cast(static_cast(a) & - static_cast(b)); -} - -constexpr PandaControlMode operator~(PandaControlMode a) { - return static_cast(~static_cast(a)); -} - -constexpr PandaControlMode& operator|=(PandaControlMode& a, - PandaControlMode b) { - return a = a | b; -} - -constexpr PandaControlMode& operator&=(PandaControlMode& a, - PandaControlMode b) { - return a = a & b; -} +/** Control mode constants for the Panda robot. */ +namespace PandaControlModes { +constexpr PandaControlMode kNone = 0; +constexpr PandaControlMode kPosition = 1; +constexpr PandaControlMode kVelocity = 2; +constexpr PandaControlMode kTorque = 4; +} // namespace PandaControlModes // Helper to convert to int for use in comparisons and LCM messages constexpr int to_int(PandaControlMode mode) { @@ -49,15 +27,15 @@ constexpr int to_int(PandaControlMode mode) { } // Ensure our constants match the LCM message definition -static_assert(to_int(PandaControlMode::kPosition) == +static_assert(to_int(PandaControlModes::kPosition) == drake::lcmt_panda_status::CONTROL_MODE_POSITION, - "PandaControlMode::kPosition must match LCM definition"); -static_assert(to_int(PandaControlMode::kVelocity) == + "PandaControlModes::kPosition must match LCM definition"); +static_assert(to_int(PandaControlModes::kVelocity) == drake::lcmt_panda_status::CONTROL_MODE_VELOCITY, - "PandaControlMode::kVelocity must match LCM definition"); -static_assert(to_int(PandaControlMode::kTorque) == + "PandaControlModes::kVelocity must match LCM definition"); +static_assert(to_int(PandaControlModes::kTorque) == drake::lcmt_panda_status::CONTROL_MODE_TORQUE, - "PandaControlMode::kTorque must match LCM definition"); + "PandaControlModes::kTorque must match LCM definition"); } // namespace franka_panda } // namespace manipulation diff --git a/manipulation/franka_panda/test/panda_command_receiver_test.cc b/manipulation/franka_panda/test/panda_command_receiver_test.cc index 967bda07d731..4882c984d245 100644 --- a/manipulation/franka_panda/test/panda_command_receiver_test.cc +++ b/manipulation/franka_panda/test/panda_command_receiver_test.cc @@ -24,7 +24,7 @@ class PandaCommandReceiverTest : public testing::Test { public: PandaCommandReceiverTest() : dut_(kPandaArmNumJoints, - PandaControlMode::kPosition | PandaControlMode::kTorque), + PandaControlModes::kPosition | PandaControlModes::kTorque), context_ptr_(dut_.CreateDefaultContext()), context_(*context_ptr_), fixed_input_(FixInput()) {} @@ -81,7 +81,7 @@ TEST_F(PandaCommandReceiverTest, AcceptanceTestWithMeasuredPositionInput) { const VectorXd t1 = VectorXd::LinSpaced(N, 0.5, 0.6); lcmt_panda_command command{}; command.control_mode_expected = - to_int(PandaControlMode::kPosition | PandaControlMode::kTorque); + to_int(PandaControlModes::kPosition | PandaControlModes::kTorque); command.utime = 0; command.num_joint_position = N; command.joint_position = {q1.data(), q1.data() + q1.size()}; @@ -127,7 +127,7 @@ TEST_F(PandaCommandReceiverTest, AcceptanceTestWithLatching) { const VectorXd t3 = VectorXd::LinSpaced(N, 0.6, 0.7); lcmt_panda_command command{}; command.control_mode_expected = - to_int(PandaControlMode::kPosition | PandaControlMode::kTorque); + to_int(PandaControlModes::kPosition | PandaControlModes::kTorque); command.utime = 0; command.num_joint_position = N; command.joint_position = {q3.data(), q3.data() + q3.size()}; @@ -140,7 +140,7 @@ TEST_F(PandaCommandReceiverTest, AcceptanceTestWithLatching) { GTEST_TEST(PandaCommandReceiverTestVelocity, VelocityControl) { PandaCommandReceiver receiver(kPandaArmNumJoints, - PandaControlMode::kVelocity); + PandaControlModes::kVelocity); EXPECT_EQ(receiver.num_discrete_state_groups(), 0); EXPECT_THROW(receiver.get_commanded_position_output_port(), @@ -148,7 +148,7 @@ GTEST_TEST(PandaCommandReceiverTestVelocity, VelocityControl) { EXPECT_THROW(receiver.get_commanded_torque_output_port(), std::runtime_error); lcmt_panda_command command{}; - command.control_mode_expected = to_int(PandaControlMode::kVelocity); + command.control_mode_expected = to_int(PandaControlModes::kVelocity); command.num_joint_velocity = kPandaArmNumJoints; command.joint_velocity.resize(kPandaArmNumJoints, 0.1); diff --git a/manipulation/franka_panda/test/panda_command_sender_test.cc b/manipulation/franka_panda/test/panda_command_sender_test.cc index 1ac9ac38c293..b8f3d6fa962e 100644 --- a/manipulation/franka_panda/test/panda_command_sender_test.cc +++ b/manipulation/franka_panda/test/panda_command_sender_test.cc @@ -24,7 +24,7 @@ constexpr int N = kPandaArmNumJoints; class PandaCommandSenderTest : public testing::Test { public: PandaCommandSenderTest() - : dut_(N, PandaControlMode::kPosition | PandaControlMode::kTorque), + : dut_(N, PandaControlModes::kPosition | PandaControlModes::kTorque), context_ptr_(dut_.CreateDefaultContext()), context_(*context_ptr_) {} @@ -51,7 +51,7 @@ TEST_F(PandaCommandSenderTest, PositionAndTorque) { dut_.get_position_input_port().FixValue(&context_, q0_); dut_.get_torque_input_port().FixValue(&context_, t0_); EXPECT_EQ(output().control_mode_expected, - to_int(PandaControlMode::kPosition | PandaControlMode::kTorque)); + to_int(PandaControlModes::kPosition | PandaControlModes::kTorque)); EXPECT_EQ(output().num_joint_position, N); EXPECT_EQ(output().joint_position, std_q0_); EXPECT_EQ(output().num_joint_velocity, 0); @@ -61,12 +61,12 @@ TEST_F(PandaCommandSenderTest, PositionAndTorque) { } TEST_F(PandaCommandSenderTest, PositionOnly) { - const PandaCommandSender sender(N, PandaControlMode::kPosition); + const PandaCommandSender sender(N, PandaControlModes::kPosition); auto context = sender.CreateDefaultContext(); sender.get_position_input_port().FixValue(context.get(), q0_); const lcmt_panda_command output = sender.get_output_port().Eval(*context); - EXPECT_EQ(output.control_mode_expected, to_int(PandaControlMode::kPosition)); + EXPECT_EQ(output.control_mode_expected, to_int(PandaControlModes::kPosition)); EXPECT_EQ(output.num_joint_position, N); EXPECT_EQ(output.joint_position, std_q0_); EXPECT_EQ(output.num_joint_velocity, 0); @@ -76,12 +76,12 @@ TEST_F(PandaCommandSenderTest, PositionOnly) { } TEST_F(PandaCommandSenderTest, VelocityOnly) { - const PandaCommandSender sender(N, PandaControlMode::kVelocity); + const PandaCommandSender sender(N, PandaControlModes::kVelocity); auto context = sender.CreateDefaultContext(); sender.get_velocity_input_port().FixValue(context.get(), v0_); const lcmt_panda_command output = sender.get_output_port().Eval(*context); - EXPECT_EQ(output.control_mode_expected, to_int(PandaControlMode::kVelocity)); + EXPECT_EQ(output.control_mode_expected, to_int(PandaControlModes::kVelocity)); EXPECT_EQ(output.num_joint_position, 0); EXPECT_EQ(output.joint_position.size(), 0); EXPECT_EQ(output.num_joint_velocity, N); @@ -91,12 +91,12 @@ TEST_F(PandaCommandSenderTest, VelocityOnly) { } TEST_F(PandaCommandSenderTest, TorqueOnly) { - const PandaCommandSender sender(N, PandaControlMode::kTorque); + const PandaCommandSender sender(N, PandaControlModes::kTorque); auto context = sender.CreateDefaultContext(); sender.get_torque_input_port().FixValue(context.get(), t0_); const lcmt_panda_command output = sender.get_output_port().Eval(*context); - EXPECT_EQ(output.control_mode_expected, to_int(PandaControlMode::kTorque)); + EXPECT_EQ(output.control_mode_expected, to_int(PandaControlModes::kTorque)); EXPECT_EQ(output.num_joint_position, 0); EXPECT_EQ(output.joint_position.size(), 0); EXPECT_EQ(output.num_joint_velocity, 0); From dad0f6f8cb984c53b23d9ca65d5ae9379709967d Mon Sep 17 00:00:00 2001 From: Peter Date: Tue, 11 Nov 2025 15:14:01 -0500 Subject: [PATCH 10/12] adds pete to drake contributors --- doc/_pages/credits.md | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/_pages/credits.md b/doc/_pages/credits.md index 366a74bfcff7..408908748d16 100644 --- a/doc/_pages/credits.md +++ b/doc/_pages/credits.md @@ -65,6 +65,7 @@ this is modeled directly, and shamelessly, on: http://eigen.tuxfamily.org/index. * [Russ Tedrake](http://people.csail.mit.edu/russt/) * Belinda Teh * Andres Valenzuela +* Peter Werner * [David von Wrangel](https://www.davidvonwrangel.com/) * Matthew Woehlke * Huihua Zhao From 31fc2dd3dcf41ecbb590f01ea2250dc4c037d2ae Mon Sep 17 00:00:00 2001 From: Grant Gould Date: Thu, 13 Nov 2025 11:55:50 -0500 Subject: [PATCH 11/12] Update bindings docstring and fix lint errors --- .../manipulation_franka_panda.h | 20 +++++++++++++++++-- .../manipulation_py_franka_panda.cc | 9 +++++---- .../manipulation/test/franka_panda_test.py | 2 +- 3 files changed, 24 insertions(+), 7 deletions(-) diff --git a/bindings/generated_docstrings/manipulation_franka_panda.h b/bindings/generated_docstrings/manipulation_franka_panda.h index 0c93a778714c..4a6ff0d21df6 100644 --- a/bindings/generated_docstrings/manipulation_franka_panda.h +++ b/bindings/generated_docstrings/manipulation_franka_panda.h @@ -92,7 +92,7 @@ already using a Simulator or other special cases. const char* doc = R"""(Parameter ``control_mode``: is a bitset of one or more control mode constants defined in - lcmt_panda_status::CONTROL_MODE_{POSITION,VELOCITY,TORQUE}.)"""; + PandaControlModes namespace. Use bitwise OR to combine modes.)"""; } ctor; // Symbol: drake::manipulation::franka_panda::PandaCommandReceiver::get_commanded_position_output_port struct /* get_commanded_position_output_port */ { @@ -157,7 +157,7 @@ See also: const char* doc = R"""(Parameter ``control_mode``: is a bitset of one or more control mode constants defined in - lcmt_panda_status::CONTROL_MODE_{POSITION,VELOCITY,TORQUE}.)"""; + PandaControlModes namespace. Use bitwise OR to combine modes.)"""; } ctor; // Symbol: drake::manipulation::franka_panda::PandaCommandSender::get_position_input_port struct /* get_position_input_port */ { @@ -175,6 +175,17 @@ R"""(Parameter ``control_mode``: const char* doc = R"""()"""; } get_velocity_input_port; } PandaCommandSender; + // Symbol: drake::manipulation::franka_panda::PandaControlMode + struct /* PandaControlMode */ { + // Source: drake/manipulation/franka_panda/panda_constants.h + const char* doc = +R"""(Type alias for Panda control mode bitfields. Control modes can be +bitwise OR'd together to enable multiple control modes simultaneously. +Values match lcmt_panda_status::CONTROL_MODE_* constants.)"""; + } PandaControlMode; + // Symbol: drake::manipulation::franka_panda::PandaControlModes + struct /* PandaControlModes */ { + } PandaControlModes; // Symbol: drake::manipulation::franka_panda::PandaStatusReceiver struct /* PandaStatusReceiver */ { // Source: drake/manipulation/franka_panda/panda_status_receiver.h @@ -324,6 +335,11 @@ See also: const char* doc = R"""()"""; } get_velocity_measured_input_port; } PandaStatusSender; + // Symbol: drake::manipulation::franka_panda::to_int + struct /* to_int */ { + // Source: drake/manipulation/franka_panda/panda_constants.h + const char* doc = R"""()"""; + } to_int; } franka_panda; } manipulation; } drake; diff --git a/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc b/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc index f1cf962ad78a..5e25212be844 100644 --- a/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc +++ b/bindings/pydrake/manipulation/manipulation_py_franka_panda.cc @@ -25,10 +25,11 @@ void DefineManipulationFrankaPanda(py::module m) { m.attr("kPandaArmNumJoints") = kPandaArmNumJoints; // Control mode constants. PandaControlMode is a uint64_t typedef, so we - // expose the constants as integers. Python's native bitwise operators (| and &) - // work with these integers, and pybind11 automatically converts them to uint64_t - // when calling C++ functions. - py::object control_mode_class = py::module_::import("types").attr("SimpleNamespace")(); + // expose the constants as integers. Python's native bitwise operators (| and + // &) work with these integers, and pybind11 automatically converts them to + // uint64_t when calling C++ functions. + py::object control_mode_class = + py::module_::import("types").attr("SimpleNamespace")(); control_mode_class.attr("__doc__") = "Control modes for the Panda robot. These can be bitwise OR'd together."; control_mode_class.attr("kNone") = py::int_(PandaControlModes::kNone); diff --git a/bindings/pydrake/manipulation/test/franka_panda_test.py b/bindings/pydrake/manipulation/test/franka_panda_test.py index 0c0834a0653b..0a3a05516405 100644 --- a/bindings/pydrake/manipulation/test/franka_panda_test.py +++ b/bindings/pydrake/manipulation/test/franka_panda_test.py @@ -35,7 +35,7 @@ def test_control_mode_bitwise_or(self): mut.PandaControlMode.kPosition | mut.PandaControlMode.kVelocity | mut.PandaControlMode.kTorque, - 7 + 7, ) def test_franka_panda_command_receiver(self): From 275137e9bf04a24d2863016d34868f89e6c37aed Mon Sep 17 00:00:00 2001 From: Peter Date: Fri, 14 Nov 2025 13:58:17 -0500 Subject: [PATCH 12/12] fixes nit --- manipulation/franka_panda/panda_status_sender.cc | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/manipulation/franka_panda/panda_status_sender.cc b/manipulation/franka_panda/panda_status_sender.cc index 457fb7db51e2..e44be67ed8cc 100644 --- a/manipulation/franka_panda/panda_status_sender.cc +++ b/manipulation/franka_panda/panda_status_sender.cc @@ -115,9 +115,10 @@ void PandaStatusSender::CalcOutput(const Context& context, const auto& torque_measured = EvalFirstConnected( context, 1, 2, zero_vector_, get_torque_measured_input_port(), get_torque_commanded_input_port()); - const auto& torque_external = EvalFirstConnected( - context, 0, 2, zero_vector_, get_torque_external_input_port(), - get_torque_external_input_port()); + const auto& torque_external = + get_torque_external_input_port().HasValue(context) + ? get_torque_external_input_port().Eval(context) + : zero_vector_.head(num_joints_); lcmt_panda_status& status = *output; status.utime = context.get_time() * 1e6;