Moves ZmpPlanner to planning directory and adds python bindings. (Rob…

…otLocomotion#21305)

Co-authored-by: Jeremy Nimmer <[email protected]>
Co-authored-by: Rick Poyner <[email protected]>

Co-Authored-By: Jeremy Nimmer <[email protected]>
Co-Authored-By: Rick Poyner <[email protected]>

bindings/pydrake/planning/BUILD.bazel

@@ -41,6 +41,7 @@ drake_pybind_library(
         "planning_py_trajectory_optimization.cc",
         "planning_py_visibility_graph.cc",
         "planning_py_iris_from_clique_cover.cc",
+        "planning_py_zmp_planner.cc",
     ],
     package_info = PACKAGE_INFO,
     py_deps = [
@@ -129,4 +130,12 @@ drake_py_unittest(
     ],
 )
 
+drake_py_unittest(
+    name = "zmp_planner_test",
+    deps = [
+        ":planning",
+        "//bindings/pydrake/common/test_utilities:numpy_compare_py",
+    ],
+)
+
 add_lint_tests_pydrake()

bindings/pydrake/planning/planning_py.cc

@@ -28,6 +28,7 @@ and/or trajectories of dynamical systems.
   internal::DefinePlanningTrajectoryOptimization(m);
   internal::DefinePlanningVisibilityGraph(m);
   internal::DefinePlanningIrisFromCliqueCover(m);
+  internal::DefinePlanningZmpPlanner(m);
 }
 
 }  // namespace pydrake

bindings/pydrake/planning/planning_py.h

@@ -35,6 +35,9 @@ void DefinePlanningTrajectoryOptimization(py::module m);
 /* Defines bindings per planning_py_visibility_graph.cc. */
 void DefinePlanningVisibilityGraph(py::module m);
 
+/* Defines bindings per planning_py_zmp_planner.cc. */
+void DefinePlanningZmpPlanner(py::module m);
+
 }  // namespace internal
 }  // namespace pydrake
 }  // namespace drake

bindings/pydrake/planning/planning_py_zmp_planner.cc

