Changed DoDifferentialInverseKinematics to take MultibodyPlant instea…

…d of MultibodyTree (RobotLocomotion#9662)

* Changed DoDifferentialInverseKinematics to take MultibodyPlant instead of MultibodyTree.

bindings/pydrake/manipulation/planner_py.cc

@@ -107,7 +107,7 @@ PYBIND11_MODULE(planner, m) {
         doc.DoDifferentialInverseKinematics.doc);
 
   m.def("DoDifferentialInverseKinematics",
-        [](const multibody::MultibodyTree<double>& robot,
+        [](const multibody::multibody_plant::MultibodyPlant<double>& robot,
            const systems::Context<double>& context,
            const Vector6<double>& V_WE_desired,
            const multibody::Frame<double>& frame_E,
@@ -116,12 +116,12 @@ PYBIND11_MODULE(planner, m) {
           return manipulation::planner::DoDifferentialInverseKinematics(
               robot, context, V_WE_desired, frame_E, parameters);
         },
-        py::arg("robot"), py::arg("context"),
-        py::arg("V_WE_desired"), py::arg("frame_E"), py::arg("parameters"),
+        py::arg("robot"), py::arg("context"), py::arg("V_WE_desired"),
+        py::arg("frame_E"), py::arg("parameters"),
         doc.DoDifferentialInverseKinematics.doc_4);
 
   m.def("DoDifferentialInverseKinematics",
-        [](const multibody::MultibodyTree<double>& robot,
+        [](const multibody::multibody_plant::MultibodyPlant<double>& robot,
            const systems::Context<double>& context,
            const Isometry3<double>& X_WE_desired,
            const multibody::Frame<double>& frame_E,
@@ -130,8 +130,8 @@ PYBIND11_MODULE(planner, m) {
           return manipulation::planner::DoDifferentialInverseKinematics(
               robot, context, X_WE_desired, frame_E, parameters);
         },
-        py::arg("robot"), py::arg("context"),
-        py::arg("X_WE_desired"), py::arg("frame_E"), py::arg("parameters"),
+        py::arg("robot"), py::arg("context"), py::arg("X_WE_desired"),
+        py::arg("frame_E"), py::arg("parameters"),
         doc.DoDifferentialInverseKinematics.doc_5);
 }
 

bindings/pydrake/manipulation/test/planner_test.py

@@ -73,9 +73,9 @@ def test_mbp_overloads(self):
         frame = plant.GetFrameByName("Link2")
         parameters = mut.DifferentialInverseKinematicsParameters(2, 2)
 
-        mut.DoDifferentialInverseKinematics(plant.model(), context,
+        mut.DoDifferentialInverseKinematics(plant, context,
                                             np.zeros(6), frame, parameters)
 
-        mut.DoDifferentialInverseKinematics(plant.model(), context,
+        mut.DoDifferentialInverseKinematics(plant, context,
                                             Isometry3.Identity(), frame,
                                             parameters)

manipulation/planner/BUILD.bazel

@@ -40,7 +40,7 @@ drake_cc_library(
     deps = [
         "//attic/multibody/parsers",
         "//multibody:rigid_body_tree",
-        "//multibody/multibody_tree",
+        "//multibody/multibody_tree/multibody_plant",
         "//solvers:mathematical_program",
     ],
 )
@@ -89,7 +89,7 @@ drake_cc_googletest(
         "//common/test_utilities:eigen_matrix_compare",
         "//examples/kuka_iiwa_arm:iiwa_common",
         "//manipulation/util:world_sim_tree_builder",
-        "//multibody/benchmarks/kuka_iiwa_robot",
+        "//multibody/multibody_tree/parsing",
     ],
 )
 

manipulation/planner/differential_inverse_kinematics.cc

@@ -250,6 +250,42 @@ DifferentialInverseKinematicsResult DoDifferentialInverseKinematics(
                                          V_WE_desired, parameters);
 }
 
+DifferentialInverseKinematicsResult DoDifferentialInverseKinematics(
+    const multibody::multibody_plant::MultibodyPlant<double>& plant,
+    const systems::Context<double>& context,
+    const Vector6<double>& V_WE_desired,
+    const multibody::Frame<double>& frame_E,
+    const DifferentialInverseKinematicsParameters& parameters) {
+  const multibody::MultibodyTree<double>& robot = plant.tree();
+  const Isometry3<double> X_WE =
+      robot.CalcRelativeTransform(context, robot.world_frame(), frame_E);
+  MatrixX<double> J_WE(6, robot.num_velocities());
+  robot.CalcFrameGeometricJacobianExpressedInWorld(
+      context, frame_E, Vector3<double>::Zero(), &J_WE);
+
+  const auto& mbt_context =
+      dynamic_cast<const multibody::MultibodyTreeContext<double>&>(context);
+  return DoDifferentialInverseKinematics(mbt_context.get_positions(),
+                                         mbt_context.get_velocities(), X_WE,
+                                         J_WE, V_WE_desired, parameters);
+}
+
+DifferentialInverseKinematicsResult DoDifferentialInverseKinematics(
+    const multibody::multibody_plant::MultibodyPlant<double>& plant,
+    const systems::Context<double>& context,
+    const Isometry3<double>& X_WE_desired,
+    const multibody::Frame<double>& frame_E,
+    const DifferentialInverseKinematicsParameters& parameters) {
+  const Isometry3<double> X_WE =
+      plant.tree().EvalBodyPoseInWorld(context, frame_E.body()) *
+      frame_E.CalcPoseInBodyFrame(context);
+  const Vector6<double> V_WE_desired =
+      ComputePoseDiffInCommonFrame(X_WE, X_WE_desired) /
+      parameters.get_timestep();
+  return DoDifferentialInverseKinematics(plant, context, V_WE_desired, frame_E,
+                                         parameters);
+}
+
 DifferentialInverseKinematicsResult DoDifferentialInverseKinematics(
     const multibody::MultibodyTree<double>& robot,
     const systems::Context<double>& context,
@@ -281,8 +317,11 @@ DifferentialInverseKinematicsResult DoDifferentialInverseKinematics(
   const Vector6<double> V_WE_desired =
       ComputePoseDiffInCommonFrame(X_WE, X_WE_desired) /
       parameters.get_timestep();
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
   return DoDifferentialInverseKinematics(robot, context, V_WE_desired, frame_E,
                                          parameters);
+#pragma GCC diagnostic pop  // pop -Wdeprecated-declarations
 }
 
 }  // namespace planner

manipulation/planner/differential_inverse_kinematics.h

@@ -9,9 +9,10 @@
 
 #include "drake/common/copyable_unique_ptr.h"
 #include "drake/common/drake_copyable.h"
+#include "drake/common/drake_deprecated.h"
 #include "drake/common/drake_optional.h"
 #include "drake/common/eigen_types.h"
-#include "drake/multibody/multibody_tree/multibody_tree.h"
+#include "drake/multibody/multibody_tree/multibody_plant/multibody_plant.h"
 #include "drake/multibody/rigid_body_tree.h"
 #include "drake/solvers/mathematical_program.h"
 
@@ -305,8 +306,9 @@ DifferentialInverseKinematicsResult DoDifferentialInverseKinematics(
  * transforming @p V_WE to V_WE_E, then taking the element-wise product between
  * V_WE_E and the gains (specified in frame E) in @p parameters, and only
  * selecting the non zero elements. J is computed similarly.
- * @param robot A MultibodyTree model.
- * @param context Contains the current generalized position and velocity.
+ * @param robot A MultibodyPlant model.
+ * @param context Must be the Context of the MultibodyPlant. Contains the
+ * current generalized position and velocity.
  * @param V_WE_desired Desired world frame spatial velocity of @p frame_E.
  * @param frame_E End effector frame.
  * @param parameters Collection of various problem specific constraints and
@@ -315,7 +317,7 @@ DifferentialInverseKinematicsResult DoDifferentialInverseKinematics(
  * be set to v, otherwise it will be nullopt.
  */
 DifferentialInverseKinematicsResult DoDifferentialInverseKinematics(
-    const multibody::MultibodyTree<double>& robot,
+    const multibody::multibody_plant::MultibodyPlant<double>& robot,
     const systems::Context<double>& context,
     const Vector6<double>& V_WE_desired,
     const multibody::Frame<double>& frame_E,
@@ -328,15 +330,38 @@ DifferentialInverseKinematicsResult DoDifferentialInverseKinematics(
  * q_current and v_current are taken from @p cache. V_WE is computed by
  * ComputePoseDiffInCommonFrame(X_WE, X_WE_desired) / dt, where X_WE is computed
  * from @p context, and dt is taken from @p parameters.
- * @param robot A MultibodyTree model.
- * @param context Contains the current generalized position and velocity.
+ * @param robot A MultibodyPlant model.
+ * @param context Must be the Context of the MultibodyPlant. Contains the
+ * current generalized position and velocity.
  * @param X_WE_desired Desired pose of @p frame_E in the world frame.
  * @param frame_E End effector frame.
  * @param parameters Collection of various problem specific constraints and
  * constants.
  * @return If the solver successfully finds a solution, joint_velocities will
  * be set to v, otherwise it will be nullopt.
  */
+DifferentialInverseKinematicsResult DoDifferentialInverseKinematics(
+    const multibody::multibody_plant::MultibodyPlant<double>& robot,
+    const systems::Context<double>& context,
+    const Isometry3<double>& X_WE_desired,
+    const multibody::Frame<double>& frame_E,
+    const DifferentialInverseKinematicsParameters& parameters);
+
+/**
+ * Deprecated: Please use the MultibodyPlant version.
+ */
+DRAKE_DEPRECATED("Please use the MultibodyPlant version.")
+DifferentialInverseKinematicsResult DoDifferentialInverseKinematics(
+    const multibody::MultibodyTree<double>& robot,
+    const systems::Context<double>& context,
+    const Vector6<double>& V_WE_desired,
+    const multibody::Frame<double>& frame_E,
+    const DifferentialInverseKinematicsParameters& parameters);
+
+/**
+ * Deprecated: Please use the MultibodyPlant version.
+ */
+DRAKE_DEPRECATED("Please use the MultibodyPlant version.")
 DifferentialInverseKinematicsResult DoDifferentialInverseKinematics(
     const multibody::MultibodyTree<double>& robot,
     const systems::Context<double>& context,

manipulation/planner/test/differential_inverse_kinematics_test.cc

@@ -12,7 +12,7 @@
 #include "drake/common/find_resource.h"
 #include "drake/common/test_utilities/eigen_matrix_compare.h"
 #include "drake/examples/kuka_iiwa_arm/iiwa_common.h"
-#include "drake/multibody/benchmarks/kuka_iiwa_robot/make_kuka_iiwa_model.h"
+#include "drake/multibody/multibody_tree/parsing/multibody_plant_sdf_parser.h"
 #include "drake/multibody/parsers/urdf_parser.h"
 #include "drake/multibody/rigid_body.h"
 #include "drake/multibody/rigid_body_frame.h"
@@ -26,13 +26,11 @@ namespace planner {
 namespace {
 
 using examples::kuka_iiwa_arm::get_iiwa_max_joint_velocities;
-using multibody::MultibodyTree;
-using multibody::MultibodyTreeSystem;
-using multibody::RevoluteJoint;
+using multibody::multibody_plant::MultibodyPlant;
 using multibody::FixedOffsetFrame;
 using solvers::LinearConstraint;
 
-std::unique_ptr<RigidBodyTree<double>> BuildTree() {
+std::unique_ptr<RigidBodyTree<double>> BuildRigidBodyTree() {
   const std::string iiwa_absolute_path = FindResourceOrThrow(
       "drake/manipulation/models/iiwa_description/urdf/"
       "iiwa14_primitive_collision.urdf");
@@ -42,10 +40,20 @@ std::unique_ptr<RigidBodyTree<double>> BuildTree() {
   return tree;
 }
 
+std::unique_ptr<MultibodyPlant<double>> BuildMultibodyPlant() {
+  const std::string iiwa_absolute_path = FindResourceOrThrow(
+      "drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf");
+  auto plant = std::make_unique<MultibodyPlant<double>>();
+  drake::multibody::parsing::AddModelFromSdfFile(iiwa_absolute_path, "iiwa",
+                                                 plant.get());
+  plant->WeldFrames(plant->world_frame(), plant->GetFrameByName("iiwa_link_0"));
+  return plant;
+}
+
 class DifferentialInverseKinematicsTest : public ::testing::Test {
  protected:
   void SetUp() {
-    tree_ = BuildTree();
+    tree_ = BuildRigidBodyTree();
     const Isometry3<double> X_7E =
         Translation3<double>(Vector3<double>(0.1, 0, 0)) *
         AngleAxis<double>(M_PI, Vector3<double>::UnitZ());
@@ -75,37 +83,24 @@ class DifferentialInverseKinematicsTest : public ::testing::Test {
     params_->set_joint_velocity_limits(v_bounds);
 
     // For the MBT version.
-    auto model =
-        multibody::benchmarks::kuka_iiwa_robot::MakeKukaIiwaModel<double>(false,
-                                                                          9.81);
-    frame_E_mbt_ = &model->AddFrame<FixedOffsetFrame>(
-        model->GetBodyByName("iiwa_link_7").body_frame(),
-        frame_E_->get_transform_to_body());
-    mbt_system_ =
-        std::make_unique<MultibodyTreeSystem<double>>(std::move(model));
-    mbt_ = &mbt_system_->tree();
-
-    context_ = mbt_system_->CreateDefaultContext();
-    joints_.push_back(&mbt_->GetJointByName<RevoluteJoint>("iiwa_joint_1"));
-    joints_.push_back(&mbt_->GetJointByName<RevoluteJoint>("iiwa_joint_2"));
-    joints_.push_back(&mbt_->GetJointByName<RevoluteJoint>("iiwa_joint_3"));
-    joints_.push_back(&mbt_->GetJointByName<RevoluteJoint>("iiwa_joint_4"));
-    joints_.push_back(&mbt_->GetJointByName<RevoluteJoint>("iiwa_joint_5"));
-    joints_.push_back(&mbt_->GetJointByName<RevoluteJoint>("iiwa_joint_6"));
-    joints_.push_back(&mbt_->GetJointByName<RevoluteJoint>("iiwa_joint_7"));
+    mbp_ = BuildMultibodyPlant();
+    frame_E_mbt_ = &mbp_->AddFrame(std::make_unique<FixedOffsetFrame<double>>(
+        mbp_->tree().GetBodyByName("iiwa_link_7").body_frame(),
+        frame_E_->get_transform_to_body()));
+    mbp_->Finalize();
+
+    context_ = mbp_->CreateDefaultContext();
 
     SetMBTState(q, v);
   }
 
   void SetMBTState(const VectorX<double>& q, const VectorX<double>& v) {
-    DRAKE_DEMAND(q.size() == mbt_->num_positions());
-    DRAKE_DEMAND(v.size() == mbt_->num_velocities());
-    int angle_index = 0;
-    for (const RevoluteJoint<double>* joint : joints_) {
-      joint->set_angle(context_.get(), q[angle_index]);
-      joint->set_angular_rate(context_.get(), v[angle_index]);
-      angle_index++;
-    }
+    DRAKE_DEMAND(q.size() == mbp_->num_positions());
+    DRAKE_DEMAND(v.size() == mbp_->num_velocities());
+    auto context =
+        dynamic_cast<multibody::MultibodyTreeContext<double>*>(context_.get());
+    context->get_mutable_positions() = q;
+    context->get_mutable_velocities() = v;
   }
 
   void CheckPositiveResult(const Vector6<double>& V_WE,
@@ -146,10 +141,8 @@ class DifferentialInverseKinematicsTest : public ::testing::Test {
   std::shared_ptr<RigidBodyFrame<double>> frame_E_;
   std::unique_ptr<DifferentialInverseKinematicsParameters> params_;
 
-  std::unique_ptr<MultibodyTreeSystem<double>> mbt_system_;
-  const MultibodyTree<double>* mbt_{};
+  std::unique_ptr<MultibodyPlant<double>> mbp_;
   std::unique_ptr<systems::Context<double>> context_;
-  std::vector<const RevoluteJoint<double>*> joints_;
   const FixedOffsetFrame<double>* frame_E_mbt_;
 };
 
@@ -202,27 +195,42 @@ TEST_F(DifferentialInverseKinematicsTest, MultiBodyTreeTest) {
   DifferentialInverseKinematicsResult rbt_result =
       DoDifferentialInverseKinematics(*tree_, *cache_, V_WE, *frame_E_,
                                       *params_);
-  DifferentialInverseKinematicsResult mbt_result =
-      DoDifferentialInverseKinematics(*mbt_, *context_, V_WE, *frame_E_mbt_,
+  DifferentialInverseKinematicsResult mbp_result =
+      DoDifferentialInverseKinematics(*mbp_, *context_, V_WE, *frame_E_mbt_,
                                       *params_);
   // TODO(siyuanfeng-tri) Ideally a smaller tolerance would pass, but there
   // seems to be differences in the RBT and MBT outcomes for unknown reasons.
   EXPECT_TRUE(CompareMatrices(rbt_result.joint_velocities.value(),
-                              mbt_result.joint_velocities.value(),
-                              1e5 * eps));
+                              mbp_result.joint_velocities.value(), 1e5 * eps));
+// Test MBP and MBT version gives the same answer.
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+  DifferentialInverseKinematicsResult mbt_result =
+      DoDifferentialInverseKinematics(mbp_->tree(), *context_, V_WE,
+                                      *frame_E_mbt_, *params_);
+  EXPECT_TRUE(CompareMatrices(mbp_result.joint_velocities.value(),
+                              mbt_result.joint_velocities.value(), eps));
+#pragma GCC diagnostic pop  // pop -Wdeprecated-declarations
 
   Isometry3<double> X_WE_desired =
       Translation3<double>(Vector3<double>(0.1, 0.2, 0.3)) *
       AngleAxis<double>(3.44, Vector3<double>(0.3, -0.2, 0.1).normalized());
   rbt_result = DoDifferentialInverseKinematics(*tree_, *cache_, X_WE_desired,
                                                *frame_E_, *params_);
-  mbt_result = DoDifferentialInverseKinematics(*mbt_, *context_, X_WE_desired,
+  mbp_result = DoDifferentialInverseKinematics(*mbp_, *context_, X_WE_desired,
                                                *frame_E_mbt_, *params_);
   // TODO(siyuanfeng-tri) Ideally a smaller tolerance would pass, but there
   // seems to be differences in the RBT and MBT outcomes for unknown reasons.
   EXPECT_TRUE(CompareMatrices(rbt_result.joint_velocities.value(),
-                              mbt_result.joint_velocities.value(),
-                              1e7 * eps));
+                              mbp_result.joint_velocities.value(), 1e7 * eps));
+// Test MBP and MBT version gives the same answer.
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+  mbt_result = DoDifferentialInverseKinematics(
+      mbp_->tree(), *context_, X_WE_desired, *frame_E_mbt_, *params_);
+  EXPECT_TRUE(CompareMatrices(mbp_result.joint_velocities.value(),
+                              mbt_result.joint_velocities.value(), eps));
+#pragma GCC diagnostic pop  // pop -Wdeprecated-declarations
 }
 
 TEST_F(DifferentialInverseKinematicsTest, GainTest) {