Move resolve_center_of_pressure into its own file

RobotLocomotion · Oct 29, 2017 · cb7db41 · cb7db41
1 parent f5186ca
commit cb7db41
Show file tree

Hide file tree

Showing 9 changed files with 31 additions and 42 deletions.
diff --git 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 → ...e/multibody/resolve_center_of_pressure.cc b/drake/util/drakeUtil.cpp → ...e/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 <limits>
 #include <stdexcept>
 
-#include <Eigen/Dense>
-
-void baseZeroToBaseOne(std::vector<int>& vec) {
-  for (std::vector<int>::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 <typename DerivedTorque, typename DerivedForce, typename DerivedNormal,
           typename DerivedPoint>
 std::pair<Eigen::Vector3d, double> resolveCenterOfPressure(

diff --git a/drake/multibody/resolve_center_of_pressure.h b/drake/multibody/resolve_center_of_pressure.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include <utility>
+
+#include <Eigen/Dense>
+
+// TODO(siyuan.feng): Cleanup the naming according to the style guide.
+
+template <typename DerivedTorque, typename DerivedForce, typename DerivedNormal,
+          typename DerivedPoint>
+std::pair<Eigen::Vector3d, double> resolveCenterOfPressure(
+    const Eigen::MatrixBase<DerivedTorque>& torque,
+    const Eigen::MatrixBase<DerivedForce>& force,
+    const Eigen::MatrixBase<DerivedNormal>& normal,
+    const Eigen::MatrixBase<DerivedPoint>& point_on_contact_plane);
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -198,12 +198,3 @@ void addOffset(std::vector<T>& v, const T& offset) {
 void baseZeroToBaseOne(std::vector<int>& vec);
 
 double angleAverage(double theta1, double theta2);
-
-template <typename DerivedTorque, typename DerivedForce, typename DerivedNormal,
-          typename DerivedPoint>
-std::pair<Eigen::Vector3d, double> resolveCenterOfPressure(
-    const Eigen::MatrixBase<DerivedTorque>& torque,
-    const Eigen::MatrixBase<DerivedForce>& force,
-    const Eigen::MatrixBase<DerivedNormal>& normal,
-    const Eigen::MatrixBase<DerivedPoint>& point_on_contact_plane);
-