still has some debugging info, but I believe it's resolved the issue

Nguyenski91 · Oct 7, 2015 · 17c6779 · 17c6779
1 parent c214683
commit 17c6779
Show file tree

Hide file tree

Showing 11 changed files with 36 additions and 37 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -88,7 +88,7 @@ set(bullet_GIT_TAG 4319ffd7e9251066d93064f5a8dab12e33dbe5e2)
 set(bullet_IS_PUBLIC TRUE)
 set(bullet_IS_CMAKE_POD TRUE)
 set(cmake_GIT_REPOSITORY https://github.com/RobotLocomotion/cmake.git)
-set(cmake_GIT_TAG 2c4ee11aa719ad548df7eaf3c7047a98c6e3e01c)
+set(cmake_GIT_TAG 7f3d95b5928b61cd84302838732d97104464d001)
 set(cmake_GIT_CLONE_DIR "${PROJECT_SOURCE_DIR}/drake/cmake")
 set(cmake_NO_BUILD TRUE)
 set(cmake_IS_PUBLIC TRUE)

diff --git a/drake/systems/plants/constructModelmex.cpp b/drake/systems/plants/constructModelmex.cpp
@@ -52,18 +52,18 @@ void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
     return;
   }
 
-  {
+  if (false) {  // just for debugging
 	  string joint_name("test");
 	  Isometry3d transform_to_parent_body = Isometry3d::Identity();
-	  Vector3d joint_axis;  joint_axis << 1.0, 0.0, 0.0; 
+	  Vector3d joint_axis;  joint_axis << 1.0, 0.0, 0.0;
 	  mexPrintf("creating quat joint\n");
 	  QuaternionFloatingJoint *quat_joint = new QuaternionFloatingJoint(joint_name, transform_to_parent_body);
 	  mexPrintf("destroying quat joint\n");
 	  delete quat_joint;
-//	  mexPrintf("creating revolute joint\n");
-//	  RevoluteJoint *revolute_joint = new RevoluteJoint(joint_name, transform_to_parent_body, joint_axis);
-//	  mexPrintf("destroying revolute joint\n");
-//	  delete revolute_joint;
+	  mexPrintf("creating revolute joint\n");
+	  RevoluteJoint *revolute_joint = new RevoluteJoint(joint_name, transform_to_parent_body, joint_axis);
+	  mexPrintf("destroying revolute joint\n");
+	  delete revolute_joint;
 	  mexPrintf("returning\n");
 	  return;
   }
@@ -81,7 +81,7 @@ void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
   const mxArray* pFrames = mxGetPropertySafe(pRBM,0,"frame");
   if (!pFrames) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the frame array is invalid");
   int num_frames = static_cast<int>(mxGetNumberOfElements(pFrames));
-  
+
   for (int i = 0; i < num_bodies; i++) {
     //DEBUG
     //mexPrintf("constructModelmex: body %d\n",i);
@@ -142,9 +142,9 @@ void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
 		case 0: {
 			if (pitch == 0.0) {
 				RevoluteJoint *revolute_joint = new RevoluteJoint(joint_name, transform_to_parent_body, joint_axis);
-//				joint = std::unique_ptr<RevoluteJoint>(revolute_joint);
-//				setLimits(pBodies, i, revolute_joint);
-//				setDynamics(pBodies, i, revolute_joint);
+				joint = std::unique_ptr<RevoluteJoint>(revolute_joint);
+				setLimits(pBodies, i, revolute_joint);
+				setDynamics(pBodies, i, revolute_joint);
 			} else if (std::isinf(static_cast<double>(pitch))) {
 				PrismaticJoint *prismatic_joint = new PrismaticJoint(joint_name, transform_to_parent_body, joint_axis);
 				joint = std::unique_ptr<PrismaticJoint>(prismatic_joint);
@@ -169,7 +169,7 @@ void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
 			throw std::runtime_error(stream.str());
         }
 
-//		b->setJoint(move(joint));
+		b->setJoint(move(joint));
       }
     }
 
@@ -178,7 +178,7 @@ void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
     //END_DEBUG
     pm = mxGetPropertySafe(pBodies,i,"collision_geometry");
     Matrix4d T;
-    if (false) { //!mxIsEmpty(pm)){
+    if (!mxIsEmpty(pm)) {
       for (int j=0; j<mxGetNumberOfElements(pm); j++) {
         //DEBUG
         //cout << "constructModelmex: Body " << i << ", Element " << j << endl;
@@ -245,7 +245,7 @@ void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
             break;
           default:
             // intentionally do nothing..
-            
+
             //DEBUG
             //cout << "constructModelmex: SHOULD NOT GET HERE" << endl;
             //END_DEBUG
@@ -316,7 +316,7 @@ void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
   //cout << "constructModelmex: Get struct" << endl;
   //END_DEBUG
   mxArray* contact_pts_struct[1];
-  if (false) { //~mexCallMATLAB(1,contact_pts_struct,1,const_cast<mxArray**>(&pRBM),"getTerrainContactPoints")) {
+  if (!mexCallMATLAB(1,contact_pts_struct,1,const_cast<mxArray**>(&pRBM),"getTerrainContactPoints")) {
     //DEBUG
     //mexPrintf("constructModelmex: Got terrain contact points struct\n");
     //if (contact_pts_struct) {

diff --git a/drake/systems/plants/joints/DrakeJoint.cpp b/drake/systems/plants/joints/DrakeJoint.cpp
@@ -11,8 +11,8 @@ DrakeJoint::DrakeJoint(
         joint_limit_min(VectorXd::Constant(_num_positions, -std::numeric_limits<double>::infinity())),
         joint_limit_max(VectorXd::Constant(_num_positions, std::numeric_limits<double>::infinity()))
 {
-//  assert(num_positions <= MAX_NUM_POSITIONS);
-//  assert(num_velocities <= MAX_NUM_VELOCITIES);
+  assert(num_positions <= MAX_NUM_POSITIONS);
+  assert(num_velocities <= MAX_NUM_VELOCITIES);
 }
 
 DrakeJoint::~DrakeJoint()
@@ -49,4 +49,3 @@ const Eigen::VectorXd& DrakeJoint::getJointLimitMax() const
 {
   return joint_limit_max;
 }
-
diff --git a/drake/systems/plants/joints/DrakeJointImpl.h b/drake/systems/plants/joints/DrakeJointImpl.h
@@ -4,22 +4,22 @@
 #include "DrakeJoint.h"
 
 #define POSITION_AND_VELOCITY_DEPENDENT_METHODS_IMPL(Scalar) \
-  virtual Eigen::Transform<Scalar, 3, Eigen::Isometry> jointTransform(const Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> > &q) const override { return derived->jointTransform(q); }; \
-  virtual void motionSubspace(const Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> > &q, Eigen::Matrix<Scalar, TWIST_SIZE, Eigen::Dynamic, 0, TWIST_SIZE, MAX_NUM_VELOCITIES> &motion_subspace, Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> *dmotion_subspace = nullptr) const override { derived->motionSubspace(q, motion_subspace, dmotion_subspace); }; \
-  virtual void motionSubspaceDotTimesV(const Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> &q, const Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> &v, Eigen::Matrix<Scalar, 6, 1> &motion_subspace_dot_times_v, typename Gradient<Eigen::Matrix<Scalar, 6, 1>, Eigen::Dynamic>::type *dmotion_subspace_dot_times_vdq = nullptr, typename Gradient<Eigen::Matrix<Scalar, 6, 1>, Eigen::Dynamic>::type *dmotion_subspace_dot_times_vdv = nullptr) const override { derived->motionSubspaceDotTimesV(q ,v, motion_subspace_dot_times_v, dmotion_subspace_dot_times_vdq, dmotion_subspace_dot_times_vdv); }; \
-  virtual void qdot2v(const Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> > &q, Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, 0, MAX_NUM_VELOCITIES, MAX_NUM_POSITIONS> &qdot_to_v, Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> *dqdot_to_v) const override { derived->qdot2v(q, qdot_to_v, dqdot_to_v); }; \
-  virtual void v2qdot(const Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> &q, Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, 0, MAX_NUM_POSITIONS, MAX_NUM_VELOCITIES> &v_to_qdot, Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> *dv_to_qdot) const override { derived->v2qdot(q, v_to_qdot, dv_to_qdot); }; \
-  virtual GradientVar<Scalar, Eigen::Dynamic, 1> frictionTorque(const Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>>& v, int gradient_order) const { return derived->frictionTorque(v, gradient_order); };
+  virtual Eigen::Transform<Scalar, 3, Eigen::Isometry> jointTransform(const Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> > &q) const override { return derived.jointTransform(q); }; \
+  virtual void motionSubspace(const Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> > &q, Eigen::Matrix<Scalar, TWIST_SIZE, Eigen::Dynamic, 0, TWIST_SIZE, MAX_NUM_VELOCITIES> &motion_subspace, Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> *dmotion_subspace = nullptr) const override { derived.motionSubspace(q, motion_subspace, dmotion_subspace); }; \
+  virtual void motionSubspaceDotTimesV(const Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> &q, const Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> &v, Eigen::Matrix<Scalar, 6, 1> &motion_subspace_dot_times_v, typename Gradient<Eigen::Matrix<Scalar, 6, 1>, Eigen::Dynamic>::type *dmotion_subspace_dot_times_vdq = nullptr, typename Gradient<Eigen::Matrix<Scalar, 6, 1>, Eigen::Dynamic>::type *dmotion_subspace_dot_times_vdv = nullptr) const override { derived.motionSubspaceDotTimesV(q ,v, motion_subspace_dot_times_v, dmotion_subspace_dot_times_vdq, dmotion_subspace_dot_times_vdv); }; \
+  virtual void qdot2v(const Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> > &q, Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, 0, MAX_NUM_VELOCITIES, MAX_NUM_POSITIONS> &qdot_to_v, Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> *dqdot_to_v) const override { derived.qdot2v(q, qdot_to_v, dqdot_to_v); }; \
+  virtual void v2qdot(const Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> &q, Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, 0, MAX_NUM_POSITIONS, MAX_NUM_VELOCITIES> &v_to_qdot, Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> *dv_to_qdot) const override { derived.v2qdot(q, v_to_qdot, dv_to_qdot); }; \
+  virtual GradientVar<Scalar, Eigen::Dynamic, 1> frictionTorque(const Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>>& v, int gradient_order) const { return derived.frictionTorque(v, gradient_order); };
 
 template <typename Derived>
 class DrakeJointImpl : public DrakeJoint
 {
 private:
-  Derived* derived;
+  Derived& derived;
 
 public:
-	DrakeJointImpl(Derived* _derived, const std::string& name, const Eigen::Isometry3d& transform_to_parent_body, int num_positions,
-		int num_velocities) : DrakeJoint(name, transform_to_parent_body, num_positions, num_velocities), derived(_derived) {}; 
+	DrakeJointImpl(Derived& _derived, const std::string& name, const Eigen::Isometry3d& transform_to_parent_body, int num_positions,
+		int num_velocities) : DrakeJoint(name, transform_to_parent_body, num_positions, num_velocities), derived(_derived) {};
   virtual ~DrakeJointImpl() {};
 
   POSITION_AND_VELOCITY_DEPENDENT_METHODS_IMPL(double)

diff --git a/drake/systems/plants/joints/FixedAxisOneDoFJoint.h b/drake/systems/plants/joints/FixedAxisOneDoFJoint.h
@@ -24,7 +24,7 @@ class FixedAxisOneDoFJoint : public DrakeJointImpl<Derived>
   double coulomb_window;
 
 protected:
-  FixedAxisOneDoFJoint(Derived* derived, const std::string& name, const Eigen::Isometry3d& transform_to_parent_body, const Eigen::Matrix<double, TWIST_SIZE, 1>& _joint_axis) :
+  FixedAxisOneDoFJoint(Derived& derived, const std::string& name, const Eigen::Isometry3d& transform_to_parent_body, const Eigen::Matrix<double, TWIST_SIZE, 1>& _joint_axis) :
       DrakeJointImpl<Derived>(derived, name, transform_to_parent_body, 1, 1),
       joint_axis(_joint_axis),
       damping(0.0),

diff --git a/drake/systems/plants/joints/FixedJoint.h b/drake/systems/plants/joints/FixedJoint.h
@@ -13,7 +13,7 @@
 class DLLEXPORT_DRAKEJOINT FixedJoint: public DrakeJointImpl<FixedJoint> {
 public:
   FixedJoint(const std::string &name, const Eigen::Isometry3d &transform_to_parent_body)
-          : DrakeJointImpl(this, name, transform_to_parent_body, 0, 0) { };
+          : DrakeJointImpl(*this, name, transform_to_parent_body, 0, 0) { };
 
   virtual ~FixedJoint() { };
 
@@ -81,7 +81,7 @@ class DLLEXPORT_DRAKEJOINT FixedJoint: public DrakeJointImpl<FixedJoint> {
 
     virtual std::string getPositionName(int index) const;
     virtual Eigen::VectorXd randomConfiguration(std::default_random_engine& generator) const;
-    
+
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };

diff --git a/drake/systems/plants/joints/HelicalJoint.h b/drake/systems/plants/joints/HelicalJoint.h
@@ -15,7 +15,7 @@ class DLLEXPORT_DRAKEJOINT HelicalJoint: public FixedAxisOneDoFJoint<HelicalJoin
 
 public:
   HelicalJoint(const std::string& name, const Eigen::Isometry3d& transform_to_parent_body, const Eigen::Vector3d& axis, double pitch) :
-      FixedAxisOneDoFJoint<HelicalJoint>(this, name, transform_to_parent_body, spatialJointAxis(axis, pitch)), axis(axis), pitch(pitch) { };
+      FixedAxisOneDoFJoint<HelicalJoint>(*this, name, transform_to_parent_body, spatialJointAxis(axis, pitch)), axis(axis), pitch(pitch) { };
 
   virtual ~HelicalJoint() { };
 

diff --git a/drake/systems/plants/joints/PrismaticJoint.h b/drake/systems/plants/joints/PrismaticJoint.h
@@ -14,7 +14,7 @@ class DLLEXPORT_DRAKEJOINT PrismaticJoint: public FixedAxisOneDoFJoint<Prismatic
 public:
 
   PrismaticJoint(const std::string& name, const Eigen::Isometry3d& transform_to_parent_body, const Eigen::Vector3d& translation_axis) :
-      FixedAxisOneDoFJoint<PrismaticJoint>(this, name, transform_to_parent_body, spatialJointAxis(translation_axis)), translation_axis(translation_axis) {
+      FixedAxisOneDoFJoint<PrismaticJoint>(*this, name, transform_to_parent_body, spatialJointAxis(translation_axis)), translation_axis(translation_axis) {
     assert(std::abs(translation_axis.norm() - 1.0) < 1e-10);
   };
 

diff --git a/drake/systems/plants/joints/QuaternionFloatingJoint.h b/drake/systems/plants/joints/QuaternionFloatingJoint.h
@@ -12,7 +12,7 @@ class DLLEXPORT_DRAKEJOINT QuaternionFloatingJoint: public DrakeJointImpl<Quater
 
 public:
   QuaternionFloatingJoint(const std::string & name, const Eigen::Isometry3d & transform_to_parent_body) :
-          DrakeJointImpl(this, name, transform_to_parent_body, 7, 6) { };
+          DrakeJointImpl(*this, name, transform_to_parent_body, 7, 6) { };
 
   virtual ~QuaternionFloatingJoint() { };
 

diff --git a/drake/systems/plants/joints/RevoluteJoint.h b/drake/systems/plants/joints/RevoluteJoint.h
@@ -14,10 +14,10 @@ class DLLEXPORT_DRAKEJOINT RevoluteJoint : public FixedAxisOneDoFJoint<RevoluteJ
   Eigen::Vector3d rotation_axis;
 public:
   RevoluteJoint(const std::string& name, const Eigen::Isometry3d& transform_to_parent_body, const Eigen::Vector3d& _rotation_axis) :
-      FixedAxisOneDoFJoint<RevoluteJoint>(this, name, transform_to_parent_body, spatialJointAxis(_rotation_axis)), 
+      FixedAxisOneDoFJoint<RevoluteJoint>(*this, name, transform_to_parent_body, spatialJointAxis(_rotation_axis)),
 	  rotation_axis(_rotation_axis)
   {
-//    assert(std::abs(rotation_axis.norm() - 1.0) < 1e-10);
+    assert(std::abs(rotation_axis.norm() - 1.0) < 1e-10);
   };
 
   virtual ~RevoluteJoint() {};

diff --git a/drake/systems/plants/joints/RollPitchYawFloatingJoint.h b/drake/systems/plants/joints/RollPitchYawFloatingJoint.h
@@ -11,8 +11,8 @@ class DLLEXPORT_DRAKEJOINT RollPitchYawFloatingJoint: public DrakeJointImpl<Roll
   //RollPitchYawFloatingJoint& operator=(const RollPitchYawFloatingJoint&) = delete;
 
 public:
-  RollPitchYawFloatingJoint(const std::string& name, const Eigen::Isometry3d& transform_to_parent_body) : 
-	  DrakeJointImpl(this, name, transform_to_parent_body, 6, 6) { };
+  RollPitchYawFloatingJoint(const std::string& name, const Eigen::Isometry3d& transform_to_parent_body) :
+	  DrakeJointImpl(*this, name, transform_to_parent_body, 6, 6) { };
 
   virtual ~RollPitchYawFloatingJoint() { };