From 80e8941524823198cb50d096457d1a9cbf082348 Mon Sep 17 00:00:00 2001 From: "Hyung Ju Suh (Terry)" Date: Sun, 2 May 2021 09:47:07 -0400 Subject: [PATCH] Add bindings for IiwaCommand/Status Sender/Receivers (#14987) --- bindings/pydrake/manipulation/BUILD.bazel | 22 +++ bindings/pydrake/manipulation/all.py | 1 + bindings/pydrake/manipulation/kuka_iiwa_py.cc | 129 ++++++++++++++++++ .../manipulation/test/kuka_iiwa_test.py | 58 ++++++++ bindings/pydrake/systems/BUILD.bazel | 2 + .../systems/lcm_py_bind_cpp_serializers.cc | 8 ++ 6 files changed, 220 insertions(+) create mode 100644 bindings/pydrake/manipulation/kuka_iiwa_py.cc create mode 100644 bindings/pydrake/manipulation/test/kuka_iiwa_test.py diff --git a/bindings/pydrake/manipulation/BUILD.bazel b/bindings/pydrake/manipulation/BUILD.bazel index 846a0e4d3ccf..049f6476506e 100644 --- a/bindings/pydrake/manipulation/BUILD.bazel +++ b/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 = [ diff --git a/bindings/pydrake/manipulation/all.py b/bindings/pydrake/manipulation/all.py index 01b100ca5599..1818164e48a1 100644 --- a/bindings/pydrake/manipulation/all.py +++ b/bindings/pydrake/manipulation/all.py @@ -1,3 +1,4 @@ +from .kuka_iiwa import * from .planner import * from .schunk_wsg import * from .simple_ui import * diff --git a/bindings/pydrake/manipulation/kuka_iiwa_py.cc b/bindings/pydrake/manipulation/kuka_iiwa_py.cc new file mode 100644 index 000000000000..f999d823cca3 --- /dev/null +++ b/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_>(m, "IiwaCommandReceiver", cls_doc.doc) + .def(py::init(), 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_>(m, "IiwaCommandSender", cls_doc.doc) + .def(py::init(), 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_>(m, "IiwaStatusReceiver", cls_doc.doc) + .def(py::init(), 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_>(m, "IiwaStatusSender", cls_doc.doc) + .def(py::init(), 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 diff --git a/bindings/pydrake/manipulation/test/kuka_iiwa_test.py b/bindings/pydrake/manipulation/test/kuka_iiwa_test.py new file mode 100644 index 000000000000..175612e98d7f --- /dev/null +++ b/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,)) diff --git a/bindings/pydrake/systems/BUILD.bazel b/bindings/pydrake/systems/BUILD.bazel index 14374b8e74c9..abef1d8ffe26 100644 --- a/bindings/pydrake/systems/BUILD.bazel +++ b/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", diff --git a/bindings/pydrake/systems/lcm_py_bind_cpp_serializers.cc b/bindings/pydrake/systems/lcm_py_bind_cpp_serializers.cc index af96d933eb52..0dae233c490f 100644 --- a/bindings/pydrake/systems/lcm_py_bind_cpp_serializers.cc +++ b/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"); + BindCppSerializer("drake"); + BindCppSerializer("drake"); BindCppSerializer("drake"); BindCppSerializer("drake"); + BindCppSerializer("drake"); + BindCppSerializer("drake"); } } // namespace pylcm