diff --git a/drake/examples/Acrobot/@AcrobotPlantCpp/manipulatorDynamics.cpp b/drake/examples/Acrobot/@AcrobotPlantCpp/manipulatorDynamics.cpp index 6660c6fee360..ef60b75413be 100644 --- a/drake/examples/Acrobot/@AcrobotPlantCpp/manipulatorDynamics.cpp +++ b/drake/examples/Acrobot/@AcrobotPlantCpp/manipulatorDynamics.cpp @@ -3,7 +3,6 @@ #include #include #include "drakeMexUtil.h" -#include #include using namespace Eigen; @@ -50,89 +49,6 @@ void manipulatorDynamics(const mxArray* pobj, const MatrixBase &q, con B << 0.0, 1.0; } -template -Eigen::Matrix autoDiffToValueMatrix(const Eigen::MatrixBase& auto_diff_matrix) { - Eigen::Matrix ret(auto_diff_matrix.rows(), auto_diff_matrix.cols()); - for (int i = 0; i < auto_diff_matrix.rows(); i++) { - for (int j = 0; j < auto_diff_matrix.cols(); ++j) { - ret(i, j) = auto_diff_matrix(i, j).value(); - } - } - return ret; -}; - -template -typename Gradient, Eigen::Dynamic>::type autoDiffToGradientMatrix( - const Eigen::MatrixBase& auto_diff_matrix, int num_variables = Eigen::Dynamic) -{ - int num_variables_from_matrix = 0; - for (int i = 0; i < auto_diff_matrix.size(); ++i) { - num_variables_from_matrix = std::max(num_variables_from_matrix, static_cast(auto_diff_matrix(i).derivatives().size())); - } - if (num_variables == Eigen::Dynamic) { - num_variables = num_variables_from_matrix; - } - else if (num_variables_from_matrix != 0 && num_variables_from_matrix != num_variables) { - std::stringstream buf; - buf << "Input matrix has derivatives w.r.t " << num_variables_from_matrix << ", variables" << ", whereas num_variables is " << num_variables << ".\n"; - buf << "Either num_variables_from_matrix should be zero, or it should match num_variables"; - throw std::runtime_error(buf.str()); - } - - typename Gradient, Eigen::Dynamic>::type gradient(auto_diff_matrix.size(), num_variables); - for (int row = 0; row < auto_diff_matrix.rows(); row++) { - for (int col = 0; col < auto_diff_matrix.cols(); col++) { - auto gradient_row = gradient.row(row + col * auto_diff_matrix.rows()).transpose(); - if (auto_diff_matrix(row, col).derivatives().size() == 0) { - gradient_row.setZero(); - } else { - gradient_row = auto_diff_matrix(row, col).derivatives(); - } - } - } - return gradient; -} - -template -void gradientMatrixToAutoDiff(const Eigen::MatrixBase& gradient, Eigen::MatrixBase& auto_diff_matrix) -{ - int nq = gradient.cols(); - for (size_t row = 0; row < auto_diff_matrix.rows(); row++) { - for (size_t col = 0; col < auto_diff_matrix.cols(); col++) { - auto_diff_matrix(row, col).derivatives().resize(nq, 1); - auto_diff_matrix(row, col).derivatives() = gradient.row(row + col * auto_diff_matrix.rows()).transpose(); - } - } -} - -template -Eigen::Matrix, Rows, Cols> taylorVarToEigen(const mxArray* taylor_var) { - auto f = mxGetPropertySafe(taylor_var, "f"); - auto df = mxGetPropertySafe(taylor_var, "df"); - if (mxGetNumberOfElements(df) > 1) - throw runtime_error("TaylorVars of order higher than 1 currently not supported"); - auto ret = matlabToEigenMap(f).template cast>().eval(); - typedef Gradient GradientType; - auto gradient_matrix = matlabToEigenMap(mxGetCell(df, 0)); - gradientMatrixToAutoDiff(gradient_matrix, ret); - - return ret; -} - -template -mxArray* eigenToTaylorVar(const MatrixBase& m, int num_variables = Eigen::Dynamic) -{ - const int nrhs = 2; - mxArray *prhs[nrhs]; - prhs[0] = eigenToMatlab(autoDiffToValueMatrix(m)); - mwSize dims[] = {1}; - prhs[1] = mxCreateCellArray(1, dims); - mxArray *plhs[1]; - mxSetCell(prhs[1], 0, eigenToMatlab(autoDiffToGradientMatrix(m, num_variables))); - mexCallMATLABsafe(1, plhs, nrhs, prhs, "TaylorVar"); - return plhs[0]; -} - void mexFunction( int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[] ) { const mxArray* pobj = prhs[0]; diff --git a/drake/util/CMakeLists.txt b/drake/util/CMakeLists.txt index 889eda586339..35266a5c022a 100644 --- a/drake/util/CMakeLists.txt +++ b/drake/util/CMakeLists.txt @@ -33,7 +33,7 @@ pods_install_pkg_config_file(drake-util if (MATLAB_FOUND) add_mex(drakeMexUtil SHARED drakeMexUtil.cpp MexWrapper.cpp) - target_link_libraries(drakeMexUtil drakePolynomial) + target_link_libraries(drakeMexUtil drakePolynomial drakeGradientUtil) # todo: use this again once I can assume everyone has CMAKE version >= 2.8.8 #add_mex(drakeUtil OBJECT drakeUtil.cpp) diff --git a/drake/util/drakeGradientUtil.h b/drake/util/drakeGradientUtil.h index 6a92c0bd20d2..56fa3d0f5c1d 100644 --- a/drake/util/drakeGradientUtil.h +++ b/drake/util/drakeGradientUtil.h @@ -128,4 +128,59 @@ template DLLEXPORT void setSubMatrixGradient(Eigen::MatrixBase& dM, const Eigen::MatrixBase& dM_submatrix, int row, int col, typename DerivedDM::Index M_rows, typename DerivedDM::Index q_start = 0, typename DerivedDM::Index q_subvector_size = QSubvectorSize); +template +Eigen::Matrix autoDiffToValueMatrix(const Eigen::MatrixBase& auto_diff_matrix) { + Eigen::Matrix ret(auto_diff_matrix.rows(), auto_diff_matrix.cols()); + for (int i = 0; i < auto_diff_matrix.rows(); i++) { + for (int j = 0; j < auto_diff_matrix.cols(); ++j) { + ret(i, j) = auto_diff_matrix(i, j).value(); + } + } + return ret; +}; + +template +typename Gradient, Eigen::Dynamic>::type autoDiffToGradientMatrix( + const Eigen::MatrixBase& auto_diff_matrix, int num_variables = Eigen::Dynamic) +{ + int num_variables_from_matrix = 0; + for (int i = 0; i < auto_diff_matrix.size(); ++i) { + num_variables_from_matrix = std::max(num_variables_from_matrix, static_cast(auto_diff_matrix(i).derivatives().size())); + } + if (num_variables == Eigen::Dynamic) { + num_variables = num_variables_from_matrix; + } + else if (num_variables_from_matrix != 0 && num_variables_from_matrix != num_variables) { + std::stringstream buf; + buf << "Input matrix has derivatives w.r.t " << num_variables_from_matrix << " variables, whereas num_variables is " << num_variables << ".\n"; + buf << "Either num_variables_from_matrix should be zero, or it should match num_variables."; + throw std::runtime_error(buf.str()); + } + + typename Gradient, Eigen::Dynamic>::type gradient(auto_diff_matrix.size(), num_variables); + for (int row = 0; row < auto_diff_matrix.rows(); row++) { + for (int col = 0; col < auto_diff_matrix.cols(); col++) { + auto gradient_row = gradient.row(row + col * auto_diff_matrix.rows()).transpose(); + if (auto_diff_matrix(row, col).derivatives().size() == 0) { + gradient_row.setZero(); + } else { + gradient_row = auto_diff_matrix(row, col).derivatives(); + } + } + } + return gradient; +} + +template +void gradientMatrixToAutoDiff(const Eigen::MatrixBase& gradient, Eigen::MatrixBase& auto_diff_matrix) +{ + int nx = gradient.cols(); + for (size_t row = 0; row < auto_diff_matrix.rows(); row++) { + for (size_t col = 0; col < auto_diff_matrix.cols(); col++) { + auto_diff_matrix(row, col).derivatives().resize(nx, 1); + auto_diff_matrix(row, col).derivatives() = gradient.row(row + col * auto_diff_matrix.rows()).transpose(); + } + } +} + #endif /* DRAKEGRADIENTUTIL_H_ */ diff --git a/drake/util/drakeMexUtil.h b/drake/util/drakeMexUtil.h index 02c21055eb0b..d0da2f93416b 100644 --- a/drake/util/drakeMexUtil.h +++ b/drake/util/drakeMexUtil.h @@ -1,7 +1,9 @@ #include "mex.h" #include #include +#include #include "TrigPoly.h" +#include "drakeGradientUtil.h" #ifndef DRAKE_MEX_UTIL_H_ #define DRAKE_MEX_UTIL_H_ @@ -207,5 +209,32 @@ mxArray* eigenToTrigPoly(const Eigen::Matrix & trigpoly_m return plhs[0]; } +template +Eigen::Matrix, Rows, Cols> taylorVarToEigen(const mxArray* taylor_var) { + auto f = mxGetPropertySafe(taylor_var, "f"); + auto df = mxGetPropertySafe(taylor_var, "df"); + if (mxGetNumberOfElements(df) > 1) + throw std::runtime_error("TaylorVars of order higher than 1 currently not supported"); + auto ret = matlabToEigenMap(f).template cast>().eval(); + typedef Gradient GradientType; + auto gradient_matrix = matlabToEigenMap(mxGetCell(df, 0)); + gradientMatrixToAutoDiff(gradient_matrix, ret); + return ret; +} + +template +mxArray* eigenToTaylorVar(const Eigen::MatrixBase& m, int num_variables = Eigen::Dynamic) +{ + const int nrhs = 2; + mxArray *prhs[nrhs]; + prhs[0] = eigenToMatlab(autoDiffToValueMatrix(m)); + mwSize dims[] = {1}; + prhs[1] = mxCreateCellArray(1, dims); + mxArray *plhs[1]; + mxSetCell(prhs[1], 0, eigenToMatlab(autoDiffToGradientMatrix(m, num_variables))); + mexCallMATLABsafe(1, plhs, nrhs, prhs, "TaylorVar"); + return plhs[0]; +} + #endif