Merge branch 'master' into convertJointTrajectory

CMakeLists.txt

@@ -152,7 +152,6 @@ include(CompilerSettings)
 #==============================================================================
 # Libraries and unit tests.
 #
-
 configure_file(
   ${CMAKE_MODULE_PATH}/version.hpp.in
   ${CMAKE_CURRENT_BINARY_DIR}/include/aikido/version.hpp

cmake/CompilerSettings.cmake

@@ -17,6 +17,7 @@ if(CMAKE_COMPILER_IS_GNUCXX)
 
   set(AIKIDO_CXX_STANDARD_FLAGS -std=c++11)
 
+  add_compile_options(-Wall -Wextra -Wpedantic)
   if(TREAT_WARNINGS_AS_ERRORS)
     add_compile_options(-Werror)
   endif()
@@ -33,6 +34,7 @@ elseif("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
 
   set(AIKIDO_CXX_STANDARD_FLAGS -std=c++11)
 
+  add_compile_options(-Wall -Wextra -Wpedantic)
   if(TREAT_WARNINGS_AS_ERRORS)
     add_compile_options(-Werror)
   endif()
@@ -49,6 +51,7 @@ elseif("${CMAKE_CXX_COMPILER_ID}" STREQUAL "AppleClang")
 
   set(AIKIDO_CXX_STANDARD_FLAGS -std=c++11)
 
+  add_compile_options(-Wall -Wextra -Wpedantic)
   if(TREAT_WARNINGS_AS_ERRORS)
     add_compile_options(-Werror)
   endif()
@@ -64,6 +67,7 @@ elseif(MSVC)
    # C++11 (e.g., -std=c++11) since Visual Studio enables it by default when
    # available.
 
+  add_compile_options(/Wall)
   if(TREAT_WARNINGS_AS_ERRORS)
     add_compile_options(/WX)
   endif()

include/aikido/constraint/NewtonsMethodProjectable.hpp

@@ -38,10 +38,10 @@ class NewtonsMethodProjectable : public Projectable
 
 private:
   DifferentiablePtr mDifferentiable;
-  statespace::StateSpacePtr mStateSpace;
-  int mMaxIteration;
   std::vector<double> mTolerance;
+  int mMaxIteration;
   double mMinStepSize;
+  statespace::StateSpacePtr mStateSpace;
 
   bool contains(const statespace::StateSpace::State* _s) const;
 };

include/aikido/constraint/detail/JointStateSpaceHelpers-impl.hpp

@@ -303,7 +303,7 @@ struct createDifferentiableFor_impl<statespace::dart::SE2Joint>
   using StateSpace = statespace::dart::SE2Joint;
   using StateSpacePtr = std::shared_ptr<StateSpace>;
 
-  static std::unique_ptr<Differentiable> create(StateSpacePtr _stateSpace)
+  static std::unique_ptr<Differentiable> create(StateSpacePtr /*_stateSpace*/)
   {
     throw std::runtime_error(
       "No DifferentiableConstraint is available for SE2Joint.");
@@ -317,7 +317,7 @@ struct createTestableFor_impl<statespace::dart::SE2Joint>
   using StateSpace = statespace::dart::SE2Joint;
   using StateSpacePtr = std::shared_ptr<StateSpace>;
 
-  static std::unique_ptr<Testable> create(StateSpacePtr _stateSpace)
+  static std::unique_ptr<Testable> create(StateSpacePtr /*_stateSpace*/)
   {
     throw std::runtime_error(
       "No Testable is available for SE2Joint.");
@@ -331,7 +331,7 @@ struct createProjectableFor_impl<statespace::dart::SE2Joint>
   using StateSpace = statespace::dart::SE2Joint;
   using StateSpacePtr = std::shared_ptr<StateSpace>;
 
-  static std::unique_ptr<Projectable> create(StateSpacePtr _stateSpace)
+  static std::unique_ptr<Projectable> create(StateSpacePtr /*_stateSpace*/)
   {
     throw std::runtime_error(
       "No Projectable is available for SE2Joint.");
@@ -346,7 +346,7 @@ struct createSampleableFor_impl<statespace::dart::SE2Joint>
   using StateSpacePtr = std::shared_ptr<StateSpace>;
 
   static std::unique_ptr<Sampleable> create(
-    StateSpacePtr _stateSpace, std::unique_ptr<util::RNG> _rng)
+    StateSpacePtr /*_stateSpace*/, std::unique_ptr<util::RNG> /*_rng*/)
   {
     throw std::runtime_error(
       "No Sampleable is available for SE2Joint.");
@@ -360,7 +360,7 @@ struct createDifferentiableFor_impl<statespace::dart::SE3Joint>
   using StateSpace = statespace::dart::SE3Joint;
   using StateSpacePtr = std::shared_ptr<StateSpace>;
 
-  static std::unique_ptr<Differentiable> create(StateSpacePtr _stateSpace)
+  static std::unique_ptr<Differentiable> create(StateSpacePtr /*_stateSpace*/)
   {
     throw std::runtime_error(
       "No DifferentiableConstraint is available for SE3Joint.");
@@ -374,7 +374,7 @@ struct createTestableFor_impl<statespace::dart::SE3Joint>
   using StateSpace = statespace::dart::SE3Joint;
   using StateSpacePtr = std::shared_ptr<StateSpace>;
 
-  static std::unique_ptr<Testable> create(StateSpacePtr _stateSpace)
+  static std::unique_ptr<Testable> create(StateSpacePtr /*_stateSpace*/)
   {
     throw std::runtime_error(
       "No Testable is available for SE3Joint.");
@@ -388,7 +388,7 @@ struct createProjectableFor_impl<statespace::dart::SE3Joint>
   using StateSpace = statespace::dart::SE3Joint;
   using StateSpacePtr = std::shared_ptr<StateSpace>;
 
-  static std::unique_ptr<Projectable> create(StateSpacePtr _stateSpace)
+  static std::unique_ptr<Projectable> create(StateSpacePtr /*_stateSpace*/)
   {
     throw std::runtime_error(
       "No Projectable is available for SE3Joint.");
@@ -403,7 +403,7 @@ struct createSampleableFor_impl<statespace::dart::SE3Joint>
   using StateSpacePtr = std::shared_ptr<StateSpace>;
 
   static std::unique_ptr<Sampleable> create(
-    StateSpacePtr _stateSpace, std::unique_ptr<util::RNG> _rng)
+    StateSpacePtr /*_stateSpace*/, std::unique_ptr<util::RNG> /*_rng*/)
   {
     throw std::runtime_error(
       "No Sampleable is available for SE3Joint.");

include/aikido/distance/SO2Angular.hpp

@@ -29,6 +29,6 @@ class SO2Angular : public DistanceMetric
 private:
   std::shared_ptr<statespace::SO2> mStateSpace;
 };
-};
+}
 }
 #endif

include/aikido/distance/SO3Angular.hpp

@@ -29,6 +29,6 @@ class SO3Angular : public DistanceMetric
 private:
   std::shared_ptr<statespace::SO3> mStateSpace;
 };
-};
+}
 }
 #endif

