Bind PoseBundle, FrameVelocity, PoseAggregator, SpatialVector, Spatia…

…lVelocity (RobotLocomotion#8282)

* Bind PoseBundle, FrameVelocity, PoseAggregator

* multibody_tree_py: Make `math` an internal submodule to avoid solib shenanigans

bindings/pydrake/multibody/BUILD.bazel

@@ -31,6 +31,20 @@ drake_py_library(
     ],
 )
 
+drake_pybind_library(
+    name = "multibody_tree_py",
+    cc_deps = [
+        "//bindings/pydrake/util:eigen_geometry_pybind",
+    ],
+    cc_so_name = "multibody_tree",
+    cc_srcs = ["multibody_tree_py.cc"],
+    package_info = PACKAGE_INFO,
+    py_deps = [
+        ":module_py",
+        "//bindings/pydrake/util:eigen_geometry_py",
+    ],
+)
+
 drake_pybind_library(
     name = "parsers_py",
     cc_so_name = "parsers",
@@ -85,6 +99,7 @@ drake_pybind_library(
 )
 
 PY_LIBRARIES_WITH_INSTALL = [
+    ":multibody_tree_py",
     ":parsers_py",
     ":rigid_body_plant_py",
     ":rigid_body_tree_py",
@@ -157,4 +172,12 @@ drake_py_unittest(
     ],
 )
 
+drake_py_unittest(
+    name = "multibody_tree_test",
+    size = "small",
+    deps = [
+        ":multibody_tree_py",
+    ],
+)
+
 add_lint_tests()

bindings/pydrake/multibody/all.py

@@ -2,3 +2,6 @@
 from .rigid_body_plant import *
 from .rigid_body_tree import *
 from .shapes import *
+
+# Submodules.
+from .multibody_tree.all import *

bindings/pydrake/multibody/multibody_tree_py.cc

@@ -0,0 +1,62 @@
+#include "pybind11/eigen.h"
+#include "pybind11/eval.h"
+#include "pybind11/pybind11.h"
+
+#include "drake/bindings/pydrake/pydrake_pybind.h"
+#include "drake/bindings/pydrake/util/eigen_geometry_pybind.h"
+#include "drake/multibody/multibody_tree/math/spatial_force.h"
+#include "drake/multibody/multibody_tree/math/spatial_vector.h"
+#include "drake/multibody/multibody_tree/math/spatial_velocity.h"
+
+namespace drake {
+namespace pydrake {
+
+void init_math(py::module m) {
+  // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace.
+  using namespace drake::multibody;
+
+  m.doc() = "MultibodyTree math functionality.";
+
+  using T = double;
+
+  py::class_<SpatialVector<SpatialVelocity, T>>(m, "SpatialVector")
+      .def("rotational",
+           [](const SpatialVector<SpatialVelocity, T>* self)
+               -> const Vector3<T>& { return self->rotational(); },
+           py_reference_internal)
+      .def("translational",
+           [](const SpatialVector<SpatialVelocity, T>* self)
+               -> const Vector3<T>& { return self->translational(); },
+           py_reference_internal);
+
+  py::class_<SpatialVelocity<T>, SpatialVector<SpatialVelocity, T>>(
+      m, "SpatialVelocity")
+      .def(py::init())
+      .def(py::init<const Eigen::Ref<const Vector3<T>>&,
+                    const Eigen::Ref<const Vector3<T>>&>(),
+           py::arg("w"), py::arg("v"));
+
+  // TODO(jadecastro, eric.cousineau): Bind additional classes as necessary.
+}
+
+void init_all(py::module m) {
+  // Not sure if relative imports will work in this context, so we will
+  // manually spell it out.
+  py::dict vars = m.attr("__dict__");
+  py::exec(
+      "from pydrake.multibody.multibody_tree.math import *",
+      py::globals(), vars);
+}
+
+PYBIND11_MODULE(multibody_tree, m) {
+  m.doc() = "MultibodyTree functionality.";
+
+  // N.B. At present, we cannot have `math` as a submodule here, and in
+  // `pydrake`. The current solution is to manually define submodules.
+  // See the dicussion in #8282 for more information.
+  init_math(m.def_submodule("math"));
+  init_all(m.def_submodule("all"));
+}
+
+}  // namespace pydrake
+}  // namespace drake

bindings/pydrake/multibody/test/multibody_tree_test.py

@@ -0,0 +1,25 @@
+# -*- coding: utf-8 -*-
+
+from pydrake.multibody.multibody_tree.math import (
+    SpatialVelocity,
+)
+
+import copy
+import unittest
+import numpy as np
+
+
+class TestMath(unittest.TestCase):
+    def test_spatial_velocity(self):
+        velocity = SpatialVelocity()
+        # - Accessors.
+        self.assertTrue(isinstance(velocity.rotational(), np.ndarray))
+        self.assertTrue(isinstance(velocity.translational(), np.ndarray))
+        self.assertEqual(velocity.rotational().shape, (3,))
+        self.assertEqual(velocity.translational().shape, (3,))
+        # - Fully-parameterized constructor.
+        w = [0.1, 0.3, 0.5]
+        v = [0., 1., 2.]
+        velocity1 = SpatialVelocity(w=w, v=v)
+        self.assertTrue(np.allclose(velocity1.rotational(), w))
+        self.assertTrue(np.allclose(velocity1.translational(), v))

bindings/pydrake/systems/BUILD.bazel

@@ -130,6 +130,7 @@ drake_pybind_library(
 drake_pybind_library(
     name = "rendering_py",
     cc_deps = [
+        ":systems_pybind",
         "//bindings/pydrake/util:eigen_geometry_pybind",
     ],
     cc_so_name = "rendering",
@@ -287,6 +288,7 @@ drake_py_unittest(
     size = "small",
     deps = [
         ":rendering_py",
+        "//bindings/pydrake/multibody:multibody_tree_py",
     ],
 )
 

bindings/pydrake/systems/framework_py.cc

@@ -494,7 +494,8 @@ PYBIND11_MODULE(framework, m) {
          py_reference_internal);
 
   py::class_<InputPortDescriptor<T>>(m, "InputPortDescriptor")
-    .def("size", &InputPortDescriptor<T>::size);
+    .def("size", &InputPortDescriptor<T>::size)
+    .def("get_data_type", &InputPortDescriptor<T>::get_data_type);
 
   // Value types.
   py::class_<VectorBase<T>>(m, "VectorBase")

bindings/pydrake/systems/rendering_py.cc

@@ -1,8 +1,13 @@
+#include <Eigen/Dense>
 #include "pybind11/eigen.h"
 #include "pybind11/pybind11.h"
 
 #include "drake/bindings/pydrake/pydrake_pybind.h"
+#include "drake/bindings/pydrake/systems/systems_pybind.h"
 #include "drake/bindings/pydrake/util/eigen_geometry_pybind.h"
+#include "drake/multibody/multibody_tree/math/spatial_velocity.h"
+#include "drake/systems/rendering/frame_velocity.h"
+#include "drake/systems/rendering/pose_aggregator.h"
 #include "drake/systems/rendering/pose_vector.h"
 
 namespace drake {
@@ -23,6 +28,9 @@ PYBIND11_MODULE(rendering, m) {
   py::class_<PoseVector<T>, BasicVector<T>> pose_vector(m, "PoseVector");
   pose_vector
     .def(py::init())
+    .def(py::init<const Eigen::Quaternion<T>&,
+                  const Eigen::Translation<T, 3>&>(),
+         py::arg("rotation"), py::arg("translation"))
     .def("get_isometry", &PoseVector<T>::get_isometry)
     .def("get_translation", &PoseVector<T>::get_translation)
     .def("set_translation", &PoseVector<T>::set_translation)
@@ -31,6 +39,38 @@ PYBIND11_MODULE(rendering, m) {
 
   pose_vector.attr("kSize") = int{PoseVector<T>::kSize};
 
+  py::class_<FrameVelocity<T>, BasicVector<T>> frame_velocity(
+      m, "FrameVelocity");
+  frame_velocity
+    .def(py::init())
+    .def(py::init<const multibody::SpatialVelocity<T>&>(), py::arg("velocity"))
+    .def("get_velocity", &FrameVelocity<T>::get_velocity)
+    .def("set_velocity", &FrameVelocity<T>::set_velocity, py::arg("velocity"));
+
+  frame_velocity.attr("kSize") = int{FrameVelocity<T>::kSize};
+
+  py::class_<PoseBundle<T>>(m, "PoseBundle")
+    .def(py::init<int>())
+    .def("get_num_poses", &PoseBundle<T>::get_num_poses)
+    .def("get_pose", &PoseBundle<T>::get_pose)
+    .def("set_pose", &PoseBundle<T>::set_pose)
+    .def("get_velocity", &PoseBundle<T>::get_velocity)
+    .def("set_velocity", &PoseBundle<T>::set_velocity)
+    .def("get_name", &PoseBundle<T>::get_name)
+    .def("set_name", &PoseBundle<T>::set_name)
+    .def("get_model_instance_id", &PoseBundle<T>::get_model_instance_id)
+    .def("set_model_instance_id", &PoseBundle<T>::set_model_instance_id);
+  pysystems::AddValueInstantiation<PoseBundle<T>>(m);
+
+  py::class_<PoseAggregator<T>, LeafSystem<T>>(m, "PoseAggregator")
+    .def(py::init<>())
+    .def("AddSingleInput", &PoseAggregator<T>::AddSingleInput,
+         py_reference_internal)
+    .def("AddSinglePoseAndVelocityInput",
+         &PoseAggregator<T>::AddSinglePoseAndVelocityInput)
+    .def("AddBundleInput", &PoseAggregator<T>::AddBundleInput,
+         py_reference_internal);
+
   // TODO(eric.cousineau): Add more systems as needed.
 }
 

bindings/pydrake/systems/test/rendering_test.py

@@ -1,15 +1,23 @@
 # -*- coding: utf-8 -*-
 
 from pydrake.systems.rendering import (
+    FrameVelocity,
+    PoseBundle,
+    PoseAggregator,
     PoseVector,
 )
 
 import copy
 import unittest
 import numpy as np
 
+from pydrake.multibody.multibody_tree.math import (
+    SpatialVelocity,
+)
 from pydrake.systems.framework import (
+    AbstractValue,
     BasicVector,
+    PortDataType,
 )
 from pydrake.util.eigen_geometry import (
     Isometry3,
@@ -35,7 +43,7 @@ def test_pose_vector(self):
             value.get_rotation(), Quaternion))
         self.assertTrue(isinstance(
             value.get_translation(), np.ndarray))
-        # - - Value.
+        # - Value.
         self.assertTrue(np.allclose(
             value.get_isometry().matrix(), np.eye(4, 4)))
         # - Mutators.
@@ -50,3 +58,131 @@ def test_pose_vector(self):
         vector_expected = np.hstack((p, q.wxyz()))
         vector_actual = value.get_value()
         self.assertTrue(np.allclose(vector_actual, vector_expected))
+        # - Fully-parameterized constructor.
+        value1 = PoseVector(rotation=q, translation=p)
+        self.assertTrue(np.allclose(
+            value1.get_isometry().matrix(), X_expected.matrix()))
+
+    def test_frame_velocity(self):
+        frame_velocity = FrameVelocity()
+        self.assertTrue(isinstance(frame_velocity, BasicVector))
+        self.assertTrue(isinstance(copy.copy(frame_velocity), FrameVelocity))
+        self.assertTrue(isinstance(frame_velocity.Clone(), FrameVelocity))
+        self.assertEquals(frame_velocity.size(), FrameVelocity.kSize)
+        # - Accessors.
+        self.assertTrue(isinstance(
+            frame_velocity.get_velocity(), SpatialVelocity))
+        # - Value.
+        self.assertTrue(np.allclose(
+            frame_velocity.get_velocity().rotational(), 3 * [0.]))
+        self.assertTrue(np.allclose(
+            frame_velocity.get_velocity().translational(), 3 * [0.]))
+        # - Mutators.
+        w = [0.1, 0.3, 0.5]
+        v = [0., 1., 2.]
+        frame_velocity.set_velocity(SpatialVelocity(w=w, v=v))
+        self.assertTrue(np.allclose(
+            frame_velocity.get_velocity().rotational(), w))
+        self.assertTrue(np.allclose(
+            frame_velocity.get_velocity().translational(), v))
+        # - Ensure ordering is ((wx, wy, wz), (vx, vy, vz))
+        velocity_expected = np.hstack((w, v))
+        velocity_actual = frame_velocity.get_value()
+        self.assertTrue(np.allclose(velocity_actual, velocity_expected))
+        # - Fully-parameterized constructor.
+        frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v))
+        self.assertTrue(np.allclose(
+            frame_velocity1.get_velocity().rotational(), w))
+        self.assertTrue(np.allclose(
+            frame_velocity1.get_velocity().translational(), v))
+
+    def test_pose_bundle(self):
+        num_poses = 7
+        bundle = PoseBundle(num_poses)
+        # - Accessors.
+        self.assertEqual(bundle.get_num_poses(), num_poses)
+        self.assertTrue(isinstance(bundle.get_pose(0), Isometry3))
+        self.assertTrue(isinstance(bundle.get_velocity(0), FrameVelocity))
+        # - Mutators.
+        kIndex = 5
+        p = [0, 1, 2]
+        q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
+        bundle.set_pose(kIndex, Isometry3(q, p))
+        w = [0.1, 0.3, 0.5]
+        v = [0., 1., 2.]
+        frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v))
+        bundle.set_velocity(kIndex, frame_velocity)
+        self.assertTrue((bundle.get_pose(kIndex).matrix() ==
+                         Isometry3(q, p).matrix()).all())
+        vel_actual = bundle.get_velocity(kIndex).get_velocity()
+        self.assertTrue(np.allclose(vel_actual.rotational(), w))
+        self.assertTrue(np.allclose(vel_actual.translational(), v))
+        name = "Alice"
+        bundle.set_name(kIndex, name)
+        self.assertEqual(bundle.get_name(kIndex), name)
+        instance_id = 42  # Supply a random instance id.
+        bundle.set_model_instance_id(kIndex, instance_id)
+        self.assertEqual(bundle.get_model_instance_id(kIndex), instance_id)
+
+    def test_pose_aggregator(self):
+        aggregator = PoseAggregator()
+        # - Set-up.
+        instance_id1 = 5  # Supply a random instance id.
+        port1 = aggregator.AddSingleInput("pose_only", instance_id1)
+        self.assertEqual(port1.get_data_type(), PortDataType.kVectorValued)
+        self.assertEqual(port1.size(), PoseVector.kSize)
+        instance_id2 = 42  # Supply another random, but unique, id.
+        ports2 = aggregator.AddSinglePoseAndVelocityInput(
+            "pose_and_velocity", instance_id2)
+        self.assertEqual(
+            ports2[0].get_data_type(), PortDataType.kVectorValued)
+        self.assertEqual(ports2[0].size(), PoseVector.kSize)
+        self.assertEqual(
+            ports2[1].get_data_type(), PortDataType.kVectorValued)
+        self.assertEqual(ports2[1].size(), FrameVelocity.kSize)
+        num_poses = 1
+        port3 = aggregator.AddBundleInput("pose_bundle", num_poses)
+        self.assertEqual(port3.get_data_type(), PortDataType.kAbstractValued)
+
+        # - CalcOutput.
+        context = aggregator.CreateDefaultContext()
+        output = aggregator.AllocateOutput(context)
+
+        p1 = [0, 1, 2]
+        pose1 = PoseVector()
+        pose1.set_translation(p1)
+        p2 = [5, 7, 9]
+        pose2 = PoseVector()
+        pose2.set_translation(p2)
+        w = [0.3, 0.4, 0.5]
+        v = [0.5, 0.6, 0.7]
+        velocity = FrameVelocity()
+        velocity.set_velocity(SpatialVelocity(w=w, v=v))
+        p3 = [50, 70, 90]
+        q3 = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
+        bundle = PoseBundle(num_poses)
+        bundle.set_pose(0, Isometry3(q3, p3))
+        bundle_value = AbstractValue.Make(bundle)
+
+        context.FixInputPort(0, pose1)
+        context.FixInputPort(1, pose2)
+        context.FixInputPort(2, velocity)
+        context.FixInputPort(3, bundle_value)
+
+        aggregator.CalcOutput(context, output)
+
+        value = output.get_data(0).get_value()
+        self.assertEqual(value.get_num_poses(), 3)
+        isom1_actual = Isometry3()
+        isom1_actual.set_translation(p1)
+        self.assertTrue(
+            (value.get_pose(0).matrix() == isom1_actual.matrix()).all())
+        isom2_actual = Isometry3()
+        isom2_actual.set_translation(p2)
+        self.assertTrue(
+            (value.get_pose(1).matrix() == isom2_actual.matrix()).all())
+        vel_actual = value.get_velocity(1).get_velocity()
+        self.assertTrue(np.allclose(vel_actual.rotational(), w))
+        self.assertTrue(np.allclose(vel_actual.translational(), v))
+        self.assertTrue(
+            (value.get_pose(2).matrix() == Isometry3(q3, p3).matrix()).all())