Bullet dispatch in RBT

.gitmodules

@@ -88,3 +88,4 @@
 [submodule "externals/ipopt"]
 	path = externals/ipopt
 	url = https://github.com/RobotLocomotion/ipopt-mirror.git
+	ignore = untracked

CHANGELOG.md

@@ -16,11 +16,14 @@ Unreleased: changes on master, not yet released
 [//]: # "New functionality or APIs."
 ### Added
 
- - None
+ - [#2602][] Added `DRAKE_ASSERT` and `DRAKE_ABORT_UNLESS` macros to replace
+ built-in `assert()` macro.
+ - [#2621][] Added `DRAKE_DEPRECATED` macro for portable deprecation.
 
 [//]: # "Altered functionality or APIs."
 ### Changed
 
+ - [#2597][] Changed `RigidBodyTree::findLink()` to be `RigidBodyTree::FindBody()`.
  - [#2426][] Changed `RigidBodyTree::findLinkId()` to be `RigidBodyTree::FindBodyIndex()`. Updated APIs of `RigidBodyTree`, `RigidBody`, `RigidBodyTree`, and `RigidBodyFrame` to support notion of a "model ID" that uniquely identifies a model within a `RigidBodySystem`. This enables the same SDF file to be loaded multiple times into the same `RigidBodySystem`.
  - [#2303][] The following member variables should now be accessed via accessor methods: `RigidBody::linkname`, `RigidBody::model_name`, `RigidBodyTree::num_positions`, and `RigidBodyTree::num_velocities`.
  - [#2325][] The following member variable should now be accessed via an accessor method: `RigidBodyConstraint::robot`.
@@ -33,6 +36,7 @@ Unreleased: changes on master, not yet released
 [//]: # "Lost functionality or APIs."
 ### Removed / Deprecated
 
+ - [#2610][] RigidBodyTree::getContactPositions and ::getContactPositionsJac are removed.
  - [#2102][] Macports and Cygwin are no longer supported.
  - [#2067][] `core/Core.h` is removed.
  - [#2039][] Ubuntu 12.04 Trusty is no longer supported.
@@ -70,3 +74,6 @@ Changes in version v0.9.11 and before are not provided.
 [#2303]: https://github.com/RobotLocomotion/drake/issues/2303
 [#2325]: https://github.com/RobotLocomotion/drake/issues/2325
 [#2426]: https://github.com/RobotLocomotion/drake/issues/2426
+[#2597]: https://github.com/RobotLocomotion/drake/issues/2597
+[#2602]: https://github.com/RobotLocomotion/drake/issues/2602
+[#2621]: https://github.com/RobotLocomotion/drake/issues/2621

CMakeLists.txt

@@ -93,7 +93,11 @@ else()
   option(WITH_DIRECTOR "vtk-based visualization tool and robot user interface" ON) # not win32 yet.  it builds on windows, but requires manually installation of vtk, etc.  perhaps make a precompiled director pod (a bit like snopt)
   option(WITH_LIBBOT "simple open-gl visualizer + lcmgl for director" ON) # there is hope, but i spent a long time cleaning up c/c++ language errors trying to make msvc happy.. and still had a long way to go.
   option(WITH_NLOPT "nonlinear optimization solver" ON)
-  option(WITH_IPOPT "nonlinear optimization solver" ON)
+  # IPOPT is currently disabled on Mac due to MATLAB compatibility
+  # issues: https://github.com/RobotLocomotion/drake/issues/2578
+  if (NOT APPLE)
+    option(WITH_IPOPT "nonlinear optimization solver" ON)
+  endif()
   option(WITH_SWIG_MATLAB "A version of SWIG with MATLAB support" ON)
 endif()
 

drake/CMakeLists.txt

@@ -89,13 +89,7 @@ include_directories(BEFORE ${PROJECT_BINARY_DIR}/exports/)
 include_directories(BEFORE ${PROJECT_SOURCE_DIR}/..)
 
 if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror=all")
-
-  # TODO(#2372) These are warnings that we can't handle yet.
-  if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-missing-braces")
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-unused-private-field")
-  endif()
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror=all -Werror=ignored-qualifiers")
 elseif(MSVC)
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /wd4251")
   # disable warning C4251: e.g.'RigidBody::linkname' : class 'std::basic_string<_Elem,_Traits,_Ax>' needs to have dll-interface to be used by clients of class 'RigidBody'
@@ -299,6 +293,7 @@ endif()
 # Setup ROS if it is available on the local system
 find_package(roscpp QUIET)
 if(roscpp_FOUND)
+  set(ROS_FOUND ON)
   message(STATUS "Found roscpp installed at ${roscpp_DIR}, so building ROS-specific functionality.")
   include_directories(${roscpp_INCLUDE_DIRS})
 
@@ -317,13 +312,15 @@ if(roscpp_FOUND)
   find_package(tf REQUIRED)
   include_directories(${tf_INCLUDE_DIRS})
 else()
+  set(ROS_FOUND OFF)
   message(STATUS "Couldn't find roscpp so not building ROS-specific functionality. If you want ROS support, be sure to source the ROS setup.sh prior to building Drake.")
 endif()
 
 include_directories(thirdParty/spruce/include)
 include_directories(thirdParty/cimg)
 
 add_subdirectory(common)
+add_subdirectory(math)
 add_subdirectory(core)
 add_subdirectory(util)
 add_subdirectory(solvers)
@@ -334,7 +331,7 @@ add_subdirectory(thirdParty/tinyxml2)
 add_subdirectory(examples)
 add_subdirectory(bindings)
 
-if(roscpp_FOUND AND catkin_FOUND AND genmsg_FOUND AND std_msgs_FOUND AND sensor_msgs_FOUND AND ackermann_msgs_FOUND AND tf_FOUND)
+if(ROS_FOUND)
   add_subdirectory(ros)
 endif()
 
@@ -385,9 +382,9 @@ if(MATLAB_FOUND)
     PROPERTIES TIMEOUT 300)
 endif()
 
-option(BUILD_DOCUMENTATION "Build Doxygen and Sphinx documentation." OFF)
+option(ENABLE_DOCUMENTATION "Enable build target for Doxygen and Sphinx documentation." ON)
 
-if(BUILD_DOCUMENTATION)
+if(ENABLE_DOCUMENTATION)
   add_subdirectory(doc)
 endif()
 

drake/bindings/python/pydrake/test/testPR2IK.py

@@ -11,15 +11,15 @@
 # along that link's x axis
 robot.addFrame(
            pydrake.rbtree.RigidBodyFrame("r_hand_frame",
-                                         robot.findLink("r_gripper_palm_link"),
+                                         robot.FindBody("r_gripper_palm_link"),
                                          np.array([0.1, 0, 0]),
                                          np.array([0., 0, 0])))
 
 # Make sure attribute access works on bodies
 assert robot.world().name() == "world"
 
 hand_frame_id = robot.findFrame("r_hand_frame").frame_index
-base_body_id = robot.findLink('base_footprint').body_index
+base_body_id = robot.FindBody('base_footprint').body_index
 
 constraints = [
                # These three constraints ensure that the base of the robot is

drake/common/CMakeLists.txt

@@ -3,15 +3,23 @@
 #
 
 # List all source files used to build libdrakeCommon.
-set(sources nice_type_name.cc)
+set(sources
+  drake_assert.cc
+  nice_type_name.cc
+  )
 
 # List headers that should be installed with Drake so that they
 # are available elsewhere via #include "drake/common/xxx.h".
-set(installed_headers nice_type_name.h)
+set(installed_headers
+  drake_assert.h
+  drake_deprecated.h
+  nice_type_name.h
+  )
 
 # List headers that are needed by code here but should not
 # be exposed anywhere else.
-set(private_headers)
+set(private_headers
+  )
 
 # Create the library target and note its dependencies. Also generates
 # a header defining DRAKECOMMON_EXPORT for dealing with .dlls on Windows.

drake/common/drake_assert.cc

@@ -0,0 +1,24 @@
+#include "drake/common/drake_assert.h"
+
+#include <cstdlib>
+#include <iostream>
+
+namespace drake {
+namespace detail {
+
+void Abort(const char* condition, const char* func, const char* file,
+           int line) {
+  std::cerr << "abort: failure at " << file << ":" << line
+            << " in " << func << "()";
+  if (condition) {
+    std::cerr << ": assertion '" << condition << "' failed.";
+  } else {
+    std::cerr << ".";
+  }
+  std::cerr << std::endl;
+
+  std::abort();
+}
+
+}  // namespace detail
+}  // namespace drake

drake/common/drake_assert.h

@@ -0,0 +1,92 @@
+#pragma once
+
+#include <type_traits>
+
+#include "drake/drakeCommon_export.h"
+
+/// @file
+/// Provides Drake's assertion implementation.  This is intended to be used
+/// both within Drake and by other software.  Drake's asserts can be armed
+/// and disarmed independently from the system-wide asserts.
+
+#ifdef DRAKE_DOXYGEN_CXX
+/// @p DRAKE_ASSERT(condition) is similar to the built-in @p assert(condition)
+/// from the C++ system header @p <cassert>.  Unless Drake's assertions are
+/// disarmed by the pre-processor definitions listed below, @p DRAKE_ASSERT
+/// will evaluate @p condition and iff the value is false will ::abort() the
+/// program with a message showing at least the condition text, function name,
+/// file, and line.
+///
+/// Assertions are enabled or disabled using the following pre-processor macros:
+/// - If @p DRAKE_ENABLE_ASSERTS is defined, then @p DRAKE_ASSERT is armed.
+/// - If @p DRAKE_DISABLE_ASSERTS is defined, then @p DRAKE_ASSERT is disarmed.
+/// - If both macros are defined, then it is a compile-time error.
+/// - If neither are defined, then NDEBUG governs assertions as usual.
+///
+/// This header will define exactly one of either @p DRAKE_ASSERT_IS_ARMED or
+/// @p DRAKE_ASSERT_IS_DISARMED to indicate whether @p DRAKE_ASSERT is armed.
+///
+/// One difference versus the standard @p assert(condition) is that the
+/// @p condition within @p DRAKE_ASSERT is always syntax-checked, even if
+/// Drake's assertions are disarmed.
+///
+/// Treat @p DRAKE_ASSERT like a statement -- it must always be used
+/// in block scope, and must always be followed by a semicolon.
+#define DRAKE_ASSERT(condition)
+/// Evaluates @p condition and iff the value is false will ::abort() the
+/// program with a message showing at least the condition text, function name,
+/// file, and line.
+#define DRAKE_ABORT_UNLESS(condition)
+/// Like @p DRAKE_ABORT_UNLESS(false), except that the meaningless "false"
+/// condition test is not included in the message.
+#define DRAKE_ABORT()
+#else  //  DRAKE_DOXYGEN_CXX
+
+// Users should NOT set these; only this header should set them.
+#ifdef DRAKE_ASSERT_IS_ARMED
+# error Unexpected DRAKE_ASSERT_IS_ARMED defined.
+#endif
+#ifdef DRAKE_ASSERT_IS_DISARMED
+# error Unexpected DRAKE_ASSERT_IS_DISARMED defined.
+#endif
+
+// Decide whether Drake assertions are enabled.
+#if defined(DRAKE_ENABLE_ASSERTS) && defined(DRAKE_DISABLE_ASSERTS)
+# error Conflicting assertion toggles.
+#elif defined(DRAKE_ENABLE_ASSERTS)
+# define DRAKE_ASSERT_IS_ARMED
+#elif defined(DRAKE_DISABLE_ASSERTS) || defined(NDEBUG)
+# define DRAKE_ASSERT_IS_DISARMED
+#else
+# define DRAKE_ASSERT_IS_ARMED
+#endif
+
+namespace drake {
+namespace detail {
+// Abort the program with an error message.
+DRAKECOMMON_EXPORT
+void Abort(const char* condition, const char* func, const char* file, int line);
+}
+}
+
+#define DRAKE_ABORT()                                           \
+  ::drake::detail::Abort(nullptr, __func__, __FILE__, __LINE__)
+
+#define DRAKE_ABORT_UNLESS(condition)                                   \
+  do {                                                                  \
+    if (!(condition)) {                                                 \
+      ::drake::detail::Abort(#condition, __func__, __FILE__, __LINE__); \
+    }                                                                   \
+  } while (0)
+
+#ifdef DRAKE_ASSERT_IS_ARMED
+// Assertions are enabled.
+# define DRAKE_ASSERT(condition) DRAKE_ABORT_UNLESS(condition)
+#else
+// Assertions are disabled, so just typecheck the expression.
+# define DRAKE_ASSERT(condition) static_assert(                 \
+      std::is_convertible<decltype(condition), bool>::value,    \
+      "Assertion condition should be bool-convertible.")
+#endif
+
+#endif  // DRAKE_DOXYGEN_CXX

drake/common/drake_deprecated.h

@@ -0,0 +1,61 @@
+#pragma once
+
+/** @file
+Provides a portable macro for use in generating compile-time warnings for
+use of code that is permitted but discouraged. **/
+
+#ifdef DRAKE_DOXYGEN_CXX
+/** Use `DRAKE_DEPRECATED("message")` to discourage use of particular
+classes, typedefs, variables, non-static data members, functions, arguments,
+enumerations, and template specializations. A compile time warning will be
+issued displaying the given message, preceded by "DRAKE DEPRECATED: ".
+
+This is typically used for constructs that have been replaced by something
+better and it is good practice to suggest the appropriate replacement in the
+deprecation message. Deprecation warnings are conventionally used to convey to
+users that a feature they are depending on will be removed in a future release.
+
+Usage: @code
+  // Attribute comes *before* declaration of a deprecated function or variable;
+  // no semicolon is allowed.
+  DRAKE_DEPRECATED("f() is slow; use g() instead.")
+  int f(int arg);
+
+  // Attribute comes *after* struct, class, enum keyword.
+  class DRAKE_DEPRECATED("Use MyNewClass instead.")
+  MyClass {
+  };
+@endcode
+
+This feature is standard in C++14 compilers via the `[[deprecated]]` attribute,
+and this macro will generate the standard attribute when compiled with a
+C++14-compliant compiler. **/
+#define DRAKE_DEPRECATED(message)
+
+#else  // DRAKE_DOXYGEN_CXX
+
+/* C++14 introduces a standard way to mark deprecated declarations. Before
+that we can use non-standard compiler hacks. */
+#ifndef SWIG
+  /* Make sure warnings are enabled in VC++; it is a common Windows hack for
+  programmers to turn them off due to much inconvenient deprecation of useful
+  things by Microsoft. */
+  #if _MSC_VER
+    #pragma warning(default:4996)
+  #endif
+  /* Figure out the best form of deprecation for this compiler. */
+  #if __cplusplus >= 201402L
+    /* C++14 */
+    #define DRAKE_DEPRECATED(MSG) [[deprecated("\nDRAKE DEPRECATED: " MSG)]]
+  #elif _MSC_VER
+    #define DRAKE_DEPRECATED(MSG) \
+      __declspec(deprecated("\nDRAKE DEPRECATED: " MSG))
+  #else /* gcc or clang */
+    #define DRAKE_DEPRECATED(MSG) \
+      __attribute__((deprecated("\nDRAKE DEPRECATED: " MSG)))
+  #endif
+#else /* Swigging */
+  #define DRAKE_DEPRECATED(MSG)
+#endif
+
+#endif  // DRAKE_DOXYGEN_CXX

drake/common/eigen_types.h

@@ -0,0 +1,41 @@
+/// @file
+/// This file contains abbreviated definitions for certain specializations of
+/// Eigen::Matrix that are commonly used in Drake.
+
+#pragma once
+
+#include <Eigen/Dense>
+#include <unsupported/Eigen/AutoDiff>
+
+namespace drake {
+
+/// A column vector of size 3, templated on scalar type.
+template <typename Scalar>
+using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
+
+/// A column vector of any size, templated on scalar type.
+template <typename Scalar>
+using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+
+/// A matrix of 3 rows, dynamic columns, templated on scalar type.
+template <typename Scalar>
+using Matrix3X = Eigen::Matrix<Scalar, 3, Eigen::Dynamic>;
+
+/// A matrix of 4 rows, dynamic columns, templated on scalar type.
+template <typename Scalar>
+using Matrix4X = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>;
+
+/// A matrix of dynamic size, templated on scalar type.
+template <typename Scalar>
+using MatrixX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
+
+/// A column vector of dynamic size, up to a maximum of 73 elements.
+using VectorUpTo73d = Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 73, 1>;
+
+/// An autodiff variable with a dynamic number of partials, up to 73 maximum.
+using AutoDiffUpTo73d = Eigen::AutoDiffScalar<VectorUpTo73d>;
+
+/// An autodiff variable with a dynamic number of partials.
+using AutoDiffXd = Eigen::AutoDiffScalar<Eigen::VectorXd>;
+
+}  // namespace drake

drake/common/nice_type_name.cc

@@ -46,7 +46,7 @@ string drake::NiceTypeName::Demangle(const char* typeid_name) {
 string drake::NiceTypeName::Canonicalize(const string& demangled) {
   using SPair = std::pair<std::regex, string>;
   // These are applied in this order.
-  const std::array<SPair, 8> subs{
+  const std::array<SPair, 8> subs{{
     // Remove unwanted keywords and following space. (\b is word boundary.)
     SPair(std::regex("\\b(class|struct|enum|union) "), ""),
     // Tidy up anonymous namespace.
@@ -65,7 +65,7 @@ string drake::NiceTypeName::Canonicalize(const string& demangled) {
     SPair(std::regex("\\bstd::basic_string<char,std::char_traits<char>,"
                      "std::allocator<char>>"),
           "std::string")
-  };
+  }};
 
   string canonical(demangled);
   for (const auto& sp : subs) {