Skip to content

Commit

Permalink
Respond to reviewer comments.
Browse files Browse the repository at this point in the history
  • Loading branch information
mitiguy committed Jul 1, 2024
1 parent 88c9053 commit 6f49d2c
Showing 1 changed file with 63 additions and 40 deletions.
103 changes: 63 additions & 40 deletions multibody/plant/test/multibody_plant_kinematics_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,25 +31,27 @@ using Eigen::Vector3d;
// Fixture for a two degree-of-freedom pendulum having two thin links A and B.
// Link A is connected to world (frame W) with a z-axis pin joint (PinJoint1).
// Link B is connected to link A with another z-axis pin joint (PinJoint2).
// Hence links A and B only move in the world's x-y plane (perpendicular to Wz).
// Hence links A and B only move in the world's x-y plane, i.e., perpendicular
// to Wz (gravity is directed in the -Wz direction).
// The long axis of link A is parallel to A's unit vector Ax and
// the long axis of link B is parallel to B's unit vector Bx.
// PinJoint1 connects point Wo (world W's origin) to link A at point Am of A.
// PinJoint2 connects point Bm of link B to point Af of link A.
// In the baseline configuration, points Wo, Am, Ao, Af, Bm, Bo, are sequential
// along the links (they form a line parallel to Wx = Ax = Bx).
// Note: The applied forces (such as gravity) on this system are irrelevant as
// this text fixture is currently only used to test kinematics for an
// instantaneous given state. The mass/inertia properties of this plant are
// irrelevant and there are no actuators, sensors, controller, or ports.
// The mass of each link is concentrated at its distal end, e.g., link A's
// center of mass is at Af (location of PinJoint2).
// Note: These tests have no actuators, sensors, or controllers.
// Many (but not all) of the tests in this file are solely kinematic, i.e., they
// do not depend on mass/inertia properties or applied forces.
class TwoDOFPlanarPendulumTest : public ::testing::Test {
public:
// Setup the multibody plant.
void SetUp() override {
// Set a spatial inertia for each link.
// Set a spatial inertia for each link (particle at distal end of link).
const double link_mass = 5.0; // kg
const SpatialInertia<double> link_central_inertia =
SpatialInertia<double>::ThinRodWithMass(link_mass_, link_length_,
Vector3<double>::UnitX());
SpatialInertia<double>::PointMass(link_mass, p_AoAf_A_);

// Add the two links to the MultibodyPlant.
bodyA_ = &plant_.AddRigidBody("BodyA", link_central_inertia);
Expand Down Expand Up @@ -85,6 +87,8 @@ class TwoDOFPlanarPendulumTest : public ::testing::Test {
const Vector3<double> p_AmAp_A = -p_AoAm_A_ + p_AoAp_A;
const Vector3<double> w_WA_A(0, 0, wAz_); // w_WA_A = wAz Az
const Vector3<double> alpha_WA_A(0, 0, wAzdot); // alpha_WA_A = ẇAz Az

// Calculate Ap's translational acceleration in frame W, expressed in A.
// a_WAp_A = alpha_WA_A x p_AmAp_A + w_WA_A x (w_WA_A x p_AmAp_A).
const Vector3<double> a_WAp_A =
alpha_WA_A.cross(p_AmAp_A) + w_WA_A.cross(w_WA_A.cross(p_AmAp_A));
Expand All @@ -106,6 +110,7 @@ class TwoDOFPlanarPendulumTest : public ::testing::Test {
math::RotationMatrix<double>::MakeZRotation(qB_);
const Vector3<double> p_BmBp_A = R_AB * p_BmBp_B;

// Calculate Bp's translational acceleration in frame A, expressed in A.
// a_ABp_A = alpha_AB_A x p_BmBp_A + w_AB_A x (w_AB_A x p_BmBp_A).
const Vector3<double> a_ABp_A =
alpha_AB_A.cross(p_BmBp_A) + w_AB_A.cross(w_AB_A.cross(p_BmBp_A));
Expand Down Expand Up @@ -134,6 +139,7 @@ class TwoDOFPlanarPendulumTest : public ::testing::Test {
math::RotationMatrix<double>::MakeZRotation(qB_);
const Vector3<double> p_BmBp_A = R_AB * p_BmBp_B;

// Calculate Bp's translational acceleration in frame W, expressed in A.
// a_WBp_A = a_WAf_A + alpha_WB_A x p_BmBp_A + w_WB_A x (w_WB_A x p_BmBp_A).
const Vector3<double> a_WBp_A = a_WAf_A + alpha_WB_A.cross(p_BmBp_A) +
w_WB_A.cross(w_WB_A.cross(p_BmBp_A));
Expand All @@ -142,20 +148,28 @@ class TwoDOFPlanarPendulumTest : public ::testing::Test {

// Calculate and return [ẇAz, ẇBz], the time derivative of [wAz, wBz].
Vector2<double> CalcVdotForNoAppliedForces() {
// The calculation below was generated by MotionGenesis.
const double mLsq = link_mass_ * link_length_ * link_length_;
const double Izz = mLsq / 12.0;
const double z1 = 2.0 * Izz + 1.5 * mLsq + mLsq * std::cos(qB_);
const double z2 = Izz + 0.25 * mLsq * (1 + 2 * std::cos(qB_));
const double z3 = mLsq * sin(qB_);
const double z4 = Izz + 0.25 * mLsq;
const double z5 = wBz_ * (wBz_ + 2 * wAz_);
const double z2sq = z2 * z2;
// One way to do this calculation is with Drake built-in functionality.
VectorX<double> vdot = plant_.get_generalized_acceleration_output_port()
.Eval<systems::BasicVector<double>>(*context_)
.CopyToVector();

// Another way to calculate ẇAz, ẇBz is with a by-hand analysis of this
// relatively simple planar two-link particle pendulum (with no gravity).
const double cqB = std::cos(qB_);
const double cqB1 = 1.0 + cqB;
const double cqB2 = 3.0 + 2.0 * cqB;
const double sin_over_denom = std::sin(qB_) / (2.0 - cqB * cqB);
const double wAzsq = wAz_ * wAz_;
const double denom = z4 * z1 - z2sq;
const double wADt = 0.5 * z3 * (z2 * wAzsq + z4 * z5) / denom;
const double wBDt = -0.5 * z3 * (z1 * wAzsq + z2 * z5) / denom;
return Vector2<double>(wADt, wBDt); // Returns [ẇAz, ẇBz]
const double wBzsq = wBz_ * wBz_;
const double wAzBz = wAz_ * wBz_;
const double wADt = sin_over_denom * (wBzsq + 2.0 * wAzBz + cqB1 * wAzsq);
const double wBDt =
-sin_over_denom * (cqB1 * wBzsq + cqB2 * wAzsq + 2.0 * cqB1 * wAzBz);
const Vector2<double> wABDt(wADt, wBDt); // [ẇAz, ẇBz]

// Ensure the previous two calculations are nearly identical.
EXPECT_TRUE(CompareMatrices(vdot, wABDt, kTolerance));
return Vector2<double>(vdot(0), vdot(1)); // Returns [ẇAz, ẇBz].
}

protected:
Expand Down Expand Up @@ -328,7 +342,7 @@ TEST_F(TwoDOFPlanarPendulumTest, CalcSpatialAcceleration) {
EXPECT_TRUE(CompareMatrices(A_AAo_A.get_coeffs(), Vector6<double>::Zero(),
kTolerance));

// Point Ao is the point of A located at its centroid (so Ao is Acm).
// Point Ao is the point of A located at its centroid.
// Calculate Ao's spatial acceleration in world W, expressed in link frame A.
const SpatialAcceleration<double> A_WAo_A =
test_utilities::CalcSpatialAccelerationViaAutomaticDifferentiation(
Expand Down Expand Up @@ -389,7 +403,7 @@ TEST_F(TwoDOFPlanarPendulumTest, CalcSpatialAcceleration) {
(R_WA * A_WBo_A).get_coeffs(), kTolerance));

// Point Ap is a generic point of link A.
// Calculate frame_Ap's spatial acceleration in frame W, expressed in A.
// Calculate frame_Ap's spatial acceleration in world W, expressed in A.
const Vector3<double> p_AoAp_A(0.7, -0.8, 0.9);
const Frame<double>& frame_Af = joint2_->frame_on_parent();
const Vector3<double> p_AfAp_A = -p_AoAf_A_ + p_AoAp_A;
Expand Down Expand Up @@ -418,7 +432,7 @@ TEST_F(TwoDOFPlanarPendulumTest, CalcSpatialAcceleration) {
EXPECT_TRUE(CompareMatrices(A_WBp_A.get_coeffs(),
A_WBp_A_expected.get_coeffs(), kTolerance));

// Calculate Bp's spatial acceleration in frame A, expressed in A.
// Calculate Bp's spatial acceleration in frame A, expressed in frame A.
const SpatialAcceleration<double> A_ABp_A =
test_utilities::CalcSpatialAccelerationViaAutomaticDifferentiation(
plant_, *context_, vdot, frame_B, p_BoBp_B, frame_A, frame_A);
Expand All @@ -437,48 +451,57 @@ TEST_F(TwoDOFPlanarPendulumTest, CalcCenterOfMassAcceleration) {
const Frame<double>& frame_A = bodyA_->body_frame();
const Frame<double>& frame_B = bodyB_->body_frame();

// Point Acm is link A's center of mass, colocated with Ao (A's centroid).
// Point Acm is link A's center of mass.
// Calculate Acm's spatial acceleration in world W, expressed in world W.
const Vector3<double> a_WAcm_W =
bodyA_->CalcCenterOfMassTranslationalAccelerationInWorld(*context_);

// Verify previous results by calculating Ao's spatial acceleration in W.
const SpatialAcceleration<double> A_WAo_W_expected =
frame_A.CalcSpatialAccelerationInWorld(*context_);
// Verify previous results by noting that joint2_ (and point Af of A) are
// coincident with the center of mass of link A.
const Frame<double>& joint2_parent_frame = joint2_->frame_on_parent();
const SpatialAcceleration<double> A_WAcm_W_expected =
joint2_parent_frame.CalcSpatialAccelerationInWorld(*context_);
EXPECT_TRUE(
CompareMatrices(a_WAcm_W, A_WAo_W_expected.translational(), kTolerance));
CompareMatrices(a_WAcm_W, A_WAcm_W_expected.translational(), kTolerance));

// Calculate generalized accelerations for use by test_utilities function.
const Vector2<double> vdot = CalcVdotForNoAppliedForces(); // [ẇAz, ẇBz]

// Verify results from CalcCenterOfMassTranslationalAccelerationInWorld()
// match test_utilities::CalcSpatialAccelerationViaAutomaticDifferentiation().
const Vector3<double> p_AoAcm_A = Vector3<double>::Zero();
const SpatialAcceleration<double> A_WAcm_W_expected =
const Vector3<double> p_AoAcm_A = p_AoAf_A_; // Af is colocated with Acm.
const SpatialAcceleration<double> A_WAcm_W_alternate =
test_utilities::CalcSpatialAccelerationViaAutomaticDifferentiation(
plant_, *context_, vdot, frame_A, p_AoAcm_A, frame_W, frame_W);
EXPECT_TRUE(
CompareMatrices(a_WAcm_W, A_WAcm_W_expected.translational(), kTolerance));
EXPECT_TRUE(CompareMatrices(a_WAcm_W, A_WAcm_W_alternate.translational(),
kTolerance));

// Point Bcm is link B's center of mass, colocated with Bo (B's centroid).
// Point Bcm is link B's center of mass.
// Calculate Bcm's spatial acceleration in world W, expressed in world W.
const Vector3<double> a_WBcm_W =
bodyB_->CalcCenterOfMassTranslationalAccelerationInWorld(*context_);

// Verify previous results by calculating Bo's spatial acceleration in W.
// Verify previous results by calculating Bcm's spatial acceleration in W.
// a_WBcm_W = a_WBo_W + alpha_WB_W x p_BoBcm_W + w_WB_W x (w_WB_W x p_BoBcm_W)
const SpatialAcceleration<double> A_WBo_W_expected =
frame_B.CalcSpatialAccelerationInWorld(*context_);
const Vector3<double> p_BoBcm_B(0.5 * link_length_, 0, 0);
const math::RotationMatrix<double> R_WB =
math::RotationMatrix<double>::MakeZRotation(qA_ + qB_);
const Vector3<double> p_BoBcm_W = R_WB * p_BoBcm_B;
const Vector3<double> w_WB_W(0, 0, wAz_ + wBz_);
const SpatialAcceleration<double> A_WBcm_W_expected =
A_WBo_W_expected.Shift(p_BoBcm_W, w_WB_W);
EXPECT_TRUE(
CompareMatrices(a_WBcm_W, A_WBo_W_expected.translational(), kTolerance));
CompareMatrices(a_WBcm_W, A_WBcm_W_expected.translational(), kTolerance));

// Verify results from CalcCenterOfMassTranslationalAccelerationInWorld()
// match test_utilities::CalcSpatialAccelerationViaAutomaticDifferentiation().
const Vector3<double> p_BoBcm_B = Vector3<double>::Zero();
const SpatialAcceleration<double> A_WBcm_W_expected =
const SpatialAcceleration<double> A_WBcm_W_alternate =
test_utilities::CalcSpatialAccelerationViaAutomaticDifferentiation(
plant_, *context_, vdot, frame_B, p_BoBcm_B, frame_W, frame_W);
EXPECT_TRUE(
CompareMatrices(a_WBcm_W, A_WBcm_W_expected.translational(), kTolerance));
EXPECT_TRUE(CompareMatrices(a_WBcm_W, A_WBcm_W_alternate.translational(),
kTolerance));
}

} // namespace
Expand Down

0 comments on commit 6f49d2c

Please sign in to comment.