@@ -0,0 +1,88 @@
+#include "drake/bindings/pydrake/documentation_pybind.h"
+#include "drake/bindings/pydrake/pydrake_pybind.h"
+#include "drake/planning/locomotion/zmp_planner.h"
+
+namespace drake {
+namespace pydrake {
+namespace internal {
+
+void DefinePlanningZmpPlanner(py::module m) {
+  // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace.
+  using namespace drake::planning;
+  constexpr auto& doc = pydrake_doc.drake.planning;
+
+  {
+    using Class = ZmpPlanner;
+    constexpr auto& cls_doc = doc.ZmpPlanner;
+    auto cls = py::class_<Class>(m, "ZmpPlanner", cls_doc.doc)
+                   .def(py::init<>(), cls_doc.ctor.doc);
+    cls.def("Plan", &Class::Plan, py::arg("zmp_d"), py::arg("x0"),
+           py::arg("height"), py::arg("gravity") = 9.81,
+           py::arg("Qy") = Eigen::Matrix2d::Identity(),
+           py::arg("R") = 0.1 * Eigen::Matrix2d::Identity(), cls_doc.ctor.doc)
+        .def("has_planned", &Class::has_planned, cls_doc.has_planned.doc)
+        .def("ComputeOptimalCoMdd", &Class::ComputeOptimalCoMdd,
+            py::arg("time"), py::arg("x"), cls_doc.ComputeOptimalCoMdd.doc)
+        .def("comdd_to_cop", &Class::comdd_to_cop, py::arg("x"), py::arg("u"),
+            cls_doc.comdd_to_cop.doc)
+        .def("get_A", &Class::get_A, cls_doc.get_A.doc)
+        .def("get_B", &Class::get_B, cls_doc.get_B.doc)
+        .def("get_C", &Class::get_C, cls_doc.get_C.doc)
+        .def("get_D", &Class::get_D, cls_doc.get_D.doc)
+        .def("get_Qy", &Class::get_Qy, cls_doc.get_Qy.doc)
+        .def("get_R", &Class::get_R, cls_doc.get_R.doc)
+        .def("get_desired_zmp",
+            overload_cast_explicit<Eigen::Vector2d, double>(
+                &Class::get_desired_zmp),
+            py::arg("time"), cls_doc.get_desired_zmp.doc_at_time)
+        .def("get_nominal_com",
+            overload_cast_explicit<Eigen::Vector2d, double>(
+                &Class::get_nominal_com),
+            py::arg("time"), cls_doc.get_nominal_com.doc_at_time)
+        .def("get_nominal_comd",
+            overload_cast_explicit<Eigen::Vector2d, double>(
+                &Class::get_nominal_comd),
+            py::arg("time"), cls_doc.get_nominal_comd.doc_at_time)
+        .def("get_nominal_comdd",
+            overload_cast_explicit<Eigen::Vector2d, double>(
+                &Class::get_nominal_comdd),
+            py::arg("time"), cls_doc.get_nominal_comdd.doc_at_time)
+        .def("get_final_desired_zmp", &Class::get_final_desired_zmp,
+            cls_doc.get_final_desired_zmp.doc)
+        .def("get_desired_zmp",
+            overload_cast_explicit<
+                const trajectories::PiecewisePolynomial<double>&>(
+                &Class::get_desired_zmp),
+            py_rvp::copy, cls_doc.get_desired_zmp.doc)
+        .def("get_nominal_com",
+            overload_cast_explicit<const trajectories::
+                    ExponentialPlusPiecewisePolynomial<double>&>(
+                &Class::get_nominal_com),
+            py_rvp::copy, cls_doc.get_nominal_com.doc)
+        .def("get_nominal_comd",
+            overload_cast_explicit<const trajectories::
+                    ExponentialPlusPiecewisePolynomial<double>&>(
+                &Class::get_nominal_comd),
+            py_rvp::copy, cls_doc.get_nominal_comd.doc)
+        .def("get_nominal_comdd",
+            overload_cast_explicit<const trajectories::
+                    ExponentialPlusPiecewisePolynomial<double>&>(
+                &Class::get_nominal_comdd),
+            py_rvp::copy, cls_doc.get_nominal_comdd.doc)
+        .def("get_Vxx", &Class::get_Vxx, cls_doc.get_Vxx.doc)
+        .def("get_Vx",
+            overload_cast_explicit<const Eigen::Vector4d, double>(
+                &Class::get_Vx),
+            py::arg("time"), cls_doc.get_Vx.doc_at_time)
+        .def("get_Vx",
+            overload_cast_explicit<const trajectories::
+                    ExponentialPlusPiecewisePolynomial<double>&>(
+                &Class::get_Vx),
+            py_rvp::copy, cls_doc.get_Vx.doc);
+    DefCopyAndDeepCopy(&cls);
+  }
+}
+
+}  // namespace internal
+}  // namespace pydrake
+}  // namespace drake

bindings/pydrake/planning/test/zmp_planner_test.py

@@ -0,0 +1,52 @@
+import unittest
+
+import numpy as np
+
+from pydrake.planning import (
+    ZmpPlanner,
+)
+from pydrake.trajectories import PiecewisePolynomial
+
+
+class TestZmpPlanner(unittest.TestCase):
+    def test_zmp_planner(self):
+        height = 1
+        x0 = [0, 0, 0, 0]
+        time = [0, 1, 2, 3]
+        zmp_knots = np.array([[0, 0], [0.2, -0.1], [0.4, 0.1], [0.4, 0.1]]).T
+        zmp_d = PiecewisePolynomial.FirstOrderHold(time, zmp_knots)
+        planner = ZmpPlanner()
+        planner.Plan(
+            zmp_d=zmp_d,
+            x0=x0,
+            height=height,
+            gravity=9.8,
+            Qy=np.eye(2),
+            R=0.2*np.eye(2))
+        self.assertTrue(planner.has_planned())
+        self.assertEqual(planner.get_A().shape, (4, 4))
+        self.assertEqual(planner.get_B().shape, (4, 2))
+        self.assertEqual(planner.get_C().shape, (2, 4))
+        self.assertEqual(planner.get_D().shape, (2, 2))
+        np.testing.assert_almost_equal(planner.get_Qy(), np.eye(2))
+        np.testing.assert_almost_equal(planner.get_R(), 0.2*np.eye(2))
+        sample_time = 0.1
+        np.testing.assert_almost_equal(
+            planner.get_desired_zmp(time=sample_time).reshape((2, 1)),
+            zmp_d.value(sample_time))
+        np.testing.assert_almost_equal(
+            planner.get_desired_zmp(time=sample_time).reshape((2, 1)),
+            planner.get_desired_zmp().value(sample_time))
+        np.testing.assert_almost_equal(
+            planner.get_nominal_com(time=sample_time).reshape((2, 1)),
+            planner.get_nominal_com().value(sample_time))
+        np.testing.assert_almost_equal(
+            planner.get_nominal_comd(time=sample_time).reshape((2, 1)),
+            planner.get_nominal_comd().value(sample_time))
+        np.testing.assert_almost_equal(
+            planner.get_nominal_comdd(time=sample_time).reshape((2, 1)),
+            planner.get_nominal_comdd().value(sample_time))
+        self.assertEqual(planner.get_Vxx().shape, (4, 4))
+        np.testing.assert_almost_equal(
+            planner.get_Vx(time=sample_time).reshape((4, 1)),
+            planner.get_Vx().value(sample_time))

