Enable kinematics computations on models with zero dofs (RobotLocomot…

…ion#13405)

* Enables kinematics for models with zero dofs.

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",
     ],
 )
 

multibody/plant/test/frame_kinematics_test.cc

@@ -6,13 +6,15 @@
 #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"
 
 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<double>::epsilon();
+
+  MultibodyPlant<double> plant(0.0);
+  test::AddFixedObjectsToPlant(&plant);
+  plant.Finalize();
+  std::unique_ptr<Context<double>> 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<double>& mug = plant.GetBodyByName("main_body");
+
+  // The objects frame O is affixed to a robot table defined by
+  // test::AddFixedObjectsToPlant().
+  const Frame<double>& 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<double>& V_WM =
+      mug.EvalSpatialVelocityInWorld(*context);
+  // Since all bodies are anchored, they all have zero spatial velocity.
+  EXPECT_EQ(V_WM.get_coeffs(), Vector6<double>::Zero());
+}
+
 }  // namespace
 }  // namespace multibody
 }  // namespace drake

multibody/test_utilities/add_fixed_objects_to_plant.h

@@ -4,7 +4,6 @@
 #include <string>
 
 #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"

multibody/tree/body_node.h

@@ -869,6 +869,7 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
     DRAKE_DEMAND(static_cast<int>(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<T>& H_col0 = H_array[start_index_in_v];
@@ -879,7 +880,10 @@ class BodyNode : public MultibodyElement<BodyNode, T, BodyNodeIndex> {
   /// Mutable version of GetJacobianFromArray().
   Eigen::Map<MatrixUpTo6<T>> GetMutableJacobianFromArray(
       std::vector<Vector6<T>>* H_array) const {
+    DRAKE_DEMAND(static_cast<int>(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<T>& H_col0 = (*H_array)[start_index_in_v];

multibody/tree/multibody_tree.cc

@@ -527,6 +527,13 @@ void MultibodyTree<T>::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<Vector6<T>>& H_PB_W_cache =
       EvalAcrossNodeJacobianWrtVExpressedInWorld(context);
 
@@ -1260,6 +1267,9 @@ void MultibodyTree<T>::CalcAcrossNodeJacobianWrtVExpressedInWorld(
   DRAKE_DEMAND(H_PB_W_cache != nullptr);
   DRAKE_DEMAND(static_cast<int>(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<T>& node = *body_nodes_[node_index];