diff --git a/multibody/plant/test/multibody_plant_kinematics_test.cc b/multibody/plant/test/multibody_plant_kinematics_test.cc index 78b54b98fff0..3cfa55d17cb1 100644 --- a/multibody/plant/test/multibody_plant_kinematics_test.cc +++ b/multibody/plant/test/multibody_plant_kinematics_test.cc @@ -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 link_central_inertia = - SpatialInertia::ThinRodWithMass(link_mass_, link_length_, - Vector3::UnitX()); + SpatialInertia::PointMass(link_mass, p_AoAf_A_); // Add the two links to the MultibodyPlant. bodyA_ = &plant_.AddRigidBody("BodyA", link_central_inertia); @@ -85,6 +87,8 @@ class TwoDOFPlanarPendulumTest : public ::testing::Test { const Vector3 p_AmAp_A = -p_AoAm_A_ + p_AoAp_A; const Vector3 w_WA_A(0, 0, wAz_); // w_WA_A = wAz Az const Vector3 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 a_WAp_A = alpha_WA_A.cross(p_AmAp_A) + w_WA_A.cross(w_WA_A.cross(p_AmAp_A)); @@ -106,6 +110,7 @@ class TwoDOFPlanarPendulumTest : public ::testing::Test { math::RotationMatrix::MakeZRotation(qB_); const Vector3 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 a_ABp_A = alpha_AB_A.cross(p_BmBp_A) + w_AB_A.cross(w_AB_A.cross(p_BmBp_A)); @@ -134,6 +139,7 @@ class TwoDOFPlanarPendulumTest : public ::testing::Test { math::RotationMatrix::MakeZRotation(qB_); const Vector3 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 a_WBp_A = a_WAf_A + alpha_WB_A.cross(p_BmBp_A) + w_WB_A.cross(w_WB_A.cross(p_BmBp_A)); @@ -142,20 +148,28 @@ class TwoDOFPlanarPendulumTest : public ::testing::Test { // Calculate and return [ẇAz, ẇBz], the time derivative of [wAz, wBz]. Vector2 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 vdot = plant_.get_generalized_acceleration_output_port() + .Eval>(*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(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 wABDt(wADt, wBDt); // [ẇAz, ẇBz] + + // Ensure the previous two calculations are nearly identical. + EXPECT_TRUE(CompareMatrices(vdot, wABDt, kTolerance)); + return Vector2(vdot(0), vdot(1)); // Returns [ẇAz, ẇBz]. } protected: @@ -328,7 +342,7 @@ TEST_F(TwoDOFPlanarPendulumTest, CalcSpatialAcceleration) { EXPECT_TRUE(CompareMatrices(A_AAo_A.get_coeffs(), Vector6::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 A_WAo_A = test_utilities::CalcSpatialAccelerationViaAutomaticDifferentiation( @@ -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 p_AoAp_A(0.7, -0.8, 0.9); const Frame& frame_Af = joint2_->frame_on_parent(); const Vector3 p_AfAp_A = -p_AoAf_A_ + p_AoAp_A; @@ -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 A_ABp_A = test_utilities::CalcSpatialAccelerationViaAutomaticDifferentiation( plant_, *context_, vdot, frame_B, p_BoBp_B, frame_A, frame_A); @@ -437,48 +451,57 @@ TEST_F(TwoDOFPlanarPendulumTest, CalcCenterOfMassAcceleration) { const Frame& frame_A = bodyA_->body_frame(); const Frame& 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 a_WAcm_W = bodyA_->CalcCenterOfMassTranslationalAccelerationInWorld(*context_); - // Verify previous results by calculating Ao's spatial acceleration in W. - const SpatialAcceleration 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& joint2_parent_frame = joint2_->frame_on_parent(); + const SpatialAcceleration 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 vdot = CalcVdotForNoAppliedForces(); // [ẇAz, ẇBz] // Verify results from CalcCenterOfMassTranslationalAccelerationInWorld() // match test_utilities::CalcSpatialAccelerationViaAutomaticDifferentiation(). - const Vector3 p_AoAcm_A = Vector3::Zero(); - const SpatialAcceleration A_WAcm_W_expected = + const Vector3 p_AoAcm_A = p_AoAf_A_; // Af is colocated with Acm. + const SpatialAcceleration 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 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 A_WBo_W_expected = frame_B.CalcSpatialAccelerationInWorld(*context_); + const Vector3 p_BoBcm_B(0.5 * link_length_, 0, 0); + const math::RotationMatrix R_WB = + math::RotationMatrix::MakeZRotation(qA_ + qB_); + const Vector3 p_BoBcm_W = R_WB * p_BoBcm_B; + const Vector3 w_WB_W(0, 0, wAz_ + wBz_); + const SpatialAcceleration 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 p_BoBcm_B = Vector3::Zero(); - const SpatialAcceleration A_WBcm_W_expected = + const SpatialAcceleration 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