Skip to content

Commit

Permalink
Create MultibodyPlant::CalcCenterOfMassTranslationalAccelerationInWor…
Browse files Browse the repository at this point in the history 
…ld().

Co-Authored-By: mitiguy <[email protected]>
  • Loading branch information
@mitiguy
mitiguy and mitiguy authored Jul 17, 2024
1 parent f4b32e4 commit 880d383
Show file tree
Hide file tree
Showing 5 changed files with 251 additions and 30 deletions.
73 changes: 63 additions & 10 deletions multibody/plant/multibody_plant.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3455,8 +3455,8 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
/// @throws std::exception if `this` has no body except world_body().
/// @throws std::exception if mₛ ≤ 0 (where mₛ is the mass of system S).
/// @note The world_body() is ignored. p_WoScm_W = ∑ (mᵢ pᵢ) / mₛ, where
/// mₛ = ∑ mᵢ, mᵢ is the mass of the iᵗʰ body, and pᵢ is Bcm's position vector
/// from Wo expressed in frame W (Bcm is the center of mass of the iᵗʰ body).
/// mₛ = ∑ mᵢ, mᵢ is the mass of the iᵗʰ body, and pᵢ is Bᵢcm's position from
/// Wo expressed in frame W (Bᵢcm is the center of mass of the iᵗʰ body).
Vector3<T> CalcCenterOfMassPositionInWorld(
const systems::Context<T>& context) const {
this->ValidateContext(context);
Expand All @@ -3476,8 +3476,8 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
/// @throws std::exception if mₛ ≤ 0 (where mₛ is the mass of system S).
/// @note The world_body() is ignored. p_WoScm_W = ∑ (mᵢ pᵢ) / mₛ, where
/// mₛ = ∑ mᵢ, mᵢ is the mass of the iᵗʰ body contained in model_instances,
/// and pᵢ is Bcm's position vector from Wo expressed in frame W
/// (Bcm is the center of mass of the iᵗʰ body).
/// and pᵢ is Bᵢcm's position vector from Wo expressed in frame W
/// (Bᵢcm is the center of mass of the iᵗʰ body).
Vector3<T> CalcCenterOfMassPositionInWorld(
const systems::Context<T>& context,
const std::vector<ModelInstanceIndex>& model_instances) const {
Expand Down Expand Up @@ -3513,14 +3513,65 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
/// @throws std::exception if `this` has no body except world_body().
/// @throws std::exception if mₛ ≤ 0 (where mₛ is the mass of system S).
/// @note The world_body() is ignored. v_WScm_W = ∑ (mᵢ vᵢ) / mₛ, where
/// mₛ = ∑ mᵢ, mᵢ is the mass of the iᵗʰ body, and vᵢ is Bcm's velocity in
/// world W (Bcm is the center of mass of the iᵗʰ body).
/// mₛ = ∑ mᵢ, mᵢ is the mass of the iᵗʰ body, and vᵢ is Bᵢcm's velocity in
/// world W (Bᵢcm is the center of mass of the iᵗʰ body).
Vector3<T> CalcCenterOfMassTranslationalVelocityInWorld(
const systems::Context<T>& context) const {
DRAKE_MBP_THROW_IF_NOT_FINALIZED();
this->ValidateContext(context);
return internal_tree().CalcCenterOfMassTranslationalVelocityInWorld(
context);
}

/// For the system S contained in this %MultibodyPlant, calculates Scm's
/// translational acceleration in the world frame W expressed in W, where
/// Scm is the center of mass of S.
/// @param[in] context The context contains the state of the model.
/// @retval a_WScm_W Scm's translational acceleration in the world frame W
/// expressed in the world frame W.
/// @throws std::exception if `this` has no body except world_body().
/// @throws std::exception if mₛ ≤ 0, where mₛ is the mass of system S.
/// @note The world_body() is ignored. a_WScm_W = ∑ (mᵢ aᵢ) / mₛ, where
/// mₛ = ∑ mᵢ is the mass of system S, mᵢ is the mass of the iᵗʰ body, and
/// aᵢ is the translational acceleration of Bᵢcm in world W expressed in W
/// (Bᵢcm is the center of mass of the iᵗʰ body).
/// @note When cached values are out of sync with the state stored in context,
/// this method performs an expensive forward dynamics computation, whereas
/// once evaluated, successive calls to this method are inexpensive.
Vector3<T> CalcCenterOfMassTranslationalAccelerationInWorld(
const systems::Context<T>& context) const {
DRAKE_MBP_THROW_IF_NOT_FINALIZED();
this->ValidateContext(context);
return internal_tree().CalcCenterOfMassTranslationalAccelerationInWorld(
context);
}

/// For the system S containing the selected model instances, calculates
/// Scm's translational acceleration in the world frame W expressed in W,
/// where Scm is the center of mass of S.
/// @param[in] context The context contains the state of the model.
/// @param[in] model_instances Vector of selected model instances. If a model
/// instance is repeated in the vector (unusual), it is only counted once.
/// @retval a_WScm_W Scm's translational acceleration in the world frame W
/// expressed in the world frame W.
/// @throws std::exception if model_instances is empty or only has world body.
/// @throws std::exception if mₛ ≤ 0, where mₛ is the mass of system S.
/// @note The world_body() is ignored. a_WScm_W = ∑ (mᵢ aᵢ) / mₛ, where
/// mₛ = ∑ mᵢ is the mass of system S, mᵢ is the mass of the iᵗʰ body in
/// model_instances, and aᵢ is the translational acceleration of Bᵢcm in
/// world W expressed in W (Bᵢcm is the center of mass of the iᵗʰ body).
/// @note When cached values are out of sync with the state stored in context,
/// this method performs an expensive forward dynamics computation, whereas
/// once evaluated, successive calls to this method are inexpensive.
Vector3<T> CalcCenterOfMassTranslationalAccelerationInWorld(
const systems::Context<T>& context,
const std::vector<ModelInstanceIndex>& model_instances) const {
DRAKE_MBP_THROW_IF_NOT_FINALIZED();
this->ValidateContext(context);
return internal_tree().CalcCenterOfMassTranslationalAccelerationInWorld(
context, model_instances);
}

/// Calculates system center of mass translational velocity in world frame W.
/// @param[in] context The context contains the state of the model.
/// @param[in] model_instances Vector of selected model instances. If a model
Expand All @@ -3532,11 +3583,13 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
/// @throws std::exception if mₛ ≤ 0 (where mₛ is the mass of system S).
/// @note The world_body() is ignored. v_WScm_W = ∑ (mᵢ vᵢ) / mₛ, where
/// mₛ = ∑ mᵢ, mᵢ is the mass of the iᵗʰ body contained in model_instances,
/// and vᵢ is Bcm's velocity in world W expressed in frame W
/// (Bcm is the center of mass of the iᵗʰ body).
/// and vᵢ is Bᵢcm's velocity in world W expressed in frame W
/// (Bᵢcm is the center of mass of the iᵗʰ body).
Vector3<T> CalcCenterOfMassTranslationalVelocityInWorld(
const systems::Context<T>& context,
const std::vector<ModelInstanceIndex>& model_instances) const {
DRAKE_MBP_THROW_IF_NOT_FINALIZED();
this->ValidateContext(context);
return internal_tree().CalcCenterOfMassTranslationalVelocityInWorld(
context, model_instances);
}
Expand Down Expand Up @@ -4329,8 +4382,8 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
/// @throws std::exception if model_instances is empty or only has world body.
/// @note The world_body() is ignored. J𝑠_v_ACcm_ = ∑ (mᵢ Jᵢ) / mₛ, where
/// mₛ = ∑ mᵢ, mᵢ is the mass of the iᵗʰ body contained in model_instances,
/// and Jᵢ is Bcm's translational velocity Jacobian in frame A, expressed in
/// frame E (Bcm is the center of mass of the iᵗʰ body).
/// and Jᵢ is Bᵢcm's translational velocity Jacobian in frame A, expressed in
/// frame E (Bᵢcm is the center of mass of the iᵗʰ body).
void CalcJacobianCenterOfMassTranslationalVelocity(
const systems::Context<T>& context,
const std::vector<ModelInstanceIndex>& model_instances,
Expand Down
55 changes: 49 additions & 6 deletions multibody/plant/test/multibody_plant_com_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,12 @@ GTEST_TEST(EmptyMultibodyPlantCenterOfMassTest, CalcCenterOfMassPosition) {
"CalcCenterOfMassTranslationalVelocityInWorld\\(\\): This MultibodyPlant "
"only contains the world_body\\(\\) so its center of mass is undefined.");

DRAKE_EXPECT_THROWS_MESSAGE(
plant.CalcCenterOfMassTranslationalAccelerationInWorld(*context_),
"CalcCenterOfMassTranslationalAccelerationInWorld\\(\\): "
"This MultibodyPlant only contains the world_body\\(\\) so "
"its center of mass is undefined.");

const Frame<double>& frame_W = plant.world_frame();
Eigen::MatrixXd Js_v_WScm_W(3, plant.num_velocities());
DRAKE_EXPECT_THROWS_MESSAGE(
Expand Down Expand Up @@ -125,7 +131,7 @@ class MultibodyPlantCenterOfMassTest : public ::testing::Test {
plant_.SetFreeBodySpatialVelocity(context_.get(), triangle, V_WT_W);

// Denoting Scm as the center of mass of the system formed by Sphere1 and
// Triangle1, form Scm's translational velocity in frame W, expressed in W.
// Triangle1, form Scm's translational velocity in world W, expressed in W.
const Vector3<double> v_WScm_W =
plant_.CalcCenterOfMassTranslationalVelocityInWorld(*context_);

Expand All @@ -143,6 +149,16 @@ class MultibodyPlantCenterOfMassTest : public ::testing::Test {

const double kTolerance = 16 * std::numeric_limits<double>::epsilon();
EXPECT_TRUE(CompareMatrices(v_WScm_W, v_WScm_W_expected, kTolerance));

// Calculate Scm's translational acceleration in world W, expressed in W.
const Vector3<double> a_WScm_W =
plant_.CalcCenterOfMassTranslationalAccelerationInWorld(*context_);

// Verify previous calculation knowing that bodies are in free-fall.
// Note: kTolerance had to be changed to 8 * kTolerance to pass CI due to
// sole failure on mac-arm-ventura-clang-bazel-experimental-release-21710.
const Vector3<double>& gravity = plant_.gravity_field().gravity_vector();
EXPECT_TRUE(CompareMatrices(a_WScm_W, gravity, 8 * kTolerance));
}

protected:
Expand Down Expand Up @@ -210,7 +226,7 @@ TEST_F(MultibodyPlantCenterOfMassTest, CalcTotalMass) {
DRAKE_EXPECT_NO_THROW(plant_.CalcTotalMass(*context_, model_instances));
}

TEST_F(MultibodyPlantCenterOfMassTest, CenterOfMassPosition) {
TEST_F(MultibodyPlantCenterOfMassTest, CenterOfMassPositionEtc) {
// Verify the plant's default center of mass position makes sense.
Eigen::Vector3d p_WCcm = plant_.CalcCenterOfMassPositionInWorld(*context_);
const math::RigidTransformd X_WS0 = math::RigidTransformd::Identity();
Expand Down Expand Up @@ -254,6 +270,12 @@ TEST_F(MultibodyPlantCenterOfMassTest, CenterOfMassPosition) {
"CalcCenterOfMassTranslationalVelocityInWorld\\(\\): There must be at "
"least one non-world body contained in model_instances.");

DRAKE_EXPECT_THROWS_MESSAGE(
plant_.CalcCenterOfMassTranslationalAccelerationInWorld(*context_,
model_instances),
"CalcCenterOfMassTranslationalAccelerationInWorld\\(\\): There must be "
"at least one non-world body contained in model_instances.");

Eigen::MatrixXd Js_v_WCcm_W(3, plant_.num_velocities());
const Frame<double>& frame_W = plant_.world_frame();
DRAKE_EXPECT_THROWS_MESSAGE(
Expand All @@ -274,10 +296,16 @@ TEST_F(MultibodyPlantCenterOfMassTest, CenterOfMassPosition) {
"CalcCenterOfMassTranslationalVelocityInWorld\\(\\): There must be at "
"least one non-world body contained in model_instances.");

DRAKE_EXPECT_THROWS_MESSAGE(
plant_.CalcCenterOfMassTranslationalAccelerationInWorld(
*context_, world_model_instance_array),
"CalcCenterOfMassTranslationalAccelerationInWorld\\(\\): There must be "
"at least one non-world body contained in model_instances.");

DRAKE_EXPECT_THROWS_MESSAGE(
plant_.CalcJacobianCenterOfMassTranslationalVelocity(
*context_, model_instances, JacobianWrtVariable::kV, frame_W, frame_W,
&Js_v_WCcm_W),
*context_, world_model_instance_array, JacobianWrtVariable::kV,
frame_W, frame_W, &Js_v_WCcm_W),
"CalcJacobianCenterOfMassTranslationalVelocity\\(\\): There must be at "
"least one non-world body contained in model_instances.");

Expand All @@ -289,10 +317,16 @@ TEST_F(MultibodyPlantCenterOfMassTest, CenterOfMassPosition) {
"CalcCenterOfMassTranslationalVelocityInWorld\\(\\): There must be at "
"least one non-world body contained in model_instances.");

DRAKE_EXPECT_THROWS_MESSAGE(
plant_.CalcCenterOfMassTranslationalAccelerationInWorld(
*context_, world_model_instance_array),
"CalcCenterOfMassTranslationalAccelerationInWorld\\(\\): There must be "
"at least one non-world body contained in model_instances.");

DRAKE_EXPECT_THROWS_MESSAGE(
plant_.CalcJacobianCenterOfMassTranslationalVelocity(
*context_, model_instances, JacobianWrtVariable::kV, frame_W, frame_W,
&Js_v_WCcm_W),
*context_, world_model_instance_array, JacobianWrtVariable::kV,
frame_W, frame_W, &Js_v_WCcm_W),
"CalcJacobianCenterOfMassTranslationalVelocity\\(\\): There must be at "
"least one non-world body contained in model_instances.");

Expand Down Expand Up @@ -332,6 +366,12 @@ TEST_F(MultibodyPlantCenterOfMassTest, CenterOfMassPosition) {
"CalcCenterOfMassTranslationalVelocityInWorld\\(\\): The "
"system's total mass must be greater than zero.");

DRAKE_EXPECT_THROWS_MESSAGE(
plant_.CalcCenterOfMassTranslationalAccelerationInWorld(*context_,
model_instances),
"CalcCenterOfMassTranslationalAccelerationInWorld\\(\\): The "
"system's total mass must be greater than zero.");

DRAKE_EXPECT_THROWS_MESSAGE(
plant_.CalcJacobianCenterOfMassTranslationalVelocity(
*context_, model_instances, JacobianWrtVariable::kV, frame_W, frame_W,
Expand Down Expand Up @@ -360,6 +400,9 @@ TEST_F(MultibodyPlantCenterOfMassTest, CenterOfMassPosition) {
EXPECT_THROW(plant_.CalcCenterOfMassTranslationalVelocityInWorld(
*context_, model_instances),
std::exception);
EXPECT_THROW(plant_.CalcCenterOfMassTranslationalAccelerationInWorld(
*context_, model_instances),
std::exception);
EXPECT_THROW(plant_.CalcJacobianCenterOfMassTranslationalVelocity(
*context_, model_instances, JacobianWrtVariable::kV, frame_W,
frame_W, &Js_v_WCcm_W),
Expand Down
12 changes: 12 additions & 0 deletions multibody/plant/test/multibody_plant_kinematics_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -499,6 +499,18 @@ TEST_F(TwoDOFPlanarPendulumTest, CalcCenterOfMassAccelerationForwardDynamics) {
plant_, *context_, vdot, frame_B, p_BoBcm_B, frame_W, frame_W);
EXPECT_TRUE(CompareMatrices(a_WBcm_W, A_WBcm_W_alternate.translational(),
kTolerance));

// Denoting Scm as the center of mass of the system consisting of A and B,
// calculate Scm's translational acceleration in world W, expressed in W.
const Vector3<double> a_WScm_W =
plant_.CalcCenterOfMassTranslationalAccelerationInWorld(*context_);

// Verify previous calculation with by-hand calculation of a_WScm_W.
const double mA = bodyA_->get_mass(*context_);
const double mB = bodyB_->get_mass(*context_);
const double mS = mA + mB;
Vector3<double> a_WScm_W_expected = (mA * a_WAcm_W + mB * a_WBcm_W) / mS;
EXPECT_TRUE(CompareMatrices(a_WScm_W, a_WScm_W_expected, kTolerance));
}

} // namespace
Expand Down
Loading

0 comments on commit 880d383

Please sign in to comment.