From cb7db4171b483a37225032169df9001140d70f5c Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Sun, 29 Oct 2017 10:08:23 -0400 Subject: [PATCH] Move resolve_center_of_pressure into its own file --- drake/multibody/BUILD.bazel | 10 ++++++ .../resolve_center_of_pressure.cc} | 32 ++----------------- drake/multibody/resolve_center_of_pressure.h | 15 +++++++++ drake/multibody/rigid_body_tree.cc | 2 +- drake/systems/controllers/BUILD.bazel | 1 + drake/systems/controllers/controlUtil.cpp | 1 + drake/util/BUILD.bazel | 1 - drake/util/CMakeLists.txt | 2 +- drake/util/drakeUtil.h | 9 ------ 9 files changed, 31 insertions(+), 42 deletions(-) rename drake/{util/drakeUtil.cpp => multibody/resolve_center_of_pressure.cc} (83%) create mode 100644 drake/multibody/resolve_center_of_pressure.h diff --git a/drake/multibody/BUILD.bazel b/drake/multibody/BUILD.bazel index ed9a86d6aab7..3d700ee548f7 100644 --- a/drake/multibody/BUILD.bazel +++ b/drake/multibody/BUILD.bazel @@ -83,6 +83,15 @@ drake_cc_library( ], ) +drake_cc_library( + name = "resolve_center_of_pressure", + srcs = ["resolve_center_of_pressure.cc"], + hdrs = ["resolve_center_of_pressure.h"], + deps = [ + "@eigen", + ], +) + drake_cc_library( name = "rigid_body_constraint", srcs = ["rigid_body_constraint.cc"], @@ -176,6 +185,7 @@ drake_cc_library( visibility = [], deps = [ ":kinematics_cache", + ":resolve_center_of_pressure", ":rigid_body", ":rigid_body_actuator", ":rigid_body_frame", diff --git a/drake/util/drakeUtil.cpp b/drake/multibody/resolve_center_of_pressure.cc similarity index 83% rename from drake/util/drakeUtil.cpp rename to drake/multibody/resolve_center_of_pressure.cc index 98e24caf3768..fef4866caf1d 100644 --- a/drake/util/drakeUtil.cpp +++ b/drake/multibody/resolve_center_of_pressure.cc @@ -1,36 +1,8 @@ -/* -* drakeUtil.cpp - * - * Created on: Jun 19, 2013 - * Author: russt - */ - -#include "drake/util/drakeUtil.h" +#include "drake/multibody/resolve_center_of_pressure.h" +#include #include -#include - -void baseZeroToBaseOne(std::vector& vec) { - for (std::vector::iterator iter = vec.begin(); iter != vec.end(); iter++) - (*iter)++; -} - -double angleAverage(double theta1, double theta2) { - // Computes the average between two angles by averaging points on the unit - // circle and taking the arctan of the result. - // see: http://en.wikipedia.org/wiki/Mean_of_circular_quantities - // theta1 is a scalar or column vector of angles (rad) - // theta2 is a scalar or column vector of angles (rad) - - double x_mean = 0.5 * (cos(theta1) + cos(theta2)); - double y_mean = 0.5 * (sin(theta1) + sin(theta2)); - - double angle_mean = atan2(y_mean, x_mean); - - return angle_mean; -} - template std::pair resolveCenterOfPressure( diff --git a/drake/multibody/resolve_center_of_pressure.h b/drake/multibody/resolve_center_of_pressure.h new file mode 100644 index 000000000000..d7d325d57ced --- /dev/null +++ b/drake/multibody/resolve_center_of_pressure.h @@ -0,0 +1,15 @@ +#pragma once + +#include + +#include + +// TODO(siyuan.feng): Cleanup the naming according to the style guide. + +template +std::pair resolveCenterOfPressure( + const Eigen::MatrixBase& torque, + const Eigen::MatrixBase& force, + const Eigen::MatrixBase& normal, + const Eigen::MatrixBase& point_on_contact_plane); diff --git a/drake/multibody/rigid_body_tree.cc b/drake/multibody/rigid_body_tree.cc index 10312d45ade2..01ed99767367 100644 --- a/drake/multibody/rigid_body_tree.cc +++ b/drake/multibody/rigid_body_tree.cc @@ -21,8 +21,8 @@ #include "drake/multibody/joints/fixed_joint.h" #include "drake/multibody/joints/floating_base_types.h" #include "drake/multibody/kinematics_cache-inl.h" +#include "drake/multibody/resolve_center_of_pressure.h" #include "drake/util/drakeGeometryUtil.h" -#include "drake/util/drakeUtil.h" using Eigen::AutoDiffScalar; using Eigen::Dynamic; diff --git a/drake/systems/controllers/BUILD.bazel b/drake/systems/controllers/BUILD.bazel index 08d4eb0f9d74..e078ffe4dd50 100644 --- a/drake/systems/controllers/BUILD.bazel +++ b/drake/systems/controllers/BUILD.bazel @@ -107,6 +107,7 @@ drake_cc_library( "//drake/math:autodiff", "//drake/math:expmap", "//drake/math:geometric_transform", + "//drake/multibody:resolve_center_of_pressure", "//drake/multibody:rigid_body_tree", "//drake/util", ], diff --git a/drake/systems/controllers/controlUtil.cpp b/drake/systems/controllers/controlUtil.cpp index eb37ec090a0b..9fb74c53ae2e 100644 --- a/drake/systems/controllers/controlUtil.cpp +++ b/drake/systems/controllers/controlUtil.cpp @@ -7,6 +7,7 @@ #include "drake/math/expmap.h" #include "drake/math/quaternion.h" #include "drake/math/rotation_matrix.h" +#include "drake/multibody/resolve_center_of_pressure.h" #include "drake/util/drakeGeometryUtil.h" #include "drake/util/drakeUtil.h" diff --git a/drake/util/BUILD.bazel b/drake/util/BUILD.bazel index 9b434c8c7c11..952fdc48cfb5 100644 --- a/drake/util/BUILD.bazel +++ b/drake/util/BUILD.bazel @@ -9,7 +9,6 @@ drake_cc_library( name = "util", srcs = [ "drakeGeometryUtil.cpp", - "drakeUtil.cpp", ], hdrs = [ "drakeGeometryUtil.h", diff --git a/drake/util/CMakeLists.txt b/drake/util/CMakeLists.txt index a9b2f21f3ecc..17d8ba015556 100644 --- a/drake/util/CMakeLists.txt +++ b/drake/util/CMakeLists.txt @@ -1,4 +1,4 @@ -add_library(drakeUtil drakeUtil.cpp drakeUtil.h) +add_library(drakeUtil drakeUtil.h) target_link_libraries(drakeUtil Eigen3::Eigen) drake_install_libraries(drakeUtil) drake_install_headers(drakeUtil.h) diff --git a/drake/util/drakeUtil.h b/drake/util/drakeUtil.h index ea503cff76ba..b6126cca0238 100644 --- a/drake/util/drakeUtil.h +++ b/drake/util/drakeUtil.h @@ -198,12 +198,3 @@ void addOffset(std::vector& v, const T& offset) { void baseZeroToBaseOne(std::vector& vec); double angleAverage(double theta1, double theta2); - -template -std::pair resolveCenterOfPressure( - const Eigen::MatrixBase& torque, - const Eigen::MatrixBase& force, - const Eigen::MatrixBase& normal, - const Eigen::MatrixBase& point_on_contact_plane); -