diff --git a/.gitignore b/.gitignore index fd61a73c50e4..dcf6d84b61ca 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ +*.idea *.autosave *.mex* *.exp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1de51b7111cb..5fd63d3a9245 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,7 +54,7 @@ set(avl_GIT_TAG 9b927f90619460b29f7454c714369f65bc4536a1) set(bertini_GIT_REPOSITORY https://github.com/RobotLocomotion/bertini.git) set(bertini_GIT_TAG 3ae3e3ad3534acb4b6f7a4c3c22728b493beaa80) set(bullet_GIT_REPOSITORY https://github.com/RobotLocomotion/bullet-pod.git) -set(bullet_GIT_TAG 6db7ae5e29f8b35ffcea384c53caa4bddcc6f986) +set(bullet_GIT_TAG c7c87f327b7f3ba02de279d026d6642e9f6443f7) set(cmake_GIT_REPOSITORY https://github.com/RobotLocomotion/cmake.git) set(cmake_GIT_TAG 451fdffdd0e7bfbfa53f1d5f50fad5196bb53bca) set(cmake_GIT_CLONE_DIR "${PROJECT_SOURCE_DIR}/drake/cmake") @@ -65,11 +65,11 @@ set(director_SOURCE_DIR ${PROJECT_SOURCE_DIR}/externals/director/distro/pods/dra set(signalscope_GIT_REPOSITORY https://github.com/mitdrc/signal-scope.git) set(signalscope_GIT_TAG dc61a95daf31d25c730a075b5a8c9bba9eadafb3) set(eigen_GIT_REPOSITORY https://github.com/RobotLocomotion/eigen-pod.git) -set(eigen_GIT_TAG fb56a4b8504eb115a7f257de4570c47c20c43397) +set(eigen_GIT_TAG c6c4738f9d18fa30003c33028486436adfe8bc70) set(gtk_GIT_REPOSITORY https://github.com/RobotLocomotion/gtk-pod.git) set(gtk_GIT_TAG dc9bb719ee771a2ae9c3a392796c42ccad5bca54) set(gloptipoly_GIT_REPOSITORY https://github.com/RobotLocomotion/gloptipoly3.git) # todo: rename that repo -set(gloptipoly_GIT_TAG a3547a6811bf76c4e18f00b6bcbc187c6a7fd3d8) +set(gloptipoly_GIT_TAG c9e796e99c584ecf78317348c4162a2ed4121156) set(gurobi_GIT_REPOSITORY https://github.com/RobotLocomotion/gurobi.git) set(gurobi_GIT_TAG beb0a84a1bcca1eb81a1b87d9fa140f2134d97e9) set(iris_GIT_REPOSITORY https://github.com/rdeits/iris-distro.git) @@ -85,11 +85,11 @@ set(mosek_GIT_TAG a72594c4a7c8c5c026fd291148bcac76146634c2) set(octomap_GIT_REPOSITORY https://github.com/RobotLocomotion/octomap-pod.git) set(octomap_GIT_TAG 00b28215d19580f9e964bc5d126ce55a9253671a) set(sedumi_GIT_REPOSITORY https://github.com/RobotLocomotion/sedumi.git) -set(sedumi_GIT_TAG afac8ea98f91007a3710d761efd514e10d0ad536) +set(sedumi_GIT_TAG 8263577d4ab375524060c19369ccf410133bb9eb) set(snopt_GIT_REPOSITORY https://github.com/RobotLocomotion/snopt.git) set(snopt_GIT_TAG d7abb603690f41970273643ebb6e173b0e6e452b) set(spotless_GIT_REPOSITORY https://github.com/RobotLocomotion/spotless-pod.git) -set(spotless_GIT_TAG 565b720c25414a249d8b547183cf3ea6cf043211) +set(spotless_GIT_TAG 88e8eefc9a853e96268cad7ac0d70ae2d8d39f35) set(yalmip_GIT_REPOSITORY https://github.com/RobotLocomotion/yalmip.git) set(yalmip_GIT_TAG c071fb7b7193491f8eefadba3bfff26160ad6cd4) set(xfoil_GIT_REPOSITORY https://github.com/RobotLocomotion/xfoil.git) @@ -109,7 +109,29 @@ endif() # process optional projects # note: keep drake in this loop in case externals depend on drake (e.g. the director might in the near future) set(EXTERNAL_PROJECTS) -foreach(proj IN ITEMS cmake eigen gtk lcm libbot bullet iris spotless director signalscope octomap snopt gurobi mosek yalmip gloptipoly bertini sedumi avl xfoil meshconverters drake) +foreach(proj IN ITEMS + cmake + eigen + gtk + lcm + libbot + bullet + iris + spotless + director + signalscope + octomap + snopt + gurobi + mosek + yalmip + gloptipoly + bertini + sedumi + avl + xfoil + meshconverters + drake) string(TOUPPER ${proj} proj_upper) if (${proj} STREQUAL "drake" OR ${proj} STREQUAL "cmake" OR WITH_${proj_upper} OR WITH_ALL_SUPPORTED_EXTERNALS) list(APPEND EXTERNAL_PROJECTS ${proj}) diff --git a/drake/Makefile b/drake/Makefile index 7a1081524812..06ba2e9bc3aa 100644 --- a/drake/Makefile +++ b/drake/Makefile @@ -105,8 +105,12 @@ release_filelist: -find pod-build/bin -type f 2> /dev/null clean: +ifeq ($(BUILD_SYSTEM),Windows_NT) + rd /s pod-build +else -if [ -e pod-build/install_manifest.txt ]; then rm -f `cat pod-build/install_manifest.txt`; fi -if [ -d pod-build ]; then cmake --build pod-build --target clean; rm -rf pod-build; fi +endif # other (custom) targets are passed through to the cmake-generated Makefile %:: diff --git a/drake/addpath_drake.m b/drake/addpath_drake.m index c78e8f624a7b..a3307ed81f7f 100644 --- a/drake/addpath_drake.m +++ b/drake/addpath_drake.m @@ -59,6 +59,21 @@ javaaddpath(fullfile(pods_get_base_path,'share','java','drake.jar')); +% OSX platform-specific: check if reverted to IPv4 +if (computer('arch') == 'maci64') + ipv4_preferred = java.lang.System.getProperty('java.net.preferIPv4Stack'); + if isempty(ipv4_preferred) + ipv4_preferred = 'false'; + end + if (ipv4_preferred ~= 'true') + display('WARNING: Your JVM may crash if you do not set it to prefer IPv4 over IPv6.') + display('This may cause any dependencies that involve the JVM (including LCM) to crash at runtime.') + display('Please see bug report and solution here: https://github.com/RobotLocomotion/drake/issues/558.') + display('(It just involves adding one line to your java.opts file for Matlab.)') + display('Make sure to restart Matlab after editing your java.opts file.') + end +end + if ispc setenv('PATH',[getenv('PATH'),';',GetFullPath(pods_get_lib_path),';',fullfile(root,'pod-build','lib','Release'),';',fullfile(root,'pod-build','lib')]); end diff --git a/drake/examples/Acrobot/@AcrobotPlantCpp/manipulatorDynamics.cpp b/drake/examples/Acrobot/@AcrobotPlantCpp/manipulatorDynamics.cpp index 169c59225e99..ac7fde878fff 100644 --- a/drake/examples/Acrobot/@AcrobotPlantCpp/manipulatorDynamics.cpp +++ b/drake/examples/Acrobot/@AcrobotPlantCpp/manipulatorDynamics.cpp @@ -3,84 +3,88 @@ #include #include #include "drakeMexUtil.h" +#include using namespace Eigen; using namespace std; + /* * c++ version of * function [H,C,B] = manipulatorDynamics(obj,q,qd) */ -template -void manipulatorDynamics(const mxArray* pobj, const MatrixBase &q, const MatrixBase &qd, MatrixBase &H, MatrixBase &C, MatrixBase &B) +template +void manipulatorDynamics(const mxArray *pobj, const MatrixBase& q, const MatrixBase& qd, + mxArray **plhs) { // keep it readable: - double m1 = mxGetScalar(mxGetProperty(pobj,0,"m1")); - double m2 = mxGetScalar(mxGetProperty(pobj,0,"m2")); - double l1 = mxGetScalar(mxGetProperty(pobj,0,"l1")); - double g = mxGetScalar(mxGetProperty(pobj,0,"g")); - double lc1 = mxGetScalar(mxGetProperty(pobj,0,"lc1")); - double lc2 = mxGetScalar(mxGetProperty(pobj,0,"lc2")); - double b1 = mxGetScalar(mxGetProperty(pobj,0,"b1")); - double b2 = mxGetScalar(mxGetProperty(pobj,0,"b2")); - double I1 = mxGetScalar(mxGetProperty(pobj,0,"Ic1")) + m1*lc1*lc1; - double I2 = mxGetScalar(mxGetProperty(pobj,0,"Ic2")) + m2*lc2*lc2; - double m2l1lc2 = m2*l1*lc2; // occurs often! + double m1 = mxGetScalar(mxGetProperty(pobj, 0, "m1")); + double m2 = mxGetScalar(mxGetProperty(pobj, 0, "m2")); + double l1 = mxGetScalar(mxGetProperty(pobj, 0, "l1")); + double g = mxGetScalar(mxGetProperty(pobj, 0, "g")); + double lc1 = mxGetScalar(mxGetProperty(pobj, 0, "lc1")); + double lc2 = mxGetScalar(mxGetProperty(pobj, 0, "lc2")); + double b1 = mxGetScalar(mxGetProperty(pobj, 0, "b1")); + double b2 = mxGetScalar(mxGetProperty(pobj, 0, "b2")); + double I1 = mxGetScalar(mxGetProperty(pobj, 0, "Ic1")) + m1 * lc1 * lc1; + double I2 = mxGetScalar(mxGetProperty(pobj, 0, "Ic2")) + m2 * lc2 * lc2; + double m2l1lc2 = m2 * l1 * lc2; // occurs often! auto c2 = cos(q(1)); auto s1 = sin(q(0)), s2 = sin(q(1)); - auto s12 = sin(q(0)+q(1)); - - auto h12 = I2 + m2l1lc2*c2; - H << I1 + I2 + m2*l1*l1 + 2*m2l1lc2*c2, h12, h12, I2; + auto s12 = sin(q(0) + q(1)); + + auto h12 = I2 + m2l1lc2 * c2; + + typedef typename DerivedQ::Scalar Scalar; + Matrix H; + Matrix C; + Matrix B; + + H << I1 + I2 + m2 * l1 * l1 + 2 * m2l1lc2 * c2, h12, h12, I2; //C = [ -2*m2l1lc2*s(2)*qd(2), -m2l1lc2*s(2)*qd(2); m2l1lc2*s(2)*qd(1), 0 ]; //G = g*[ m1*lc1*s(1) + m2*(l1*s(1)+lc2*s12); m2*lc2*s12 ]; - C << -2*m2l1lc2*s2*qd(1)*qd(0) + -m2l1lc2*s2*qd(1)*qd(1), m2l1lc2*s2*qd(0)*qd(0); + C << -2 * m2l1lc2 * s2 * qd(1) * qd(0) + -m2l1lc2 * s2 * qd(1) * qd(1), m2l1lc2 * s2 * qd(0) * qd(0); // add in G terms - C(0) += g*m1*lc1*s1 + g*m2*(l1*s1+lc2*s12); C(1) += g*m2*lc2*s12; + C(0) += g * m1 * lc1 * s1 + g * m2 * (l1 * s1 + lc2 * s12); + C(1) += g * m2 * lc2 * s12; // damping terms - C(0)+=b1*qd(0); C(1)+=b2*qd(1); + C(0) += b1 * qd(0); + C(1) += b2 * qd(1); + + // input matrix + // multiplying by q(0) just to get the size of the derivatives vector right; Matlab TaylorVar operations will complain otherwise. We could handle this case on the Matlab side instead. + B << 0.0 * q(0), 1.0; - B << 0.0, 1.0; + plhs[0] = eigenToMatlabGeneral<2, 2>(H); + plhs[1] = eigenToMatlabGeneral<2, 1>(C); + plhs[2] = eigenToMatlabGeneral<2, 1>(B); } -void mexFunction( int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[] ) +void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { - const mxArray* pobj = prhs[0]; + const mxArray *pobj = prhs[0]; if (mxIsDouble(prhs[1]) && mxIsDouble(prhs[2])) { - auto q = matlabToEigenMap<2,1>(prhs[1]); - auto qd = matlabToEigenMap<2,1>(prhs[2]); - - plhs[0] = mxCreateDoubleMatrix(2,2,mxREAL); - Map H(mxGetPr(plhs[0])); - plhs[1] = mxCreateDoubleMatrix(2,1,mxREAL); - Map C(mxGetPr(plhs[1]),2); - plhs[2] = mxCreateDoubleMatrix(2,1,mxREAL); - Map B(mxGetPr(plhs[2]),2); - - manipulatorDynamics(pobj,q,qd,H,C,B); - } else if (isa(prhs[1],"TrigPoly") && isa(prhs[2],"TrigPoly")) { + auto q = matlabToEigenMap<2, 1>(prhs[1]); + auto qd = matlabToEigenMap<2, 1>(prhs[2]); + manipulatorDynamics(pobj, q, qd, plhs); + } else if (isa(prhs[1], "TrigPoly") && isa(prhs[2], "TrigPoly")) { auto q = trigPolyToEigen(prhs[1]); auto qd = trigPolyToEigen(prhs[2]); - Matrix< TrigPolyd, 2, 2> H; - Matrix< TrigPolyd, 2, 1> C; - Matrix< TrigPolyd, 2, 1> B; - - manipulatorDynamics(pobj,q,qd,H,C,B); - - plhs[0] = eigenToTrigPoly<2,2>(H); - plhs[1] = eigenToTrigPoly<2,1>(C); - plhs[2] = eigenToTrigPoly<2,1>(B); + manipulatorDynamics(pobj, q, qd, plhs); + } else if (isa(prhs[1], "TaylorVar") && isa(prhs[2], "TaylorVar")) { + auto q = taylorVarToEigen(prhs[1]); + auto qd = taylorVarToEigen(prhs[2]); + manipulatorDynamics(pobj, q, qd, plhs); } else { - mexErrMsgIdAndTxt("Drake:AcrobotPLantCpp:UnknownType","don't know how to handle the datatypes passed in for q and/or qd (yet)"); + mexErrMsgIdAndTxt("Drake:AcrobotPLantCpp:UnknownType", + "don't know how to handle the datatypes passed in for q and/or qd (yet)"); } - - } diff --git a/drake/examples/Acrobot/test/testCpp.m b/drake/examples/Acrobot/test/testCpp.m index 2cb48ab6ce84..3d7d0a919d6d 100644 --- a/drake/examples/Acrobot/test/testCpp.m +++ b/drake/examples/Acrobot/test/testCpp.m @@ -11,13 +11,11 @@ [am,bm] = getPolyDynamics(r_matlab_tp); [ac,bc] = getPolyDynamics(r_cpp_tp); assert(isequal(am,ac)); -assert(isequal(bm,bm)); +assert(isequal(bm,bc)); +balanceLQR(r_cpp); -% currently fails because taylorvars get passed into the C++ from the geval -% in Manipulator/linearize. -% balanceLQR(r_cpp); - +geval_options.grad_method = 'taylorvar'; for i=1:20 t = randn(); x = randn(4,1); @@ -26,10 +24,17 @@ [H_matlab,C_matlab,B_matlab] = manipulatorDynamics(r_matlab,x(1:2),x(3:4)); [H_cpp,C_cpp,B_cpp] = manipulatorDynamics(r_cpp,x(1:2),x(3:4)); + [~, ~, ~, dH_matlab, dC_matlab, dB_matlab] = geval(3, @(x) manipulatorDynamics(r_matlab, x(1:2), x(3:4)), x, geval_options); + [~, ~, ~, dH_cpp, dC_cpp, dB_cpp] = geval(3, @(x) manipulatorDynamics(r_cpp, x(1:2), x(3:4)), x, geval_options); + valuecheck(H_cpp,H_matlab); valuecheck(C_cpp,C_matlab); valuecheck(B_cpp,B_matlab); + valuecheck(dH_cpp,dH_matlab); + valuecheck(dC_cpp,dC_matlab); + valuecheck(dB_cpp,dB_matlab); + valuecheck(dynamics(r_matlab,t,x,u),dynamics(r_cpp,t,x,u)); end diff --git a/drake/solvers/qpSpline/nWaypointCubicSplinemex.cpp b/drake/solvers/qpSpline/nWaypointCubicSplinemex.cpp index 8d16db8f4492..afc388856e60 100644 --- a/drake/solvers/qpSpline/nWaypointCubicSplinemex.cpp +++ b/drake/solvers/qpSpline/nWaypointCubicSplinemex.cpp @@ -39,7 +39,7 @@ void mexFunction(int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[]) { for (mwSize segment_index = 0; segment_index < spline.getNumberOfSegments(); segment_index++) { for (mwSize coefficient_index = 0; coefficient_index < num_coeffs_per_segment; coefficient_index++) { mwSize sub[] = {dof, segment_index, num_coeffs_per_segment - coefficient_index - 1}; // Matlab's reverse coefficient indexing... - *(mxGetPr(plhs[0]) + sub2ind(3, dims, sub)) = spline.getPolynomial(segment_index).getCoefficients()[coefficient_index]; + *(mxGetPr(plhs[0]) + sub2ind(3, dims, sub)) = spline.getPolynomial(static_cast(segment_index)).getCoefficients()[coefficient_index]; } } } diff --git a/drake/systems/plants/RigidBodyManipulator.cpp b/drake/systems/plants/RigidBodyManipulator.cpp index e068b96df8c0..4f7578091257 100644 --- a/drake/systems/plants/RigidBodyManipulator.cpp +++ b/drake/systems/plants/RigidBodyManipulator.cpp @@ -1737,7 +1737,7 @@ GradientVar Rigi J.value().template block(row_start, *it, Jrot.rows(), 1) = Jrot.col(col); col++; } - row_start += qrot.rows(); + row_start += static_cast(qrot.rows()); } } @@ -1792,7 +1792,7 @@ GradientVar Rigi } col++; } - row_start += qrot.rows(); + row_start += static_cast(qrot.rows()); } } } @@ -2202,7 +2202,7 @@ void RigidBodyManipulator::checkCachedKinematicsSettings(bool kinematics_gradien void RigidBodyManipulator::addFrame(const RigidBodyFrame& frame) { frames.push_back(frame); - num_frames = frames.size(); + num_frames = static_cast(frames.size()); } // explicit instantiations (required for linking): diff --git a/drake/systems/plants/collision/BulletModel.cpp b/drake/systems/plants/collision/BulletModel.cpp index 7ad68fbe7034..9b11cf059966 100644 --- a/drake/systems/plants/collision/BulletModel.cpp +++ b/drake/systems/plants/collision/BulletModel.cpp @@ -119,9 +119,12 @@ namespace DrakeCollision bt_shape->setMargin(BulletModel::large_margin); else bt_shape->setMargin(BulletModel::small_margin); - for (int i=0; i(bt_shape.get())->addPoint(btVector3(vertices(0,i),vertices(1,i),vertices(2,i))); + auto bt_convex_hull_shape = dynamic_cast(bt_shape.get()); + for (int i = 0; i < vertices.cols(); i++) { + bt_convex_hull_shape->addPoint(btVector3(vertices(0, i), vertices(1, i), vertices(2, i)), false); } + bt_convex_hull_shape->recalcLocalAabb(); + return bt_shape; } else { return nullptr; @@ -135,9 +138,11 @@ namespace DrakeCollision bt_shape->setMargin(BulletModel::large_margin); else bt_shape->setMargin(BulletModel::small_margin); - for (int i=0; i(bt_shape.get())->addPoint(btVector3(geometry.points(0,i),geometry.points(1,i),geometry.points(2,i))); + auto bt_convex_hull_shape = dynamic_cast(bt_shape.get()); + for (int i = 0; i < geometry.points.cols(); i++){ + bt_convex_hull_shape->addPoint(btVector3(geometry.points(0, i), geometry.points(1, i), geometry.points(2, i)), false); } + bt_convex_hull_shape->recalcLocalAabb(); return bt_shape; } diff --git a/drake/systems/plants/constraint/constructPtrRigidBodyConstraint.cpp b/drake/systems/plants/constraint/constructPtrRigidBodyConstraint.cpp index 088c2e307b3f..2917049ab058 100644 --- a/drake/systems/plants/constraint/constructPtrRigidBodyConstraint.cpp +++ b/drake/systems/plants/constraint/constructPtrRigidBodyConstraint.cpp @@ -17,7 +17,7 @@ void rigidBodyConstraintParseTspan(const mxArray* pm,Eigen::Vector2d &tspan) { mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParseTspan:BadInputs","tspan is a 1x2 vector"); } - int num_tspan = mxGetNumberOfElements(pm); + size_t num_tspan = mxGetNumberOfElements(pm); if(num_tspan == 1) { mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParseTspan:BadInputs","tspan is a 1x2 vector"); diff --git a/drake/systems/plants/constraint/constructPtrRigidBodyConstraintmex.cpp b/drake/systems/plants/constraint/constructPtrRigidBodyConstraintmex.cpp index 9db6e259b9f5..e70a2117f59c 100644 --- a/drake/systems/plants/constraint/constructPtrRigidBodyConstraintmex.cpp +++ b/drake/systems/plants/constraint/constructPtrRigidBodyConstraintmex.cpp @@ -40,7 +40,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]); Vector2d tspan; int* robotnum; - int num_robot; + size_t num_robot; if(nrhs <= 3) { num_robot = 1; @@ -115,7 +115,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","iAfun, jAvar and A must be numeric"); } - int lenA = mxGetM(prhs[2]); + size_t lenA = mxGetM(prhs[2]); if(mxGetM(prhs[3]) != lenA || mxGetM(prhs[4]) != lenA || mxGetN(prhs[2]) != 1 || mxGetN(prhs[3]) != 1 || mxGetN(prhs[4]) != 1) { mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","iAfun,jAvar,A must be column vectors of the same size"); @@ -133,7 +133,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","lb and ub must be numeric column vectors of the same size"); } - int num_constraint = mxGetM(prhs[5]); + size_t num_constraint = mxGetM(prhs[5]); VectorXd lb(num_constraint); VectorXd ub(num_constraint); memcpy(lb.data(),mxGetPrSafe(prhs[5]),sizeof(double)*num_constraint); @@ -177,7 +177,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) //DEBUG //cout << "collisionDetectmex: Received body_idx" << endl; //END_DEBUG - int n_active_bodies = mxGetNumberOfElements(body_idx); + size_t n_active_bodies = mxGetNumberOfElements(body_idx); //DEBUG //cout << "collisionDetectmex: n_active_bodies = " << n_active_bodies << endl; //END_DEBUG @@ -193,10 +193,10 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) const mxArray* collision_groups = mxGetField(active_collision_options,0, "collision_groups"); if (collision_groups != NULL) { - int num = mxGetNumberOfElements(collision_groups); + size_t num = mxGetNumberOfElements(collision_groups); for (int i=0; i(mxGetNumberOfElements(prhs[2])); double* t_ptr = new double[n_breaks]; memcpy(t_ptr,mxGetPrSafe(prhs[2]),sizeof(double)*n_breaks); int nq = cnst->getRobotPointer()->num_positions; diff --git a/drake/systems/plants/constraint/testMultipleTimeLinearPostureConstraintmex.cpp b/drake/systems/plants/constraint/testMultipleTimeLinearPostureConstraintmex.cpp index e72b0d600af7..5bdedca13c18 100644 --- a/drake/systems/plants/constraint/testMultipleTimeLinearPostureConstraintmex.cpp +++ b/drake/systems/plants/constraint/testMultipleTimeLinearPostureConstraintmex.cpp @@ -23,7 +23,7 @@ void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray *prhs[]) mexErrMsgIdAndTxt("Drake:testMultipleTimeLinearPostureConstrainttmex:BadInputs","Usage [num_cnst,cnst_val,iAfun,jAvar,A,cnst_name,lb,ub] = testMultipleTimeLinearPostureConstraintmex(kinCnst,q,t)"); } MultipleTimeLinearPostureConstraint* cnst = (MultipleTimeLinearPostureConstraint*) getDrakeMexPointer(prhs[0]); - int n_breaks = mxGetNumberOfElements(prhs[2]); + int n_breaks = static_cast(mxGetNumberOfElements(prhs[2])); double* t_ptr = new double[n_breaks]; memcpy(t_ptr,mxGetPrSafe(prhs[2]),sizeof(double)*n_breaks); int nq = cnst->getRobotPointer()->num_positions; diff --git a/drake/systems/plants/constraint/testSingleTimeKinCnstmex.cpp b/drake/systems/plants/constraint/testSingleTimeKinCnstmex.cpp index 2cef906ba664..01d4ffe0d75e 100644 --- a/drake/systems/plants/constraint/testSingleTimeKinCnstmex.cpp +++ b/drake/systems/plants/constraint/testSingleTimeKinCnstmex.cpp @@ -30,7 +30,7 @@ void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) } else { - int num_t = mxGetNumberOfElements(prhs[2]); + size_t num_t = mxGetNumberOfElements(prhs[2]); if(num_t == 0) { t_ptr = nullptr; diff --git a/drake/systems/plants/contactConstraintsmex.cpp b/drake/systems/plants/contactConstraintsmex.cpp index 10714963f3ea..ad352dd8054a 100644 --- a/drake/systems/plants/contactConstraintsmex.cpp +++ b/drake/systems/plants/contactConstraintsmex.cpp @@ -29,19 +29,20 @@ using namespace std; inline void buildSparseMatrix(Matrix3xd const & pts, SparseMatrix & sparse) { - const size_t m = pts.cols(); - const size_t numNonZero = 3*m; + typedef SparseMatrix::Index SparseIndex; + const SparseIndex m = static_cast(pts.cols()); + const SparseIndex numNonZero = 3*m; sparse.resize(m, numNonZero); sparse.reserve(VectorXi::Constant(numNonZero, 1)); - size_t j = 0; - for (size_t i = 0 ; i < m ; i++) { - for (size_t k = 0 ; k < 3 ; k++) { - sparse.insert(i, j) = pts(j); - j++; - } - } + SparseIndex j = 0; + for (SparseIndex i = 0 ; i < m ; i++) { + for (SparseIndex k = 0 ; k < 3 ; k++) { + sparse.insert(i, j) = pts(j); + j++; + } + } } void mexFunction( int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[] ) { diff --git a/drake/systems/plants/surfaceTangentsmex.cpp b/drake/systems/plants/surfaceTangentsmex.cpp index 486f2c39dc5c..181a2545789b 100644 --- a/drake/systems/plants/surfaceTangentsmex.cpp +++ b/drake/systems/plants/surfaceTangentsmex.cpp @@ -18,7 +18,7 @@ using namespace std; inline mxArray* getTangentsArray(RigidBodyManipulator* const model, Map const & normals) { - const int numContactPairs = normals.cols(); + const size_t numContactPairs = normals.cols(); const mwSize cellDims[] = {1, BASIS_VECTOR_HALF_COUNT}; mxArray* tangentCells = mxCreateCellArray(2, cellDims); @@ -41,8 +41,8 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) } RigidBodyManipulator *model= (RigidBodyManipulator*) getDrakeMexPointer(prhs[0]); - const unsigned int numNormals = mxGetN(prhs[1]); //number of normal vectors - const unsigned int dimNormals = mxGetM(prhs[1]); //dimension of each normal vector + const size_t numNormals = mxGetN(prhs[1]); //number of normal vectors + const size_t dimNormals = mxGetM(prhs[1]); //dimension of each normal vector if (dimNormals != 3) { diff --git a/drake/util/CMakeLists.txt b/drake/util/CMakeLists.txt index 8291eb1e7227..3faf0768be9a 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/MexWrapper.h b/drake/util/MexWrapper.h index 9ddde33216be..0f2dd776dc1a 100644 --- a/drake/util/MexWrapper.h +++ b/drake/util/MexWrapper.h @@ -6,7 +6,7 @@ #undef DLLEXPORT #if defined(WIN32) || defined(WIN64) - #if defined(drakeUtil_EXPORTS) + #if defined(drakeMexUtil_EXPORTS) #define DLLEXPORT __declspec( dllexport ) #else #define DLLEXPORT __declspec( dllimport ) @@ -15,7 +15,7 @@ #define DLLEXPORT #endif -DLLEXPORT class MexWrapper { +class DLLEXPORT MexWrapper { public: MexWrapper(std::string const & filename); ~MexWrapper(); diff --git a/drake/util/Polynomial.cpp b/drake/util/Polynomial.cpp index 697f2fcdc2f9..eebe97d932ed 100644 --- a/drake/util/Polynomial.cpp +++ b/drake/util/Polynomial.cpp @@ -429,7 +429,7 @@ template void Polynomial::makeMonomialsUnique(void) { VarType unique_var = 0; // also update the univariate flag - for (int i=monomials.size()-1; i>=0; i--) { + for (ptrdiff_t i=monomials.size()-1; i>=0; i--) { Monomial& mi=monomials[i]; if (!mi.terms.empty()) { if (mi.terms.size()>1) is_univariate = false; @@ -441,7 +441,7 @@ void Polynomial::makeMonomialsUnique(void) } } } - for (int j=0; j<(i-1); j++) { + for (ptrdiff_t j=0; j<(i-1); j++) { Monomial& mj=monomials[j]; if (mi.hasSameExponents(mj)) { // it's a match, so delete monomial i diff --git a/drake/util/convexHull.cpp b/drake/util/convexHull.cpp index 7d8147f0947e..5b249f4e3bf2 100644 --- a/drake/util/convexHull.cpp +++ b/drake/util/convexHull.cpp @@ -26,7 +26,8 @@ coord2_t cross(const Point &O, const Point &A, const Point &B) vector convexHull(vector P) { - size_t n = P.size(), k = 0; + ptrdiff_t n = P.size(); + ptrdiff_t k = 0; vector H(2*n); if (n == 2) { @@ -41,13 +42,13 @@ vector convexHull(vector P) sort(P.begin(), P.end()); // Build lower hull - for (int i = 0; i < n; ++i) { + for (size_t i = 0; i < n; ++i) { while (k >= 2 && cross(H[k-2], H[k-1], P[i]) <= 0) k--; H[k++] = P[i]; } // Build upper hull - for (int i = (int)n-2, t = k+1; i >= 0; i--) { + for (ptrdiff_t i = n-2, t = k+1; i >= 0; i--) { while (k >= t && cross(H[k-2], H[k-1], P[i]) <= 0) k--; H[k++] = P[i]; } diff --git a/drake/util/drakeGradientUtil.h b/drake/util/drakeGradientUtil.h index 6a92c0bd20d2..75f33a55f233 100644 --- a/drake/util/drakeGradientUtil.h +++ b/drake/util/drakeGradientUtil.h @@ -7,6 +7,7 @@ #include #include #include +#include #undef DLLEXPORT #if defined(WIN32) || defined(WIN64) @@ -76,6 +77,16 @@ template struct GetSubMatrixGradientSingleElement { typedef typename Eigen::Block type; }; + +template +struct AutoDiffToValueMatrix { + typedef typename Eigen::Matrix type; +}; + +template +struct AutoDiffToGradientMatrix { + typedef typename Gradient, Eigen::Dynamic>::type type; +}; /* * Profile results: looks like return value optimization works; a version that sets a reference @@ -128,4 +139,60 @@ 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 +typename AutoDiffToValueMatrix::type autoDiffToValueMatrix(const Eigen::MatrixBase& auto_diff_matrix) { + typename AutoDiffToValueMatrix::type 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 AutoDiffToGradientMatrix::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 AutoDiffToGradientMatrix::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) +{ + typedef typename Eigen::MatrixBase::Index Index; + auto nx = gradient.cols(); + for (Index row = 0; row < auto_diff_matrix.rows(); row++) { + for (Index 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.cpp b/drake/util/drakeMexUtil.cpp index 88f1e07b1994..386f7aec617b 100644 --- a/drake/util/drakeMexUtil.cpp +++ b/drake/util/drakeMexUtil.cpp @@ -329,7 +329,7 @@ mwSize sub2ind(mwSize ndims, const mwSize* dims, const mwSize* sub) { return ret; } -void sizecheck(const mxArray* mat, int M, int N) { +void sizecheck(const mxArray* mat, mwSize M, mwSize N) { if (mxGetM(mat) != M) { mexErrMsgIdAndTxt("Drake:WrongSize", "wrong number of rows. Expected: %d but got: %d", M, mxGetM(mat)); } diff --git a/drake/util/drakeMexUtil.h b/drake/util/drakeMexUtil.h index 7253f05e05fb..a340d0956c63 100644 --- a/drake/util/drakeMexUtil.h +++ b/drake/util/drakeMexUtil.h @@ -2,6 +2,14 @@ #include #include #include "TrigPoly.h" +/* + * NOTE: include AutoDiff AFTER TrigPoly.h. + * TrigPoly.h includes LLDT.h via Eigenvalues, PolynomialSolver, and our Polynomial.h + * MSVC versions up to and including 2013 have trouble with the rankUpdate method in LLDT.h + * For some reason there is a bad interaction with AutoDiff, even though LLDT.h still gets included if TrigPoly.h is included before AutoDiff. + */ +#include +#include "drakeGradientUtil.h" #ifndef DRAKE_MEX_UTIL_H_ #define DRAKE_MEX_UTIL_H_ @@ -107,7 +115,7 @@ mxArray* stdVectorToMatlab(const std::vector& vec) { DLLEXPORT mxArray* stdStringToMatlab(const std::string& str); DLLEXPORT mxArray* vectorOfStdStringsToMatlab(const std::vector& strs); -DLLEXPORT void sizecheck(const mxArray* mat, int M, int N); +DLLEXPORT void sizecheck(const mxArray* mat, mwSize M, mwSize N); template void matlabToCArrayOfArrays(const mxArray *source, double (&destination)[Rows][Cols]) { @@ -215,5 +223,51 @@ 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]; +} + +template +mxArray *eigenToMatlabGeneral( + const Eigen::MatrixBase, RowsAtCompileTime, ColsAtCompileTime>>& mat) +{ +return eigenToTaylorVar(mat); +}; + +template +mxArray *eigenToMatlabGeneral(const Eigen::MatrixBase>& mat) +{ + return eigenToTrigPoly(mat); +}; + +template +mxArray *eigenToMatlabGeneral(const Eigen::MatrixBase>& mat) +{ + return eigenToMatlab(mat.const_cast_derived()); +}; + #endif