Deprecate drake::SpatialForce (RobotLocomotion#11806)

attic/multibody/rigid_body_plant/contact_force.h

@@ -101,8 +101,8 @@ class ContactForce {
 
    @returns the resultant spatial force.
    */
-  SpatialForce <T> get_spatial_force() const {
-    SpatialForce<T> spatial_force;
+  Vector6<T> get_spatial_force() const {
+    Vector6<T> spatial_force;
     spatial_force.template head<3>() = torque_;
     spatial_force.template tail<3>() = force_;
     return spatial_force;

attic/multibody/rigid_body_plant/test/compliant_contact_model_test.cc

@@ -124,7 +124,7 @@ TEST_F(CompliantContactModelTestDouble, ModelSingleCollision) {
 
   // Confirms the contact details are as expected.
   const auto& resultant = info.get_resultant_force();
-  SpatialForce<double> expected_spatial_force;
+  Vector6<double> expected_spatial_force;
 
   // NOTE: the *effective* Young's modulus of the contact is half of the
   // material Young's modulus.
@@ -222,7 +222,7 @@ TEST_F(CompliantHeterogeneousModelTest, ModelSingleCollision) {
 
   // Confirms the contact details are as expected.
   const auto& resultant = info.get_resultant_force();
-  SpatialForce<double> expected_spatial_force;
+  Vector6<double> expected_spatial_force;
 
   // NOTE: Each sphere is moved toward the other offset distance which makes
   // the penetration depth 2 * offset.

attic/multibody/rigid_body_plant/test/compute_contact_result_test.cc

@@ -115,7 +115,7 @@ TEST_F(ContactResultTest, SingleCollision) {
 
   // Confirms the contact details are as expected.
   const auto& resultant = info.get_resultant_force();
-  SpatialForce<double> expected_spatial_force;
+  Vector6<double> expected_spatial_force;
   // Note: This is fragile. It assumes a particular collision model.  Once the
   // model has been generalized, this will have to adapt to account for that.
 

attic/multibody/rigid_body_plant/test/contact_force_formula_test.cc

@@ -164,7 +164,7 @@ TEST_F(ContactFormulaTest, ZeroVelocityCollision) {
   double expected_force_magnitude = kPenetrationDepth * youngs_modulus_;
   const auto info = contacts_.get_contact_info(0);
   const auto& resultant = info.get_resultant_force();
-  SpatialForce<double> expected_spatial_force;
+  Vector6<double> expected_spatial_force;
   // This force should be pushing sphere1 to the left.
   expected_spatial_force << 0, 0, 0, -expected_force_magnitude, 0, 0;
   ASSERT_TRUE(CompareMatrices(resultant.get_spatial_force(),
@@ -198,7 +198,7 @@ TEST_F(ConvergingContactFormulaTest, ConvergingContactTest) {
       kPenetrationDepth * youngs_modulus_ * (1 + dissipation_ * get_x_dot());
   const auto info = contacts_.get_contact_info(0);
   const auto& resultant = info.get_resultant_force();
-  SpatialForce<double> expected_spatial_force;
+  Vector6<double> expected_spatial_force;
   // This force should be pushing sphere1 to the left.
   expected_spatial_force << 0, 0, 0, -expected_force_magnitude, 0, 0;
   ASSERT_TRUE(CompareMatrices(resultant.get_spatial_force(),
@@ -235,7 +235,7 @@ TEST_F(DivergingContactFormulaTest, DivergingContactTest) {
       kPenetrationDepth * youngs_modulus_ * (1 + dissipation_ * get_x_dot());
   const auto info = contacts_.get_contact_info(0);
   const auto& resultant = info.get_resultant_force();
-  SpatialForce<double> expected_spatial_force;
+  Vector6<double> expected_spatial_force;
   // This force should be pushing to the left.
   expected_spatial_force << 0, 0, 0, -expected_force_magnitude, 0, 0;
   ASSERT_TRUE(CompareMatrices(resultant.get_spatial_force(),
@@ -305,7 +305,7 @@ TEST_F(StictionContactFormulaTest, StictionContactTest) {
   double expected_tangent_magnitude = expected_mu * expected_normal_magnitude;
   const auto info = contacts_.get_contact_info(0);
   const auto& resultant = info.get_resultant_force();
-  SpatialForce<double> expected_spatial_force;
+  Vector6<double> expected_spatial_force;
   // The normal component should be pointing to the left (-x) and the tangential
   // component should be pointing up (+y).
   expected_spatial_force << 0, 0, 0, -expected_normal_magnitude,
@@ -346,7 +346,7 @@ TEST_F(TransitionContactFormulaTest, TransitionContactTest) {
   double expected_tangent_magnitude = expected_mu * expected_normal_magnitude;
   const auto info = contacts_.get_contact_info(0);
   const auto& resultant = info.get_resultant_force();
-  SpatialForce<double> expected_spatial_force;
+  Vector6<double> expected_spatial_force;
   // The normal component should be pointing to the left (-x) and the tangential
   // component should be pointing up (+y).
   expected_spatial_force << 0, 0, 0, -expected_normal_magnitude,
@@ -385,7 +385,7 @@ TEST_F(SlidingContactFormulaTest, SlidingContactTest) {
   double expected_tangent_magnitude = expected_mu * expected_normal_magnitude;
   const auto info = contacts_.get_contact_info(0);
   const auto& resultant = info.get_resultant_force();
-  SpatialForce<double> expected_spatial_force;
+  Vector6<double> expected_spatial_force;
   // The normal component should be pointing to the left (-x) and the tangential
   // component should be pointing up (+y).
   expected_spatial_force << 0, 0, 0, -expected_normal_magnitude,
@@ -427,7 +427,7 @@ TEST_F(SlidingSpinContactFormulaTest, SlidingSpinContactTest) {
   double expected_tangent_magnitude = expected_mu * expected_normal_magnitude;
   const auto info = contacts_.get_contact_info(0);
   const auto& resultant = info.get_resultant_force();
-  SpatialForce<double> expected_spatial_force;
+  Vector6<double> expected_spatial_force;
   // The normal component should be pointing to the left (-x) and the tangential
   // component should be pointing up (+y).
   expected_spatial_force << 0, 0, 0, -expected_normal_magnitude,

attic/multibody/rigid_body_plant/test/contact_resultant_force_test.cc

@@ -76,7 +76,7 @@ GTEST_TEST(ContactResultantForceTest,
   ref_point << 0, 0, 3;
   ContactForce<double> cforce(pos, normal, force, torque);
 
-  SpatialForce<double> eq_wrench;
+  Vector6<double> eq_wrench;
   Isometry3<double> transform(Isometry3<double>::Identity());
   transform.translation() = pos - ref_point;
   eq_wrench = transformSpatialForce(transform, cforce.get_spatial_force());
@@ -100,7 +100,7 @@ GTEST_TEST(ContactResultantForceTest, ForceAccumulationTest) {
   normal = normal_force.normalized();
   torque << 6, 7, 8;
   pos << 10, 11, 12;
-  SpatialForce<double> full_wrench, torque_free_wrench;
+  Vector6<double> full_wrench, torque_free_wrench;
   full_wrench.template head<3>() = torque;
   full_wrench.template tail<3>() = force;
   torque_free_wrench.template head<3>() << 0, 0, 0;

common/eigen_types.h

@@ -16,6 +16,7 @@
 #include "drake/common/constants.h"
 #include "drake/common/drake_assert.h"
 #include "drake/common/drake_copyable.h"
+#include "drake/common/drake_deprecated.h"
 
 namespace drake {
 
@@ -167,7 +168,9 @@ using WrenchVector = Eigen::Matrix<Scalar, 6, 1>;
 /// SpatialForce the rotational force can be a pure torque or the accumulation
 /// of moments and need not necessarily be a function of the force term.
 template <typename Scalar>
-using SpatialForce = Eigen::Matrix<Scalar, 6, 1>;
+using SpatialForce
+DRAKE_DEPRECATED("2019-10-15", "Please use Vector6<> instead.")
+    = Eigen::Matrix<Scalar, 6, 1>;
 
 /// EigenSizeMinPreferDynamic<a, b>::value gives the min between compile-time
 /// sizes @p a and @p b. 0 has absolute priority, followed by 1, followed by

examples/valkyrie/robot_state_encoder.cc

@@ -144,7 +144,7 @@ void RobotStateEncoder::SetStateAndEfforts(
   translator_.EncodeMessageTorque(torque, message);
 }
 
-SpatialForce<double>
+Vector6<double>
 RobotStateEncoder::GetSpatialForceActingOnBody1ByBody2InBody1Frame(
     const KinematicsResults<double>& kinematics_results,
     const ContactResults<double>& contact_results,
@@ -177,7 +177,7 @@ RobotStateEncoder::GetSpatialForceActingOnBody1ByBody2InBody1Frame(
     calc.AddForce(f);
   }
 
-  SpatialForce<double> spatial_force_in_world_aligned_body_frame =
+  Vector6<double> spatial_force_in_world_aligned_body_frame =
       calc.ComputeResultant(reference_point).get_spatial_force();
   Isometry3<double> world_aligned_to_body_frame(Isometry3<double>::Identity());
   world_aligned_to_body_frame.linear() =
@@ -190,13 +190,13 @@ void RobotStateEncoder::SetForceTorque(
     const KinematicsResults<double>& kinematics_results,
     const ContactResults<double>& contact_results,
     bot_core::robot_state_t* message) const {
-  std::vector<SpatialForce<double>> spatial_force_in_sensor_frame(
+  std::vector<Vector6<double>> spatial_force_in_sensor_frame(
       force_torque_sensor_info_.size());
 
   for (size_t i = 0; i < force_torque_sensor_info_.size(); ++i) {
     const RigidBody<double>& body =
         force_torque_sensor_info_[i].get_rigid_body();
-    SpatialForce<double> spatial_force =
+    Vector6<double> spatial_force =
         GetSpatialForceActingOnBody1ByBody2InBody1Frame(
             kinematics_results, contact_results, body,
             translator_.get_robot().world());
@@ -209,7 +209,7 @@ void RobotStateEncoder::SetForceTorque(
   auto& force_torque = message->force_torque;
 
   if (l_foot_ft_sensor_idx_ != -1) {
-    const SpatialForce<double>& spatial_force =
+    const Vector6<double>& spatial_force =
         spatial_force_in_sensor_frame.at(l_foot_ft_sensor_idx_);
     force_torque.l_foot_force_z =
         static_cast<float>(spatial_force[kForceZIndex]);
@@ -219,7 +219,7 @@ void RobotStateEncoder::SetForceTorque(
         static_cast<float>(spatial_force[kTorqueYIndex]);
   }
   if (r_foot_ft_sensor_idx_ != -1) {
-    const SpatialForce<double>& spatial_force =
+    const Vector6<double>& spatial_force =
         spatial_force_in_sensor_frame.at(r_foot_ft_sensor_idx_);
     force_torque.r_foot_force_z =
         static_cast<float>(spatial_force[kForceZIndex]);
@@ -229,15 +229,15 @@ void RobotStateEncoder::SetForceTorque(
         static_cast<float>(spatial_force[kTorqueYIndex]);
   }
   if (l_hand_ft_sensor_idx_ != -1) {
-    const SpatialForce<double>& spatial_force =
+    const Vector6<double>& spatial_force =
         spatial_force_in_sensor_frame.at(l_hand_ft_sensor_idx_);
     eigenVectorToCArray(spatial_force.head<kSpaceDimension>(),
                         force_torque.l_hand_torque);
     eigenVectorToCArray(spatial_force.tail<kSpaceDimension>(),
                         force_torque.l_hand_force);
   }
   if (r_hand_ft_sensor_idx_ != -1) {
-    const SpatialForce<double>& spatial_force =
+    const Vector6<double>& spatial_force =
         spatial_force_in_sensor_frame.at(r_hand_ft_sensor_idx_);
     eigenVectorToCArray(spatial_force.head<kSpaceDimension>(),
                         force_torque.r_hand_torque);

examples/valkyrie/robot_state_encoder.h

@@ -74,7 +74,7 @@ class RobotStateEncoder final : public LeafSystem<double> {
 
   // Computes the spatial force applied at the origin of Body1 by Body2,
   // expressed in Body1's local frame.
-  SpatialForce<double> GetSpatialForceActingOnBody1ByBody2InBody1Frame(
+  Vector6<double> GetSpatialForceActingOnBody1ByBody2InBody1Frame(
       const KinematicsResults<double>& kinematics_results,
       const ContactResults<double>& contact_results,
       const RigidBody<double>& body1, const RigidBody<double>& body2) const;

examples/valkyrie/test/robot_state_encoder_decoder_test.cc

@@ -35,13 +35,13 @@ using multibody::joints::FloatingBaseType;
 static ContactForce<double> TransformContactWrench(
     const RigidBodyFrame<double>& sensor_info,
     const Isometry3<double>& body_pose,
-    const SpatialForce<double>& spatial_force_in_sensor_frame) {
+    const Vector6<double>& spatial_force_in_sensor_frame) {
   Isometry3<double> sensor_pose =
       body_pose * sensor_info.get_transform_to_body();
   Isometry3<double> sensor_to_world_aligned_sensor = sensor_pose;
   sensor_to_world_aligned_sensor.translation().setZero();
 
-  SpatialForce<double> spatial_force_in_world_aligned_sensor =
+  Vector6<double> spatial_force_in_world_aligned_sensor =
       transformSpatialForce(sensor_to_world_aligned_sensor,
                             spatial_force_in_sensor_frame);
 

multibody/triangle_quadrature/BUILD.bazel

@@ -40,6 +40,7 @@ drake_cc_library(
     deps = [
         ":triangle_quadrature_rules",
         "//common:essential",
+        "//multibody/math:spatial_force",
     ],
 )
 

multibody/triangle_quadrature/triangle_quadrature.cc

@@ -1,5 +1,7 @@
 #include "drake/multibody/triangle_quadrature/triangle_quadrature.h"
 
+#include "drake/multibody/math/spatial_force.h"
+
 // Default template instantiations below.
 namespace drake {
 namespace multibody {