include/aikido/perception/eigen_yaml.hpp

@@ -16,8 +16,6 @@ inline void deserialize(
 {
     typedef Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> MatrixType;
     typedef typename MatrixType::Index Index;
-    typedef typename MatrixType::Scalar Scalar;
-
 
     using boost::format;
     using boost::str;
@@ -187,4 +185,4 @@ inline void operator>>(Node const &node, T &value)
 
 } // namespace YAML
 
-#endif // ifndef YAML_UTILS_H_
+#endif // ifndef YAML_UTILS_H_

include/aikido/planner/ompl/GeometricStateSpace.hpp

@@ -8,6 +8,7 @@
 #include "../../distance/DistanceMetric.hpp"
 #include "../../statespace/StateSpace.hpp"
 #include "../../statespace/GeodesicInterpolator.hpp"
+#include "aikido/planner/ompl/BackwardCompatibility.hpp"
 
 namespace aikido {
 namespace planner {
@@ -64,8 +65,12 @@ class GeometricStateSpace : public ::ompl::base::StateSpace
   /// For unbounded state spaces, this function can return infinity.
   double getMaximumExtent() const override;
 
+#if OMPL_VERSION_AT_LEAST(1,0,0)
   /// Get a measure of the space.
   double getMeasure() const override;
+#else
+  double getMeasure() const;
+#endif
 
   /// Bring the state within the bounds of the state space using the
   /// boundsProjection defined in the constructor.

include/aikido/rviz/ResourceServer.hpp

@@ -10,8 +10,8 @@ namespace rviz {
 
 struct BinaryResource {
   BinaryResource()
-    : mData(nullptr)
-    , mSize(0)
+    : mSize(0)
+    , mData(nullptr)
   {
   }
 

include/aikido/util/Spline.hpp

@@ -163,7 +163,7 @@ class SplineND
   SolutionMatrices mSolution;
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(TimeVector::NeedsToAlign);
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(TimeVector::NeedsToAlign) 
 };
 
 
@@ -330,7 +330,7 @@ class SplineProblem
     || TimeVector::NeedsToAlign
     || ProblemMatrix::NeedsToAlign
     || ProblemVector::NeedsToAlign
-  );
+  ) 
 };
 
 } // namespace util

