Skip to content

Commit

Permalink
Deprecate CalcCenterOfMassPosition() in favor of CalcCenterOfMassPosi…
Browse files Browse the repository at this point in the history 
…tionInWorld(). (#14538)

Fixes #14537
  • Loading branch information
@mitiguy
mitiguy authored Jan 27, 2021
1 parent 34ad251 commit 0ac940e
Show file tree
Hide file tree
Showing 10 changed files with 168 additions and 107 deletions.
1 change: 1 addition & 0 deletions bindings/pydrake/multibody/BUILD.bazel
View file
Original file line number Diff line number Diff line change
Expand Up @@ -261,6 +261,7 @@ drake_py_unittest(
":parsing_py",
":plant_py",
":tree_py",
"//bindings/pydrake/common/test_utilities:deprecation_py",
"//bindings/pydrake/common/test_utilities:numpy_compare_py",
"//bindings/pydrake/systems:analysis_py",
"//bindings/pydrake/systems:scalar_conversion_py",
Expand Down
37 changes: 28 additions & 9 deletions bindings/pydrake/multibody/plant_py.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
#include "drake/bindings/pydrake/common/value_pybind.h"
#include "drake/bindings/pydrake/documentation_pybind.h"
#include "drake/bindings/pydrake/pydrake_pybind.h"
#include "drake/common/drake_deprecated.h"
#include "drake/common/eigen_types.h"
#include "drake/geometry/query_results/penetration_as_point_pair.h"
#include "drake/geometry/scene_graph.h"
Expand Down Expand Up @@ -279,20 +280,38 @@ void DoScalarDependentDefinitions(py::module m, T) {
return p_AQi;
},
py::arg("context"), py::arg("frame_B"), py::arg("p_BQi"),
py::arg("frame_A"), cls_doc.CalcPointsPositions.doc)
// TODO(eric.cousineau): Include `CalcInverseDynamics` once there is an
// overload that (a) services MBP directly and (b) uses body
// association that is less awkward than implicit BodyNodeIndex.
py::arg("frame_A"), cls_doc.CalcPointsPositions.doc);
// TODO(eric.cousineau): Include `CalcInverseDynamics` once there is an
// overload that (a) services MBP directly and (b) uses body
// association that is less awkward than implicit BodyNodeIndex.
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
cls // BR
.def("CalcCenterOfMassPosition",
overload_cast_explicit<Vector3<T>, const Context<T>&>(
&Class::CalcCenterOfMassPosition),
py::arg("context"), cls_doc.CalcCenterOfMassPosition.doc_1args)
WrapDeprecated(cls_doc.CalcCenterOfMassPosition.doc_deprecated,
overload_cast_explicit<Vector3<T>, const Context<T>&>(
&Class::CalcCenterOfMassPosition)),
py::arg("context"), cls_doc.CalcCenterOfMassPosition.doc_deprecated)
.def("CalcCenterOfMassPosition",
WrapDeprecated(cls_doc.CalcCenterOfMassPosition.doc_deprecated,
overload_cast_explicit<Vector3<T>, const Context<T>&,
const std::vector<ModelInstanceIndex>&>(
&Class::CalcCenterOfMassPosition)),
py::arg("context"), py::arg("model_instances"),
cls_doc.CalcCenterOfMassPosition.doc_deprecated);
#pragma GCC diagnostic pop
cls // BR
.def("CalcCenterOfMassPositionInWorld",
overload_cast_explicit<Vector3<T>, const Context<T>&>(
&Class::CalcCenterOfMassPositionInWorld),
py::arg("context"),
cls_doc.CalcCenterOfMassPositionInWorld.doc_1args)
.def("CalcCenterOfMassPositionInWorld",
overload_cast_explicit<Vector3<T>, const Context<T>&,
const std::vector<ModelInstanceIndex>&>(
&Class::CalcCenterOfMassPosition),
&Class::CalcCenterOfMassPositionInWorld),
py::arg("context"), py::arg("model_instances"),
cls_doc.CalcCenterOfMassPosition.doc_2args)
cls_doc.CalcCenterOfMassPositionInWorld.doc_2args)
.def(
"CalcSpatialMomentumInWorldAboutPoint",
[](const Class* self, const Context<T>& context,
Expand Down
12 changes: 10 additions & 2 deletions bindings/pydrake/multibody/test/plant_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,7 @@
from pydrake.common import FindResourceOrThrow
from pydrake.common.deprecation import install_numpy_warning_filters
from pydrake.common.test_utilities import numpy_compare
from pydrake.common.test_utilities.deprecation import catch_drake_warnings
from pydrake.common.value import AbstractValue, Value
from pydrake.geometry import (
Box,
Expand Down Expand Up @@ -781,12 +782,19 @@ def test_multibody_tree_kinematics(self, T):
frame_A=world_frame).T
self.assertTupleEqual(p_AQi.shape, (2, 3))

p_com = plant.CalcCenterOfMassPosition(context=context)
p_com = plant.CalcCenterOfMassPositionInWorld(context=context)
self.assertTupleEqual(p_com.shape, (3, ))
p_com = plant.CalcCenterOfMassPosition(
p_com = plant.CalcCenterOfMassPositionInWorld(
context=context, model_instances=[instance])
self.assertTupleEqual(p_com.shape, (3, ))

with catch_drake_warnings(expected_count=2):
p_com = plant.CalcCenterOfMassPosition(context=context)
self.assertTupleEqual(p_com.shape, (3, ))
p_com = plant.CalcCenterOfMassPosition(
context=context, model_instances=[instance])
self.assertTupleEqual(p_com.shape, (3, ))

nq = plant.num_positions()
nv = plant.num_velocities()
wrt_list = [
Expand Down
81 changes: 43 additions & 38 deletions multibody/plant/multibody_plant.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2428,50 +2428,54 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
context, frame_B, p_BQi, frame_A, p_AQi);
}

/// This method computes the center of mass position p_WCcm of all bodies in
/// `MultibodyPlant` measured and expressed in world frame W. The bodies are
/// considered as a single composite body C, whose center of mass
/// `composite_mass` is located at Ccm. The world_body() is ignored.
///
/// @param[in] context
/// The context containing the state of the model. It stores the
/// generalized positions q of the model.
/// @retval p_WCcm
/// The output position of center of mass in the world frame W.
///
/// @throws std::runtime_error if `MultibodyPlant` has no body except
/// `world_body()`.
/// @throws std::exception if `composite_mass <= 0`.
Vector3<T> CalcCenterOfMassPosition(
/// Calculates the position vector from the world origin Wo to the center of
/// mass of all bodies in this MultibodyPlant, expressed in the world frame W.
/// @param[in] context Contains the state of the model.
/// @retval p_WScm_W position vector from Wo to Scm expressed in world frame
/// W, where Scm is the center of mass of the system S stored by `this` plant.
/// @throws std::exception if `this` has no body except world_body().
/// @throws std::exception if mₛ ≤ 0 (mₛ is the mass of `this` system S).
/// @note The world_body() is ignored.
Vector3<T> CalcCenterOfMassPositionInWorld(
const systems::Context<T>& context) const {
this->ValidateContext(context);
return internal_tree().CalcCenterOfMassPosition(context);
return internal_tree().CalcCenterOfMassPositionInWorld(context);
}

/// This method computes the center of mass position p_WCcm of specified model
/// instances measured and expressed in world frame W. The specified model
/// instances are considered as a single composite body C, whose center of
/// mass `composite_mass` is located at Ccm. The models are selected by a
/// vector of model instances `model_instances`. This function does not
/// distinguish between welded bodies, joint connected bodies and free
/// bodies in the `model_instances`. The world_body() is ignored.
///
/// @param[in] context
/// The context containing the state of the model. It stores the
/// generalized positions q of the model.
/// @param[in] model_instances
/// The vector of selected model instances.
/// @retval p_WCcm
/// The output position of center of mass in the world frame W.
///
/// @throws std::runtime_error if `MultibodyPlant` has no model_instance
/// except `world_model_instance()`.
/// @throws std::exception if `composite_mass <= 0`.
DRAKE_DEPRECATED("2021-04-21", "Use CalcCenterOfMassPositionInWorld() "
"instead of CalcCenterOfMassPosition().")
Vector3<T> CalcCenterOfMassPosition(
const systems::Context<T>& context) const {
return CalcCenterOfMassPositionInWorld(context);
}

/// Calculates the position vector from the world origin Wo to the center of
/// mass of all bodies contained in model_instances, expressed in the world
/// frame W.
/// @param[in] context Contains the state of the model.
/// @param[in] model_instances Vector of selected model instances. This method
/// does not distinguish between welded, joint connected, or floating bodies.
/// @retval p_WScm_W position vector from world origin Wo to Scm expressed in
/// the world frame W, where Scm is the center of mass of the system S of
/// bodies contained in model_instances.
/// @throws std::exception if model_instance only has world_model_instance(),
/// i.e., model_instances has no body except world_body().
/// @throws std::exception if mₛ ≤ 0 (mₛ is the mass of the system S).
/// @note The world_body() is ignored.
Vector3<T> CalcCenterOfMassPositionInWorld(
const systems::Context<T>& context,
const std::vector<ModelInstanceIndex>& model_instances) const {
this->ValidateContext(context);
return internal_tree().CalcCenterOfMassPosition(context, model_instances);
return internal_tree().
CalcCenterOfMassPositionInWorld(context, model_instances);
}

DRAKE_DEPRECATED("2021-04-21", "Use CalcCenterOfMassPositionInWorld() "
"instead of CalcCenterOfMassPosition().")
Vector3<T> CalcCenterOfMassPosition(
const systems::Context<T>& context,
const std::vector<ModelInstanceIndex>& model_instances) const {
return CalcCenterOfMassPositionInWorld(context, model_instances);
}

/// Calculates system center of mass translational velocity in world frame W.
Expand Down Expand Up @@ -2514,7 +2518,8 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
/// (the system's center of mass), use something like: <pre>
/// MultibodyPlant<T> plant;
/// // ... code to load a model ....
/// const Vector3<T> p_WoScm_W = plant.CalcCenterOfMassPosition(context);
/// const Vector3<T> p_WoScm_W =
/// plant.CalcCenterOfMassPositionInWorld(context);
/// const SpatialMomentum<T> L_WScm_W =
/// plant.CalcSpatialMomentumInWorldAboutPoint(context, p_WoScm_W);
/// </pre>
Expand Down Expand Up @@ -2545,7 +2550,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
/// const std::vector<ModelInstanceIndex> model_instances{
/// gripper_model_instance, robot_model_instance};
/// const Vector3<T> p_WoScm_W =
/// plant.CalcCenterOfMassPosition(context, model_instances);
/// plant.CalcCenterOfMassPositionInWorld(context, model_instances);
/// SpatialMomentum<T> L_WScm_W =
/// plant.CalcSpatialMomentumInWorldAboutPoint(context, model_instances,
/// p_WoScm_W);
Expand Down
67 changes: 50 additions & 17 deletions multibody/plant/test/multibody_plant_com_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,15 +16,24 @@ namespace drake {
namespace multibody {
namespace {

GTEST_TEST(EmptyMultibodyPlantCenterOfMassTest, GetCenterOfMassPosition) {
GTEST_TEST(EmptyMultibodyPlantCenterOfMassTest, CalcCenterOfMassPosition) {
MultibodyPlant<double> plant(0.0);
plant.Finalize();
auto context_ = plant.CreateDefaultContext();
DRAKE_EXPECT_THROWS_MESSAGE(
plant.CalcCenterOfMassPositionInWorld(*context_),
std::exception,
"CalcCenterOfMassPositionInWorld\\(\\): This MultibodyPlant contains "
"only the world_body\\(\\) so its center of mass is undefined.");

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
DRAKE_EXPECT_THROWS_MESSAGE(
plant.CalcCenterOfMassPosition(*context_),
std::exception,
"CalcCenterOfMassPosition\\(\\): This MultibodyPlant contains only the "
"world_body\\(\\) so its center of mass is undefined.");
"CalcCenterOfMassPositionInWorld\\(\\): This MultibodyPlant contains "
"only the world_body\\(\\) so its center of mass is undefined.");
#pragma GCC diagnostic pop // pop -Wdeprecated-declarations

DRAKE_EXPECT_THROWS_MESSAGE(
plant.CalcCenterOfMassTranslationalVelocityInWorld(*context_),
Expand Down Expand Up @@ -76,10 +85,18 @@ class MultibodyPlantCenterOfMassTest : public ::testing::Test {
((p_WSo_W + R_WS * p_SScm_S_) * mass_S_ +
(p_WTo_W + R_WT * p_TTcm_T_) * mass_T_) /
(mass_S_ + mass_T_);
Eigen::Vector3d p_WCcm = plant_.CalcCenterOfMassPosition(*context_);
Eigen::Vector3d p_WCcm = plant_.CalcCenterOfMassPositionInWorld(*context_);
// Allow for 3 bits (2^3 = 8) of error.
const double kTolerance = 8 * std::numeric_limits<double>::epsilon();
EXPECT_TRUE(CompareMatrices(p_WCcm, p_WCcm_expected, kTolerance));

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
Eigen::Vector3d p_WCcm_deprecated =
plant_.CalcCenterOfMassPosition(*context_);
EXPECT_TRUE(
CompareMatrices(p_WCcm_deprecated, p_WCcm_expected, kTolerance));
#pragma GCC diagnostic pop // pop -Wdeprecated-declarations
}

const RigidBody<double>& GetSphereBody() {
Expand Down Expand Up @@ -141,7 +158,7 @@ class MultibodyPlantCenterOfMassTest : public ::testing::Test {

TEST_F(MultibodyPlantCenterOfMassTest, CenterOfMassPosition) {
// Verify the plant's default center of mass position makes sense.
Eigen::Vector3d p_WCcm = plant_.CalcCenterOfMassPosition(*context_);
Eigen::Vector3d p_WCcm = plant_.CalcCenterOfMassPositionInWorld(*context_);
const math::RigidTransformd X_WS0 = math::RigidTransformd::Identity();
const math::RigidTransformd X_WT0 = math::RigidTransformd::Identity();
Eigen::Vector3d result =
Expand All @@ -151,6 +168,13 @@ TEST_F(MultibodyPlantCenterOfMassTest, CenterOfMassPosition) {
const double kTolerance = 8 * std::numeric_limits<double>::epsilon();
EXPECT_TRUE(CompareMatrices(p_WCcm, result, kTolerance));

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
Eigen::Vector3d p_WCcm_deprecatedA =
plant_.CalcCenterOfMassPosition(*context_);
EXPECT_TRUE(CompareMatrices(p_WCcm_deprecatedA, result, kTolerance));
#pragma GCC diagnostic pop // pop -Wdeprecated-declarations

// Verify the plant's center of mass location for an arbitrary input.
const Eigen::Vector3d p_WSo_W(1.1, 2.3, 3.7);
const Eigen::Vector3d p_WTo_W(-5.2, 10.4, -6.8);
Expand All @@ -173,9 +197,9 @@ TEST_F(MultibodyPlantCenterOfMassTest, CenterOfMassPosition) {
// Ensure center of mass methods throw an exception for empty model_instances.
std::vector<ModelInstanceIndex> model_instances;
DRAKE_EXPECT_THROWS_MESSAGE(
plant_.CalcCenterOfMassPosition(*context_, model_instances),
plant_.CalcCenterOfMassPositionInWorld(*context_, model_instances),
std::exception,
"CalcCenterOfMassPosition\\(\\): There were no bodies specified. "
"CalcCenterOfMassPositionInWorld\\(\\): There were no bodies specified. "
"You must provide at least one selected body.");

DRAKE_EXPECT_THROWS_MESSAGE(
Expand Down Expand Up @@ -208,18 +232,26 @@ TEST_F(MultibodyPlantCenterOfMassTest, CenterOfMassPosition) {

// Try one instance in model_instances.
model_instances.push_back(triangle_instance_);
p_WCcm = plant_.CalcCenterOfMassPosition(*context_, model_instances);
p_WCcm = plant_.CalcCenterOfMassPositionInWorld(*context_, model_instances);
EXPECT_TRUE(CompareMatrices(p_WCcm, p_WTo_W + p_TTcm_T_, kTolerance));

// Verify CalcCenterOfMassPosition() works for 2 instances in model_instances.
// Verify CalcCenterOfMassPositionInWorld() works for 2 instances
// in model_instances.
model_instances.push_back(sphere_instance_);
result = ((p_WSo_W + p_SScm_S_) * mass_S_ + (p_WTo_W + p_TTcm_T_) * mass_T_) /
(mass_S_ + mass_T_);
p_WCcm = plant_.CalcCenterOfMassPosition(*context_, model_instances);
p_WCcm = plant_.CalcCenterOfMassPositionInWorld(*context_, model_instances);
EXPECT_TRUE(CompareMatrices(p_WCcm, result, kTolerance));

// Verify CalcCenterOfMassPosition() works for 2 instances in model_instances,
// where the 2 objects have arbitrary orientation and position.
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
Eigen::Vector3d p_WCcm_deprecatedB =
plant_.CalcCenterOfMassPosition(*context_, model_instances);
EXPECT_TRUE(CompareMatrices(p_WCcm_deprecatedB, result, kTolerance));
#pragma GCC diagnostic pop // pop -Wdeprecated-declarations

// Verify CalcCenterOfMassPositionInWorld() works for 2 objects in
// model_instances, where the 2 objects have arbitrary orientation/position.
const math::RigidTransformd X_WS2(math::RollPitchYawd(0.3, -1.5, 0.7),
Eigen::Vector3d(5.2, -3.1, 10.9));
const math::RigidTransformd X_WT2(math::RollPitchYawd(-2.3, -3.5, 1.2),
Expand All @@ -230,9 +262,9 @@ TEST_F(MultibodyPlantCenterOfMassTest, CenterOfMassPosition) {
set_mass_sphere(0.0);
set_mass_triangle(0.0);
DRAKE_EXPECT_THROWS_MESSAGE(
plant_.CalcCenterOfMassPosition(*context_, model_instances),
plant_.CalcCenterOfMassPositionInWorld(*context_, model_instances),
std::exception,
"CalcCenterOfMassPosition\\(\\): The "
"CalcCenterOfMassPositionInWorld\\(\\): The "
"system's total mass must be greater than zero.");

DRAKE_EXPECT_THROWS_MESSAGE(
Expand Down Expand Up @@ -261,11 +293,12 @@ TEST_F(MultibodyPlantCenterOfMassTest, CenterOfMassPosition) {
// Ensure an exception is thrown if there is an invalid ModelInstanceIndex.
ModelInstanceIndex error_index(10);
model_instances.push_back(error_index);
EXPECT_THROW(plant_.CalcCenterOfMassPosition(*context_, model_instances),
std::exception);
EXPECT_THROW(plant_.CalcCenterOfMassPositionInWorld(
*context_, model_instances),
std::exception);
EXPECT_THROW(plant_.CalcCenterOfMassTranslationalVelocityInWorld(
*context_, model_instances),
std::exception);
std::exception);
}

} // namespace
Expand Down
6 changes: 3 additions & 3 deletions multibody/plant/test/multibody_plant_momentum_energy_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -183,7 +183,7 @@ TEST_F(TwoDofPlanarPendulumTest, CalcBodyASpatialMomentumInWorldAboutPoint) {
// Calculate body A's spatial momentum in world W, about Acm, expressed in W.
// Compare to expected results.
const Vector3<double> p_WoAcm_W =
plant_.CalcCenterOfMassPosition(*context_, model_instances);
plant_.CalcCenterOfMassPositionInWorld(*context_, model_instances);
const SpatialMomentum<double> L_WAcm_W =
plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, model_instances,
p_WoAcm_W);
Expand All @@ -199,7 +199,7 @@ TEST_F(TwoDofPlanarPendulumTest, CalcBodyBSpatialMomentumInWorldAboutPoint) {
std::vector<ModelInstanceIndex> model_instances;
model_instances.push_back(bodyB_model_instance_);
const Vector3<double> p_WoBcm_W =
plant_.CalcCenterOfMassPosition(*context_, model_instances);
plant_.CalcCenterOfMassPositionInWorld(*context_, model_instances);
const SpatialMomentum<double> L_WBcm_W =
plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, model_instances,
p_WoBcm_W);
Expand Down Expand Up @@ -246,7 +246,7 @@ TEST_F(TwoDofPlanarPendulumTest, CalcSystemSpatialMomentumInWorldAboutPoint) {
model_instances.push_back(bodyB_model_instance_);
model_instances.push_back(bodyA_model_instance_);
const Vector3<double> p_WoScm_W =
plant_.CalcCenterOfMassPosition(*context_, model_instances);
plant_.CalcCenterOfMassPositionInWorld(*context_, model_instances);
SpatialMomentum<double> L_WScm_W =
plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, model_instances,
p_WoScm_W);
Expand Down
3 changes: 2 additions & 1 deletion multibody/plant/test/multibody_plant_reaction_forces_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -228,7 +228,8 @@ class LadderTest : public ::testing::Test {
const double weight = kGravity_ * kLadderMass_;

// Position of the ladder's center of gravity.
const Vector3d p_WBcm = plant_->CalcCenterOfMassPosition(*plant_context_);
const Vector3d p_WBcm =
plant_->CalcCenterOfMassPositionInWorld(*plant_context_);

// The x component of the contact force must counteract the torque due to
// gravity plus the actuation torque.
Expand Down
Loading

0 comments on commit 0ac940e

Please sign in to comment.