GTDKeyFormatter support

gtdynamics.i

@@ -2,6 +2,8 @@
 
 virtual class gtsam::NonlinearFactor;
 virtual class gtsam::NoiseModelFactor;
+virtual class gtsam::NonlinearFactorGraph;
+virtual class gtsam::Values;
 
 namespace gtdynamics {
 
@@ -521,6 +523,12 @@ gtdynamics::DynamicsSymbol ContactWrenchKey(int i, int k, int t);
 gtdynamics::DynamicsSymbol PhaseKey(int k);
 gtdynamics::DynamicsSymbol TimeKey(int t);
 
+#include <gtdynamics/utils/DynamicsSymbol-wrap.h>
+gtdynamics::StdKeyFormatter KeyFormatter();
+string str(const gtsam::Values &t);
+string str(const gtsam::Values &t, const string &s);
+string str(const gtsam::NonlinearFactorGraph &t);
+string str(const gtsam::NonlinearFactorGraph &t, const string &s);
 
 ///////////////////// Key Methods /////////////////////
 template<T = {double}>

gtdynamics/utils/DynamicsSymbol-wrap.h

@@ -0,0 +1,38 @@
+/* ----------------------------------------------------------------------------
+ * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file  DynamicsSymbol-wrap.h
+ * @brief This file provides convenience stuff for wrapping.  It wraps some
+ * gtsam objects so that printing will use DynamicsSymbol key formatting.
+ * @author Gerry Chen
+ */
+
+#pragma once
+
+#include <gtdynamics/utils/DynamicsSymbol.h>
+
+#include <gtsam/nonlinear/utilities.h>  // for RedirectCout.
+
+namespace gtdynamics {
+
+typedef std::function<std::string(gtsam::Key)> StdKeyFormatter;
+
+/// Adapts the GTDKeyFormatter from a boost::function to a std::function
+StdKeyFormatter KeyFormatter() {
+  return [](gtsam::Key key) { return GTDKeyFormatter(key); };
+}
+
+
+template <typename T>
+std::string str(const T& t, const std::string& s = "") {
+  gtsam::RedirectCout redirect;
+  t.print(s, GTDKeyFormatter);
+  return redirect.str();
+}
+
+}  // namespace gtdynamics

python/templates/__init__.py.in

@@ -1,2 +1,11 @@
 import gtsam
 from ${PROJECT_NAME}.${PROJECT_NAME} import *
+
+class GtdKeyFormatter(object):
+    def __repr__(self):
+        return str(self)
+
+class Values(GtdKeyFormatter, gtsam.Values):
+    pass
+class NonlinearFactorGraph(GtdKeyFormatter, gtsam.NonlinearFactorGraph):
+    pass

python/templates/gtdynamics.tpl

@@ -7,6 +7,8 @@
 #include <pybind11/stl_bind.h>
 #include <pybind11/pybind11.h>
 #include <pybind11/operators.h>
+#include <pybind11/functional.h>
+#include <pybind11/iostream.h>
 
 #include <boost/optional.hpp>
 

python/tests/test_print.py

@@ -0,0 +1,50 @@
+"""
+ * GTDynamics Copyright 2021, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * See LICENSE for the license information
+ *
+ * @file  test_print.py
+ * @brief Test printing with DynamicsSymbol.
+ * @author Gerry Chen
+"""
+
+from io import StringIO
+import unittest
+from unittest.mock import patch
+
+import gtdynamics as gtd
+import gtsam
+
+class Print(unittest.TestCase):
+    def test_values(self):
+        """Checks that printing Values uses the GTDKeyFormatter instead of gtsam's default"""
+        v = gtd.Values()
+        gtd.InsertJointAngleDouble(v, 0, 1, 2)
+        self.assertTrue('q(0)1' in v.__repr__())
+
+    def test_nonlinear_factor_graph(self):
+        """Checks that printing NonlinearFactorGraph uses the GTDKeyFormatter"""
+        fg = gtd.NonlinearFactorGraph()
+        fg.push_back(gtd.MinTorqueFactor(gtd.internal.TorqueKey(0, 0).key(),
+                                         gtsam.noiseModel.Unit.Create(1)))
+        self.assertTrue('T(0)0' in fg.__repr__())
+
+    def test_key_formatter(self):
+        """Tests print_ method with various key formatters"""
+        torqueKey = gtd.internal.TorqueKey(0, 0).key()
+        factor = gtd.MinTorqueFactor(torqueKey, gtsam.noiseModel.Unit.Create(1))
+        with patch('sys.stdout', new = StringIO()) as fake_out:
+            factor.print_('factor: ', gtd.GetKeyFormatter())
+            self.assertTrue('factor: min torque factor' in fake_out.getvalue())
+            self.assertTrue('keys = { T(0)0 }' in fake_out.getvalue())
+        def myKeyFormatter(key):
+            return 'this is my key formatter {}'.format(key)
+        with patch('sys.stdout', new = StringIO()) as fake_out:
+            factor.print_('factor: ', myKeyFormatter)
+            self.assertTrue('factor: min torque factor' in fake_out.getvalue())
+            self.assertTrue(
+                'keys = {{ this is my key formatter {} }}'.format(torqueKey) in fake_out.getvalue())
+
+if __name__ == "__main__":
+    unittest.main()