diff --git a/multibody/plant/BUILD.bazel b/multibody/plant/BUILD.bazel index 1f2db2b23cae..0a7a04432f9c 100644 --- a/multibody/plant/BUILD.bazel +++ b/multibody/plant/BUILD.bazel @@ -478,6 +478,7 @@ drake_cc_googletest( "//common:autodiff", "//common/test_utilities:eigen_matrix_compare", "//math:geometric_transform", + "//multibody/test_utilities:add_fixed_objects_to_plant", ], ) diff --git a/multibody/plant/test/frame_kinematics_test.cc b/multibody/plant/test/frame_kinematics_test.cc index 5b75e4860c63..03d38da3dc31 100644 --- a/multibody/plant/test/frame_kinematics_test.cc +++ b/multibody/plant/test/frame_kinematics_test.cc @@ -6,6 +6,7 @@ #include "drake/common/autodiff.h" #include "drake/common/test_utilities/eigen_matrix_compare.h" #include "drake/multibody/plant/test/kuka_iiwa_model_tests.h" +#include "drake/multibody/test_utilities/add_fixed_objects_to_plant.h" #include "drake/multibody/tree/body.h" #include "drake/multibody/tree/frame.h" @@ -13,6 +14,7 @@ namespace drake { namespace multibody { +using math::RigidTransformd; using test::KukaIiwaModelTests; namespace { @@ -105,6 +107,49 @@ TEST_F(KukaIiwaModelTests, FramesKinematics) { kTolerance, MatrixCompareType::relative)); } +GTEST_TEST(MultibodyPlantTest, FixedWorldKinematics) { + // Numerical tolerance used to verify numerical results. + const double kTolerance = 10 * std::numeric_limits::epsilon(); + + MultibodyPlant plant(0.0); + test::AddFixedObjectsToPlant(&plant); + plant.Finalize(); + std::unique_ptr> context = plant.CreateDefaultContext(); + + // The point of this test is that we can compute poses and spatial velocities + // even for a model with zero dofs. + ASSERT_EQ(plant.num_positions(), 0); + ASSERT_EQ(plant.num_velocities(), 0); + // However the world is non-empty. + ASSERT_NE(plant.num_bodies(), 0); + + const Body& mug = plant.GetBodyByName("main_body"); + + // The objects frame O is affixed to a robot table defined by + // test::AddFixedObjectsToPlant(). + const Frame& objects_frame = plant.GetFrameByName("objects_frame"); + + // This will trigger the computation of position kinematics. + const RigidTransformd& X_WM = mug.EvalPoseInWorld(*context); + + // From test::AddFixedObjectsToPlant() we know the fixed pose of the mug frame + // M in the objects frame O. + const RigidTransformd X_OM = Translation3d(0.0, 0.0, 0.05); + // Therefore we expect the pose of the mug to be: + const RigidTransformd& X_WM_expected = + objects_frame.CalcPoseInWorld(*context) * X_OM; + + // We verify the results. + EXPECT_TRUE(CompareMatrices(X_WM.GetAsMatrix34(), + X_WM_expected.GetAsMatrix34(), kTolerance)); + + // Now we evaluate some velocity kinematics. + const SpatialVelocity& V_WM = + mug.EvalSpatialVelocityInWorld(*context); + // Since all bodies are anchored, they all have zero spatial velocity. + EXPECT_EQ(V_WM.get_coeffs(), Vector6::Zero()); +} + } // namespace } // namespace multibody } // namespace drake diff --git a/multibody/test_utilities/add_fixed_objects_to_plant.h b/multibody/test_utilities/add_fixed_objects_to_plant.h index a0c474f457bc..bdecf2847f79 100644 --- a/multibody/test_utilities/add_fixed_objects_to_plant.h +++ b/multibody/test_utilities/add_fixed_objects_to_plant.h @@ -4,7 +4,6 @@ #include #include "drake/common/find_resource.h" -#include "drake/common/test_utilities/expect_no_throw.h" #include "drake/math/rigid_transform.h" #include "drake/math/rotation_matrix.h" #include "drake/multibody/parsing/parser.h" diff --git a/multibody/tree/body_node.h b/multibody/tree/body_node.h index 549db1389846..469e3ac19c8a 100644 --- a/multibody/tree/body_node.h +++ b/multibody/tree/body_node.h @@ -869,6 +869,7 @@ class BodyNode : public MultibodyElement { DRAKE_DEMAND(static_cast(H_array.size()) == this->get_parent_tree().num_velocities()); const int start_index_in_v = get_topology().mobilizer_velocities_start_in_v; + DRAKE_DEMAND(start_index_in_v < this->get_parent_tree().num_velocities()); const int num_velocities = get_topology().num_mobilizer_velocities; // The first column of this node's hinge matrix H_PB_W: const Vector6& H_col0 = H_array[start_index_in_v]; @@ -879,7 +880,10 @@ class BodyNode : public MultibodyElement { /// Mutable version of GetJacobianFromArray(). Eigen::Map> GetMutableJacobianFromArray( std::vector>* H_array) const { + DRAKE_DEMAND(static_cast(H_array->size()) == + this->get_parent_tree().num_velocities()); const int start_index_in_v = get_topology().mobilizer_velocities_start_in_v; + DRAKE_DEMAND(start_index_in_v < this->get_parent_tree().num_velocities()); const int num_velocities = get_topology().num_mobilizer_velocities; // The first column of this node's hinge matrix H_PB_W: Vector6& H_col0 = (*H_array)[start_index_in_v]; diff --git a/multibody/tree/multibody_tree.cc b/multibody/tree/multibody_tree.cc index a302b4d7eb47..7b29b9e3fbb6 100644 --- a/multibody/tree/multibody_tree.cc +++ b/multibody/tree/multibody_tree.cc @@ -527,6 +527,13 @@ void MultibodyTree::CalcVelocityKinematicsCache( // TODO(amcastro-tri): Loop over bodies to compute velocity kinematics updates // corresponding to flexible bodies. + // If the model has zero dofs we simply set all spatial velocities to zero and + // return since there is no work to be done. + if (num_velocities() == 0) { + vc->InitializeToZero(); + return; + } + const std::vector>& H_PB_W_cache = EvalAcrossNodeJacobianWrtVExpressedInWorld(context); @@ -1260,6 +1267,9 @@ void MultibodyTree::CalcAcrossNodeJacobianWrtVExpressedInWorld( DRAKE_DEMAND(H_PB_W_cache != nullptr); DRAKE_DEMAND(static_cast(H_PB_W_cache->size()) == num_velocities()); + // Quick return on nv = 0. Nothing to compute. + if (num_velocities() == 0) return; + for (BodyNodeIndex node_index(1); node_index < num_bodies(); ++node_index) { const BodyNode& node = *body_nodes_[node_index];