Adds LCM franka panda command publishers and receivers

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",

bindings/pydrake/manipulation/BUILD.bazel

@@ -23,6 +23,7 @@ PACKAGE_INFO = get_pybind_package_info("//bindings")
 drake_pybind_library(
     name = "manipulation",
     cc_deps = [
+        "//bindings/generated_docstrings:manipulation_franka_panda",
         "//bindings/generated_docstrings:manipulation_kuka_iiwa",
         "//bindings/generated_docstrings:manipulation_schunk_wsg",
         "//bindings/generated_docstrings:manipulation_util",
@@ -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 = [

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);
 

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);
 

bindings/pydrake/manipulation/manipulation_py_franka_panda.cc

@@ -0,0 +1,163 @@
+#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/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"
+#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::franka_panda;
+  constexpr auto& doc =
+      pydrake_doc_manipulation_franka_panda.drake.manipulation.franka_panda;
+
+  // Constants.
+  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")();
+  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;
+    py::class_<Class, LeafSystem<double>>(
+        m, "PandaCommandReceiver", cls_doc.doc)
+        .def(py::init<int, PandaControlMode>(), py::arg("num_joints"),
+            py::arg("control_mode"), cls_doc.ctor.doc)
+        .def("LatchInitialPosition",
+            overload_cast_explicit<void, drake::systems::Context<double>*>(
+                &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_<Class, LeafSystem<double>>(m, "PandaCommandSender", cls_doc.doc)
+        .def(py::init<int, PandaControlMode>(), 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)
+        .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_<Class, LeafSystem<double>>(m, "PandaStatusReceiver", cls_doc.doc)
+        .def(py::init<int>(), 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_<Class, LeafSystem<double>>(m, "PandaStatusSender", cls_doc.doc)
+        .def(py::init<int>(), 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

bindings/pydrake/manipulation/test/franka_panda_test.py

@@ -0,0 +1,155 @@
+# -*- coding: utf-8 -*-
+
+import unittest
+
+import pydrake.manipulation as mut
+from pydrake.systems.framework import (
+    InputPort,
+    OutputPort,
+)
+
+
+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 = (
+            mut.PandaControlMode.kPosition | mut.PandaControlMode.kTorque
+        )
+        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 = (
+            mut.PandaControlMode.kPosition
+            | mut.PandaControlMode.kVelocity
+            | mut.PandaControlMode.kTorque
+        )
+        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 = (
+            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)
+
+        # Test with position, velocity, and torque control mode
+        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
+        )
+        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)

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