examples/zmp/BUILD.bazel

@@ -10,7 +10,7 @@ drake_cc_binary(
     srcs = ["zmp_example.cc"],
     deps = [
         "//common/proto:call_python",
-        "//systems/controllers:zmp_test_util",
+        "//planning/locomotion/test_utilities",
     ],
 )
 

examples/zmp/zmp_example.cc

@@ -2,14 +2,14 @@
 #include <thread>
 
 #include "drake/common/proto/call_python.h"
-#include "drake/systems/controllers/test/zmp_test_util.h"
+#include "drake/planning/locomotion/test_utilities/zmp_test_util.h"
 
 namespace drake {
 namespace examples {
 namespace zmp {
 namespace {
 
-void PlotResults(const systems::controllers::ZmpTestTraj& traj) {
+void PlotResults(const planning::ZmpTestTraj& traj) {
   using common::CallPython;
   using common::ToPythonTuple;
 
@@ -88,20 +88,20 @@ void do_main() {
       Eigen::Vector2d(2, -0.1), Eigen::Vector2d(2.5, 0)};
 
   std::vector<trajectories::PiecewisePolynomial<double>> zmp_trajs =
-      systems::controllers::GenerateDesiredZmpTrajs(footsteps, 0.5, 1);
+      planning::GenerateDesiredZmpTrajs(footsteps, 0.5, 1);
 
   Eigen::Vector4d x0(0, 0, 0, 0);
   double z = 1;
 
-  systems::controllers::ZmpPlanner zmp_planner;
+  planning::ZmpPlanner zmp_planner;
   zmp_planner.Plan(zmp_trajs[0], x0, z);
 
   double sample_dt = 0.01;
 
   // Perturb the initial state a bit.
   x0 << 0, 0, 0.2, -0.1;
-  systems::controllers::ZmpTestTraj result =
-      systems::controllers::SimulateZmpPolicy(zmp_planner, x0, sample_dt, 2);
+  planning::ZmpTestTraj result =
+      planning::SimulateZmpPolicy(zmp_planner, x0, sample_dt, 2);
 
   PlotResults(result);
 }

planning/BUILD.bazel

@@ -213,6 +213,8 @@ drake_cc_library(
     ],
 )
 
+# === test/ ===
+
 drake_cc_googletest(
     name = "body_shape_description_test",
     deps = [

planning/locomotion/BUILD.bazel

@@ -0,0 +1,40 @@
+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 = "locomotion",
+    visibility = ["//visibility:public"],
+    deps = [
+        ":zmp_planner",
+    ],
+)
+
+drake_cc_library(
+    name = "zmp_planner",
+    srcs = ["zmp_planner.cc"],
+    hdrs = ["zmp_planner.h"],
+    deps = [
+        "//common:essential",
+        "//common/trajectories:piecewise_polynomial",
+        "//systems/controllers:linear_quadratic_regulator",
+    ],
+)
+
+# === test/ ===
+
+drake_cc_googletest(
+    name = "zmp_planner_test",
+    deps = [
+        "//common/test_utilities:eigen_matrix_compare",
+        "//planning/locomotion/test_utilities",
+    ],
+)
+
+add_lint_tests()

systems/controllers/test/zmp_planner_test.cc → planning/locomotion/test/zmp_planner_test.cc

@@ -1,15 +1,14 @@
-#include "drake/systems/controllers/zmp_planner.h"
+#include "drake/planning/locomotion/zmp_planner.h"
 
 #include <typeinfo>
 
 #include <gtest/gtest.h>
 
 #include "drake/common/test_utilities/eigen_matrix_compare.h"
-#include "drake/systems/controllers/test/zmp_test_util.h"
+#include "drake/planning/locomotion/test_utilities/zmp_test_util.h"
 
 namespace drake {
-namespace systems {
-namespace controllers {
+namespace planning {
 
 using trajectories::ExponentialPlusPiecewisePolynomial;
 using trajectories::PiecewisePolynomial;
@@ -256,6 +255,5 @@ TEST_F(ZmpPlannerTest, TestOptimalControl) {
 }
 
 }  // namespace
-}  // namespace controllers
-}  // namespace systems
+}  // namespace planning
 }  // namespace drake

planning/locomotion/test_utilities/BUILD.bazel

@@ -0,0 +1,29 @@
+load("//tools/lint:lint.bzl", "add_lint_tests")
+load(
+    "//tools/skylark:drake_cc.bzl",
+    "drake_cc_library",
+    "drake_cc_package_library",
+)
+
+package(default_visibility = ["//visibility:public"])
+
+drake_cc_package_library(
+    name = "test_utilities",
+    testonly = 1,
+    visibility = ["//visibility:public"],
+    deps = [
+        ":zmp_test_util",
+    ],
+)
+
+drake_cc_library(
+    name = "zmp_test_util",
+    testonly = 1,
+    srcs = ["zmp_test_util.cc"],
+    hdrs = ["zmp_test_util.h"],
+    deps = [
+        "//planning/locomotion:zmp_planner",
+    ],
+)
+
+add_lint_tests()

systems/controllers/test/zmp_test_util.cc → planning/locomotion/test_utilities/zmp_test_util.cc

@@ -1,10 +1,9 @@
-#include "drake/systems/controllers/test/zmp_test_util.h"
+#include "drake/planning/locomotion/test_utilities/zmp_test_util.h"
 
 #include "drake/common/drake_assert.h"
 
 namespace drake {
-namespace systems {
-namespace controllers {
+namespace planning {
 
 using trajectories::PiecewisePolynomial;
 
@@ -13,8 +12,7 @@ ZmpTestTraj SimulateZmpPolicy(const ZmpPlanner& zmp_planner,
                               double extra_time_at_the_end) {
   const PiecewisePolynomial<double>& zmp_d = zmp_planner.get_desired_zmp();
   int N = static_cast<int>(
-      (zmp_d.end_time() + extra_time_at_the_end - zmp_d.start_time())
-      / dt);
+      (zmp_d.end_time() + extra_time_at_the_end - zmp_d.start_time()) / dt);
 
   ZmpTestTraj traj(N);
 
@@ -52,7 +50,6 @@ std::vector<PiecewisePolynomial<double>> GenerateDesiredZmpTrajs(
   std::vector<double> time_steps;
   std::vector<Eigen::MatrixXd> zmp_d;
 
-  Eigen::Vector4d x0(0, 0, 0, 0);
   double time = 0;
 
   time_steps.push_back(time);
@@ -89,12 +86,10 @@ const std::function<ZmpTestTraj(const ZmpPlanner&, const Eigen::Vector4d&,
                                 double, double)>
     SimulateZMPPolicy = SimulateZmpPolicy;
 
-const std::function<
-    std::vector<trajectories::PiecewisePolynomial<double>>(
-        const std::vector<Eigen::Vector2d>&, double, double)>
+const std::function<std::vector<trajectories::PiecewisePolynomial<double>>(
+    const std::vector<Eigen::Vector2d>&, double, double)>
     GenerateDesiredZMPTrajs = GenerateDesiredZmpTrajs;
 #pragma GCC diagnostic pop
 
-}  // namespace controllers
-}  // namespace systems
+}  // namespace planning
 }  // namespace drake