include/aikido/util/StepSequence.hpp

@@ -54,7 +54,7 @@ class StepSequence::const_iterator
   bool equal(const StepSequence::const_iterator &other) const;
 
 private:
-  friend StepSequence;
+  friend class StepSequence;
 
   const_iterator(StepSequence *seq, int step)
       : mSeq(seq)

src/constraint/CartesianProductTestable.cpp

@@ -17,7 +17,7 @@ CartesianProductTestable::CartesianProductTestable(
   if (!mStateSpace)
     throw std::invalid_argument("_stateSpace is nullptr.");
 
-  for(int i = 0; i < mConstraints.size(); ++i)
+  for(size_t i = 0; i < mConstraints.size(); ++i)
   {
     if (!mConstraints[i])
     {

src/constraint/FiniteSampleable.cpp

@@ -95,7 +95,7 @@ statespace::StateSpacePtr FiniteSampleGenerator::getStateSpace() const
 //=============================================================================
 bool FiniteSampleGenerator::sample(statespace::StateSpace::State* _state)
 {
-  if (mStates.size() <= mIndex)
+  if (mStates.size() <= static_cast<size_t>(mIndex))
     return false;
 
   mStateSpace->copyState(mStates[mIndex], _state);
@@ -113,7 +113,7 @@ int FiniteSampleGenerator::getNumSamples() const
 //=============================================================================
 bool FiniteSampleGenerator::canSample() const
 {
-  return mStates.size() > mIndex;
+  return mStates.size() > static_cast<size_t>(mIndex); 
 }
 
 //=============================================================================

src/constraint/NewtonsMethodProjectable.cpp

@@ -76,7 +76,6 @@ bool NewtonsMethodProjectable::project(
   statespace::StateSpace::State* _out) const
 {
   using StateSpace = statespace::StateSpace;
-  using State = StateSpace::State;
 
   int iteration = 0;
 

src/constraint/Satisfied.cpp

@@ -32,7 +32,7 @@ std::vector<constraint::ConstraintType> Satisfied
 
 //=============================================================================
 bool Satisfied::isSatisfied(
-  const statespace::StateSpace::State* state) const
+  const statespace::StateSpace::State* /*state*/) const
 {
   return true;
 }
@@ -48,14 +48,14 @@ bool Satisfied::project(
 
 //=============================================================================
 void Satisfied::getValue(
-  const statespace::StateSpace::State* _s, Eigen::VectorXd& _out) const
+  const statespace::StateSpace::State* /*_s*/, Eigen::VectorXd& _out) const
 {
   _out = Eigen::Matrix<double, 0, 1>();
 }
 
 //=============================================================================
 void Satisfied::getJacobian(
-  const statespace::StateSpace::State* _s,
+  const statespace::StateSpace::State* /*_s*/,
   Eigen::MatrixXd& _out) const
 {
   _out = Eigen::Matrix<double, 0, 0>();

src/constraint/TSR.cpp

@@ -80,57 +80,58 @@ class TSRSampleGenerator : public SampleGenerator
 //=============================================================================
 TSR::TSR(std::unique_ptr<util::RNG> _rng, const Eigen::Isometry3d& _T0_w,
          const Eigen::Matrix<double, 6, 2>& _Bw, const Eigen::Isometry3d& _Tw_e)
-    : mRng(std::move(_rng))
-    , mStateSpace(std::make_shared<SE3>())
-    , mT0_w(_T0_w)
+    : mT0_w(_T0_w)
     , mBw(_Bw)
     , mTw_e(_Tw_e)
+    , mRng(std::move(_rng))
+    , mStateSpace(std::make_shared<SE3>())
 {
   validate();
 }
 
 //=============================================================================
 TSR::TSR(const Eigen::Isometry3d& _T0_w, const Eigen::Matrix<double, 6, 2>& _Bw,
          const Eigen::Isometry3d& _Tw_e)
-    : mRng(std::unique_ptr<util::RNG>(
-          new util::RNGWrapper<std::default_random_engine>(0)))
-    , mStateSpace(std::make_shared<SE3>())
-    , mT0_w(_T0_w)
+    : mT0_w(_T0_w)
     , mBw(_Bw)
     , mTw_e(_Tw_e)
+    , mRng(std::unique_ptr<util::RNG>(
+          new util::RNGWrapper<std::default_random_engine>(0)))
+    , mStateSpace(std::make_shared<SE3>())
 {
   validate();
 }
 
 //=============================================================================
 TSR::TSR(const TSR& other)
-    : mRng(std::move(other.mRng->clone()))
-    , mStateSpace(std::make_shared<SE3>())
-    , mT0_w(other.mT0_w)
-    , mTw_e(other.mTw_e)
+    : mT0_w(other.mT0_w)
     , mBw(other.mBw)
+    , mTw_e(other.mTw_e)
+    , mRng(std::move(other.mRng->clone()))
+    , mStateSpace(std::make_shared<SE3>())
 {
   validate();
 }
 
 //=============================================================================
 TSR::TSR(TSR&& other)
-    : mRng(std::move(other.mRng))
-    , mStateSpace(std::make_shared<SE3>())
-    , mT0_w(other.mT0_w)
-    , mTw_e(other.mTw_e)
+    : mT0_w(other.mT0_w)
     , mBw(other.mBw)
+    , mTw_e(other.mTw_e)
+    , mRng(std::move(other.mRng))
+    , mStateSpace(std::make_shared<SE3>())
 {
   validate();
 }
 
 //=============================================================================
 TSR& TSR::operator=(const TSR& other)
 {
-  mRng = std::move(other.mRng->clone());
+
   mT0_w = other.mT0_w;
-  mTw_e = other.mTw_e;
   mBw = other.mBw;
+  mTw_e = other.mTw_e;
+  mRng = std::move(other.mRng->clone());
 
   // Intentionally don't assign StateSpace.
 
@@ -140,11 +141,11 @@ TSR& TSR::operator=(const TSR& other)
 //=============================================================================
 TSR& TSR::operator=(TSR&& other)
 {
-  mRng = std::move(other.mRng);
-  mStateSpace = std::move(other.mStateSpace);
   mT0_w = std::move(other.mT0_w);
-  mTw_e = std::move(other.mTw_e);
   mBw = std::move(other.mBw);
+  mTw_e = std::move(other.mTw_e);
+  mRng = std::move(other.mRng);
+  mStateSpace = std::move(other.mStateSpace);
 
   return *this;
 }
@@ -320,8 +321,8 @@ std::vector<ConstraintType> TSR::getConstraintTypes() const
 }
 
 //=============================================================================
-bool TSR::project(const statespace::StateSpace::State* _s,
-  statespace::StateSpace::State* _out) const
+bool TSR::project(const statespace::StateSpace::State* /*_s*/,
+  statespace::StateSpace::State* /*_out*/) const
 {
   // TODO 
   return false;