diff --git a/drake/common/proto/call_matlab.cc b/drake/common/proto/call_matlab.cc index d1687f5eaa93..e8f9cf1d1fb5 100644 --- 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& mat, - MatlabArray* matlab_array) { +void internal::ToMatlabArrayMatrix( + const Eigen::Ref& 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>& - 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& 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>& 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 index 763c043133ea..447eae2550c3 100644 --- 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>& mat, - MatlabArray* matlab_array); + MatlabArray* matlab_array, bool is_vector); + +void ToMatlabArrayMatrix(const Eigen::Ref& mat, + MatlabArray* matlab_array, bool is_vector); + +void ToMatlabArrayMatrix(const Eigen::Ref& mat, + MatlabArray* matlab_array, bool is_vector); + +} // namespace internal + +void ToMatlabArray(const MatlabRemoteVariable& var, MatlabArray* matlab_array); -void ToMatlabArray(const Eigen::Ref& 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 +void ToMatlabArray(const Eigen::MatrixBase& 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 index 1a2d1fb6bb89..14b14648d55a 100644 --- 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 index 9d4e78af6670..319d505993f2 100644 --- 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 index 1bc872a0a974..468f394dc99e 100644 --- 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(sizeof(int)) * size == num_bytes); + auto array_in = + reinterpret_cast(message.rhs(i).data().data()); + double* array_out = mxGetPr(rhs[i]); + for (int j = 0; j < size; j++) + array_out[j] = static_cast(array_in[j]); + break; + } case drake::common::MatlabArray::CHAR: { mwSize dims[2]; dims[0] = message.rhs(i).rows();