Add bindings for IiwaCommand/Status Sender/Receivers (#14987)

bindings/pydrake/manipulation/BUILD.bazel

@@ -32,6 +32,19 @@ drake_py_library(
     ],
 )
 
+drake_pybind_library(
+    name = "kuka_iiwa_py",
+    cc_deps = [
+        "//bindings/pydrake:documentation_pybind",
+        "//bindings/pydrake/common:eigen_pybind",
+    ],
+    cc_srcs = ["kuka_iiwa_py.cc"],
+    package_info = PACKAGE_INFO,
+    py_deps = [
+        ":module_py",
+    ],
+)
+
 drake_pybind_library(
     name = "planner_py",
     cc_deps = [
@@ -66,6 +79,7 @@ drake_py_library(
 )
 
 PY_LIBRARIES_WITH_INSTALL = [
+    ":kuka_iiwa_py",
     ":planner_py",
     ":schunk_wsg_py",
 ]
@@ -75,6 +89,14 @@ PY_LIBRARIES = [
     ":simple_ui_py",
 ]
 
+drake_py_unittest(
+    name = "kuka_iiwa_test",
+    deps = [
+        ":kuka_iiwa_py",
+        "//bindings/pydrake/systems",
+    ],
+)
+
 drake_py_unittest(
     name = "planner_test",
     data = [

bindings/pydrake/manipulation/all.py

@@ -1,3 +1,4 @@
+from .kuka_iiwa import *
 from .planner import *
 from .schunk_wsg import *
 from .simple_ui import *

bindings/pydrake/manipulation/kuka_iiwa_py.cc

@@ -0,0 +1,129 @@
+#include "pybind11/eigen.h"
+#include "pybind11/pybind11.h"
+
+#include "drake/bindings/pydrake/documentation_pybind.h"
+#include "drake/bindings/pydrake/pydrake_pybind.h"
+#include "drake/manipulation/kuka_iiwa/iiwa_command_receiver.h"
+#include "drake/manipulation/kuka_iiwa/iiwa_command_sender.h"
+#include "drake/manipulation/kuka_iiwa/iiwa_constants.h"
+#include "drake/manipulation/kuka_iiwa/iiwa_status_receiver.h"
+#include "drake/manipulation/kuka_iiwa/iiwa_status_sender.h"
+
+namespace drake {
+namespace pydrake {
+
+PYBIND11_MODULE(kuka_iiwa, m) {
+  using drake::systems::Diagram;
+  using drake::systems::LeafSystem;
+
+  m.doc() = "Tools for kuka iiwa.";
+
+  // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace.
+  using namespace drake::manipulation::kuka_iiwa;
+  constexpr auto& doc = pydrake_doc.drake.manipulation.kuka_iiwa;
+
+  py::module::import("pydrake.systems.framework");
+
+  {
+    using Class = IiwaCommandReceiver;
+    constexpr auto& cls_doc = doc.IiwaCommandReceiver;
+    py::class_<Class, LeafSystem<double>>(m, "IiwaCommandReceiver", cls_doc.doc)
+        .def(py::init<int>(), py::arg("num_joints") = kIiwaArmNumJoints,
+            cls_doc.ctor.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_torque_output_port",
+            &Class::get_commanded_torque_output_port,
+            py_rvp::reference_internal,
+            cls_doc.get_commanded_torque_output_port.doc);
+  }
+
+  {
+    using Class = IiwaCommandSender;
+    constexpr auto& cls_doc = doc.IiwaCommandSender;
+    py::class_<Class, LeafSystem<double>>(m, "IiwaCommandSender", cls_doc.doc)
+        .def(py::init<int>(), py::arg("num_joints") = kIiwaArmNumJoints,
+            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_torque_input_port", &Class::get_torque_input_port,
+            py_rvp::reference_internal, cls_doc.get_torque_input_port.doc);
+  }
+
+  {
+    using Class = IiwaStatusReceiver;
+    constexpr auto& cls_doc = doc.IiwaStatusReceiver;
+    py::class_<Class, LeafSystem<double>>(m, "IiwaStatusReceiver", cls_doc.doc)
+        .def(py::init<int>(), py::arg("num_joints") = kIiwaArmNumJoints,
+            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_estimated_output_port",
+            &Class::get_velocity_estimated_output_port,
+            py_rvp::reference_internal,
+            cls_doc.get_velocity_estimated_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 = IiwaStatusSender;
+    constexpr auto& cls_doc = doc.IiwaStatusSender;
+    py::class_<Class, LeafSystem<double>>(m, "IiwaStatusSender", cls_doc.doc)
+        .def(py::init<int>(), py::arg("num_joints") = kIiwaArmNumJoints,
+            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_estimated_input_port",
+            &Class::get_velocity_estimated_input_port,
+            py_rvp::reference_internal,
+            cls_doc.get_velocity_estimated_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);
+  }
+
+  {
+    m.def(
+        "get_iiwa_max_joint_velocities",
+        []() { return get_iiwa_max_joint_velocities(); },
+        doc.get_iiwa_max_joint_velocities.doc);
+  }
+}
+
+}  // namespace pydrake
+}  // namespace drake

bindings/pydrake/manipulation/test/kuka_iiwa_test.py

@@ -0,0 +1,58 @@
+# -*- coding: utf-8 -*-
+
+import pydrake.manipulation.kuka_iiwa as mut
+
+import unittest
+import numpy as np
+
+from pydrake.systems.framework import InputPort, OutputPort
+
+
+class TestKukaIiwa(unittest.TestCase):
+    def test_kuka_iiwa_lcm(self):
+        command_rec = mut.IiwaCommandReceiver()
+        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)
+
+        command_send = mut.IiwaCommandSender()
+        self.assertIsInstance(
+            command_send.get_position_input_port(), InputPort)
+        self.assertIsInstance(
+            command_send.get_torque_input_port(), InputPort)
+
+        status_rec = mut.IiwaStatusReceiver()
+        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_estimated_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)
+
+        status_send = mut.IiwaStatusSender()
+        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_estimated_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)
+
+    def test_kuka_iiwa_api(self):
+        self.assertEqual(mut.get_iiwa_max_joint_velocities().shape, (7,))

bindings/pydrake/systems/BUILD.bazel

@@ -215,8 +215,10 @@ drake_pybind_library(
         ":lcm_pybind",
         "//bindings/pydrake:documentation_pybind",
         "//lcmtypes:contact_results_for_viz",
+        "//lcmtypes:iiwa",
         "//lcmtypes:image_array",
         "//lcmtypes:quaternion",
+        "//lcmtypes:schunk",
     ],
     cc_srcs = [
         "lcm_py.cc",

bindings/pydrake/systems/lcm_py_bind_cpp_serializers.cc

@@ -2,8 +2,12 @@
 
 #include "drake/bindings/pydrake/systems/lcm_pybind.h"
 #include "drake/lcmt_contact_results_for_viz.hpp"
+#include "drake/lcmt_iiwa_command.hpp"
+#include "drake/lcmt_iiwa_status.hpp"
 #include "drake/lcmt_image_array.hpp"
 #include "drake/lcmt_quaternion.hpp"
+#include "drake/lcmt_schunk_wsg_command.hpp"
+#include "drake/lcmt_schunk_wsg_status.hpp"
 
 namespace drake {
 namespace pydrake {
@@ -14,8 +18,12 @@ void BindCppSerializers() {
   // N.B. At least one type should be bound to ensure the template is defined.
   // N.B. These should be placed in the same order as the headers.
   BindCppSerializer<drake::lcmt_contact_results_for_viz>("drake");
+  BindCppSerializer<drake::lcmt_iiwa_command>("drake");
+  BindCppSerializer<drake::lcmt_iiwa_status>("drake");
   BindCppSerializer<drake::lcmt_image_array>("drake");
   BindCppSerializer<drake::lcmt_quaternion>("drake");
+  BindCppSerializer<drake::lcmt_schunk_wsg_command>("drake");
+  BindCppSerializer<drake::lcmt_schunk_wsg_status>("drake");
 }
 
 }  // namespace pylcm