resolved merge conflicts

.gitignore

@@ -1,3 +1,4 @@
+*.idea
 *.autosave
 *.mex*
 *.exp

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})

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
 %::

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

drake/examples/Acrobot/@AcrobotPlantCpp/manipulatorDynamics.cpp

@@ -3,84 +3,88 @@
 #include <cmath>
 #include <iostream>
 #include "drakeMexUtil.h"
+#include <drakeGradientUtil.h>
 
 using namespace Eigen;
 using namespace std;
+
 /*
  * c++ version of
  *     function [H,C,B] = manipulatorDynamics(obj,q,qd)
  */
 
-template <typename DerivedA, typename DerivedB, typename DerivedC>
-void manipulatorDynamics(const mxArray* pobj, const MatrixBase<DerivedA> &q, const MatrixBase<DerivedA> &qd, MatrixBase<DerivedB> &H, MatrixBase<DerivedC> &C, MatrixBase<DerivedC> &B)
+template<typename DerivedQ>
+void manipulatorDynamics(const mxArray *pobj, const MatrixBase<DerivedQ>& q, const MatrixBase<DerivedQ>& 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<Scalar, 2, 2> H;
+  Matrix<Scalar, 2, 1> C;
+  Matrix<Scalar, 2, 1> 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<Matrix2d> H(mxGetPr(plhs[0]));
-    plhs[1] = mxCreateDoubleMatrix(2,1,mxREAL);
-    Map<Vector2d> C(mxGetPr(plhs[1]),2);
-    plhs[2] = mxCreateDoubleMatrix(2,1,mxREAL);
-    Map<Vector2d> 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<Dynamic, 1>(prhs[1]);
+    auto qd = taylorVarToEigen<Dynamic, 1>(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)");
   }
-
-
 }
 
 

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
 

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<int>(segment_index)).getCoefficients()[coefficient_index];
       }
     }
   }

drake/systems/plants/RigidBodyManipulator.cpp

@@ -1737,7 +1737,7 @@ GradientVar<typename DerivedPoints::Scalar, Eigen::Dynamic, Eigen::Dynamic> Rigi
         J.value().template block<Eigen::Dynamic, 1>(row_start, *it, Jrot.rows(), 1) = Jrot.col(col);
         col++;
       }
-      row_start += qrot.rows();
+      row_start += static_cast<int>(qrot.rows());
     }
   }
 
@@ -1792,7 +1792,7 @@ GradientVar<typename DerivedPoints::Scalar, Eigen::Dynamic, Eigen::Dynamic> Rigi
           }
           col++;
         }
-        row_start += qrot.rows();
+        row_start += static_cast<int>(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<int>(frames.size());
 }
 
 // explicit instantiations (required for linking):

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<vertices.cols(); i++){
-        dynamic_cast<btConvexHullShape*>(bt_shape.get())->addPoint(btVector3(vertices(0,i),vertices(1,i),vertices(2,i)));
+      auto bt_convex_hull_shape = dynamic_cast<btConvexHullShape*>(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<geometry.points.cols(); i++){
-      dynamic_cast<btConvexHullShape*>(bt_shape.get())->addPoint(btVector3(geometry.points(0,i),geometry.points(1,i),geometry.points(2,i)));
+    auto bt_convex_hull_shape = dynamic_cast<btConvexHullShape*>(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;
   }
 

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");