Make matlab_rpc more Python friendly (for shapes and handling integ…

…ers).
Kun-Son · Nov 21, 2017 · e6672e1 · e6672e1
1 parent 44a69a6
commit e6672e1
Show file tree

Hide file tree

Showing 5 changed files with 84 additions and 15 deletions.
diff --git a/drake/common/proto/call_matlab.cc b/drake/common/proto/call_matlab.cc
@@ -28,6 +28,7 @@ MatlabRemoteVariable::MatlabRemoteVariable()
 
 void ToMatlabArray(const MatlabRemoteVariable& var, MatlabArray* matlab_array) {
   matlab_array->set_type(MatlabArray::REMOTE_VARIABLE_REFERENCE);
+  matlab_array->set_shape_type(MatlabArray::SCALAR);
   matlab_array->set_rows(1);
   matlab_array->set_cols(1);
   int num_bytes = sizeof(int64_t);
@@ -37,26 +38,52 @@ void ToMatlabArray(const MatlabRemoteVariable& var, MatlabArray* matlab_array) {
 
 void ToMatlabArray(double var, MatlabArray* matlab_array) {
   matlab_array->set_type(MatlabArray::DOUBLE);
+  matlab_array->set_shape_type(MatlabArray::SCALAR);
   matlab_array->set_rows(1);
   matlab_array->set_cols(1);
   int num_bytes = sizeof(double);
   matlab_array->set_data(&var, num_bytes);
 }
 
-void ToMatlabArray(const Eigen::Ref<const Eigen::MatrixXd>& mat,
-                   MatlabArray* matlab_array) {
+void internal::ToMatlabArrayMatrix(
+    const Eigen::Ref<const Eigen::MatrixXd>& mat,
+    MatlabArray* matlab_array, bool is_vector) {
   matlab_array->set_type(MatlabArray::DOUBLE);
+  matlab_array->set_shape_type(
+      is_vector ? MatlabArray::VECTOR : MatlabArray::MATRIX);
   matlab_array->set_rows(mat.rows());
   matlab_array->set_cols(mat.cols());
   int num_bytes = sizeof(double) * mat.rows() * mat.cols();
   matlab_array->set_data(mat.data(), num_bytes);
 }
 
-void ToMatlabArray(
-    const Eigen::Ref<const Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>>&
-        mat,
-    MatlabArray* matlab_array) {
+void ToMatlabArray(int var, MatlabArray* matlab_array) {
+  matlab_array->set_type(MatlabArray::INT);
+  matlab_array->set_shape_type(MatlabArray::SCALAR);
+  matlab_array->set_rows(1);
+  matlab_array->set_cols(1);
+  int num_bytes = sizeof(int);
+  matlab_array->set_data(&var, num_bytes);
+}
+
+void internal::ToMatlabArrayMatrix(
+    const Eigen::Ref<const Eigen::MatrixXi>& mat,
+    MatlabArray* matlab_array, bool is_vector) {
+  matlab_array->set_type(MatlabArray::INT);
+  matlab_array->set_shape_type(
+      is_vector ? MatlabArray::VECTOR : MatlabArray::MATRIX);
+  matlab_array->set_rows(mat.rows());
+  matlab_array->set_cols(mat.cols());
+  int num_bytes = sizeof(int) * mat.rows() * mat.cols();
+  matlab_array->set_data(mat.data(), num_bytes);
+}
+
+void internal::ToMatlabArrayMatrix(
+    const Eigen::Ref<const MatrixX<bool>>& mat,
+    MatlabArray* matlab_array, bool is_vector) {
   matlab_array->set_type(MatlabArray::LOGICAL);
+  matlab_array->set_shape_type(
+      is_vector ? MatlabArray::VECTOR : MatlabArray::MATRIX);
   matlab_array->set_rows(mat.rows());
   matlab_array->set_cols(mat.cols());
   int num_bytes = sizeof(bool) * mat.rows() * mat.cols();
@@ -65,6 +92,7 @@ void ToMatlabArray(
 
 void ToMatlabArray(const std::string& str, MatlabArray* matlab_array) {
   matlab_array->set_type(MatlabArray::CHAR);
+  matlab_array->set_shape_type(MatlabArray::VECTOR);
   matlab_array->set_rows(1);
   matlab_array->set_cols(str.length());
   int num_bytes = sizeof(char) * str.length();

diff --git a/drake/common/proto/call_matlab.h b/drake/common/proto/call_matlab.h
@@ -10,7 +10,8 @@
 #include "drake/common/eigen_types.h"
 #include "drake/common/proto/matlab_rpc.pb.h"
 
-/// @file Utilities for calling Matlab from C++
+/// @file
+/// @brief Utilities for calling Matlab from C++
 ///
 /// Provides a simple interface for (one-directional) RPC to a simple matlab
 /// remote client.  Methods are provided to serialize our favorite data types
@@ -35,7 +36,6 @@
 ///
 /// See call_matlab_test.cc for some simple examples.
 
-
 namespace drake {
 namespace common {
 
@@ -44,20 +44,37 @@ namespace common {
 ///  another one of these methods.
 
 class MatlabRemoteVariable;
-void ToMatlabArray(const MatlabRemoteVariable& var, MatlabArray* matlab_array);
 
-void ToMatlabArray(double scalar, MatlabArray* matlab_array);
+namespace internal {
 
-void ToMatlabArray(
+void ToMatlabArrayMatrix(
     const Eigen::Ref<const Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>>&
         mat,
-    MatlabArray* matlab_array);
+    MatlabArray* matlab_array, bool is_vector);
+
+void ToMatlabArrayMatrix(const Eigen::Ref<const Eigen::MatrixXd>& mat,
+                         MatlabArray* matlab_array, bool is_vector);
+
+void ToMatlabArrayMatrix(const Eigen::Ref<const Eigen::MatrixXi>& mat,
+                         MatlabArray* matlab_array, bool is_vector);
+
+}  // namespace internal
+
+void ToMatlabArray(const MatlabRemoteVariable& var, MatlabArray* matlab_array);
 
-void ToMatlabArray(const Eigen::Ref<const Eigen::MatrixXd>& mat,
-                   MatlabArray* matlab_array);
+void ToMatlabArray(double scalar, MatlabArray* matlab_array);
+
+void ToMatlabArray(int scalar, MatlabArray* matlab_array);
 
 void ToMatlabArray(const std::string& str, MatlabArray* matlab_array);
 
+template <typename Derived>
+void ToMatlabArray(const Eigen::MatrixBase<Derived>& mat,
+                   MatlabArray* matlab_array) {
+  const bool is_vector = (Derived::ColsAtCompileTime == 1);
+  return internal::ToMatlabArrayMatrix(mat, matlab_array, is_vector);
+}
+
 // Helper methods for variadic template call in CallMatlab.
 namespace internal {
 inline void AssembleCallMatlabMsg(MatlabRPC*) {

diff --git a/drake/common/proto/matlab_rpc.proto b/drake/common/proto/matlab_rpc.proto
@@ -14,12 +14,21 @@ message MatlabArray
     DOUBLE = 1;
     CHAR = 2;
     LOGICAL = 3;
+    INT = 4;
   }
   optional ArrayType type = 1;
 
+  enum ShapeType {
+    MATRIX = 0;
+    VECTOR = 1;
+    SCALAR = 2;
+  }
+
   // Dimensions.
   optional int32 rows = 2;
   optional int32 cols = 3;
+  // Information.
+  optional ShapeType shape_type = 5;
 
   optional bytes data = 4;
 
@@ -46,5 +55,6 @@ message MatlabRPC {
 
   // any input allowed by matlab's mexcallmatlab command
   // (http://www.mathworks.com/help/matlab/apiref/mexcallmatlab.html)
+  // or any expression that resolves to a callable Python object (for Python).
   optional string function_name = 3;
 }
diff --git a/drake/common/proto/test/call_matlab_test.cc b/drake/common/proto/test/call_matlab_test.cc
@@ -45,7 +45,7 @@ GTEST_TEST(TestCallMatlab, RemoteVarTest) {
 }
 
 GTEST_TEST(TestCallMatlab, SimplePlot) {
-  int N = 100;
+  const int N = 100;
 
   Eigen::VectorXd time(N), val(N);
   for (int i = 0; i < N; i++) {

diff --git a/matlab/call_matlab_client.cc b/matlab/call_matlab_client.cc
@@ -110,6 +110,20 @@ void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
           memcpy(mxGetPr(rhs[i]), message.rhs(i).data().data(), num_bytes);
           break;
         }
+        case drake::common::MatlabArray::INT: {
+          rhs[i] = mxCreateDoubleMatrix(message.rhs(i).rows(),
+                                        message.rhs(i).cols(), mxREAL);
+          // MATLAB is pretty picky about "figure()" and other HG arguments.
+          // Just convert the integers to doubles and call it done.
+          const int size = message.rhs(i).rows() * message.rhs(i).cols();
+          DRAKE_DEMAND(static_cast<int>(sizeof(int)) * size == num_bytes);
+          auto array_in =
+              reinterpret_cast<const int*>(message.rhs(i).data().data());
+          double* array_out = mxGetPr(rhs[i]);
+          for (int j = 0; j < size; j++)
+            array_out[j] = static_cast<double>(array_in[j]);
+          break;
+        }
         case drake::common::MatlabArray::CHAR: {
           mwSize dims[2];
           dims[0] = message.rhs(i).rows();