Merge pull request RobotLocomotion#12732 from sammy-tri/constraint_re…

…laxing_ik_mbp

 Add a MultibodyPlant version of ConstraintRelaxingIk

attic/manipulation/planner/BUILD.bazel

@@ -15,15 +15,15 @@ package(
 drake_cc_package_library(
     name = "planner",
     deps = [
-        ":constraint_relaxing_ik",
+        ":constraint_relaxing_ik_rbt",
         ":rbt_differential_inverse_kinematics",
     ],
 )
 
 drake_cc_library(
-    name = "constraint_relaxing_ik",
-    srcs = ["constraint_relaxing_ik.cc"],
-    hdrs = ["constraint_relaxing_ik.h"],
+    name = "constraint_relaxing_ik_rbt",
+    srcs = ["constraint_relaxing_ik_rbt.cc"],
+    hdrs = ["constraint_relaxing_ik_rbt.h"],
     deps = [
         "//attic/multibody:inverse_kinematics",
         "//attic/multibody:rigid_body_tree",
@@ -46,9 +46,10 @@ drake_cc_library(
 # === test/ ===
 
 drake_cc_googletest(
-    name = "constraint_relaxing_ik_test",
+    name = "constraint_relaxing_ik_rbt_test",
     timeout = "moderate",
-    srcs = ["test/constraint_relaxing_ik_test.cc"],
+    srcs = ["test/constraint_relaxing_ik_rbt_test.cc"],
+    copts = ["-Wno-deprecated-declarations"],
     data = [
         "//manipulation/models/iiwa_description:models",
     ],
@@ -58,7 +59,7 @@ drake_cc_googletest(
     # should enable this test.
     tags = ["snopt"],
     deps = [
-        ":constraint_relaxing_ik",
+        ":constraint_relaxing_ik_rbt",
         "//common:find_resource",
     ],
 )

attic/manipulation/planner/constraint_relaxing_ik.cc → attic/manipulation/planner/constraint_relaxing_ik_rbt.cc

@@ -1,4 +1,4 @@
-#include "drake/manipulation/planner/constraint_relaxing_ik.h"
+#include "drake/manipulation/planner/constraint_relaxing_ik_rbt.h"
 
 #include <memory>
 
@@ -16,7 +16,7 @@ namespace {
 constexpr int kDefaultRandomSeed = 1234;
 }  // namespace
 
-ConstraintRelaxingIk::ConstraintRelaxingIk(
+ConstraintRelaxingIkRbt::ConstraintRelaxingIkRbt(
     const std::string& model_path,
     const std::string& end_effector_link_name,
     const Isometry3<double>& base_to_world)
@@ -32,7 +32,7 @@ ConstraintRelaxingIk::ConstraintRelaxingIk(
   SetEndEffector(end_effector_link_name);
 }
 
-bool ConstraintRelaxingIk::PlanSequentialTrajectory(
+bool ConstraintRelaxingIkRbt::PlanSequentialTrajectory(
     const std::vector<IkCartesianWaypoint>& waypoints,
     const VectorX<double>& q_current, IKResults* ik_res) {
   DRAKE_DEMAND(ik_res);
@@ -149,7 +149,7 @@ bool ConstraintRelaxingIk::PlanSequentialTrajectory(
   return true;
 }
 
-bool ConstraintRelaxingIk::SolveIk(
+bool ConstraintRelaxingIkRbt::SolveIk(
     const IkCartesianWaypoint& waypoint,
     const VectorX<double>& q0,
     const VectorX<double>& q_nom,

attic/manipulation/planner/constraint_relaxing_ik.h → attic/manipulation/planner/constraint_relaxing_ik_rbt.h

@@ -6,6 +6,7 @@
 #include <vector>
 
 #include "drake/common/drake_copyable.h"
+#include "drake/common/drake_deprecated.h"
 #include "drake/common/eigen_types.h"
 #include "drake/common/trajectories/piecewise_polynomial.h"
 #include "drake/multibody/rigid_body_ik.h"
@@ -18,9 +19,10 @@ namespace planner {
  * A wrapper class around the IK planner. This class improves IK's usability by
  * handling constraint relaxing and multiple initial guesses internally.
  */
-class ConstraintRelaxingIk {
+class DRAKE_DEPRECATED("2020-06-01", "Use ConstraintRelaxingIk instead")
+ConstraintRelaxingIkRbt {
  public:
-  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ConstraintRelaxingIk);
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ConstraintRelaxingIkRbt);
 
   /**
    * Cartesian waypoint. Input to the IK solver.
@@ -46,9 +48,9 @@ class ConstraintRelaxingIk {
    * @param base_to_world X_WB, transformation from robot's base to the world
    * frame.
    */
-  ConstraintRelaxingIk(const std::string& model_path,
-                       const std::string& end_effector_link_name,
-                       const Isometry3<double>& base_to_world);
+  ConstraintRelaxingIkRbt(const std::string& model_path,
+                          const std::string& end_effector_link_name,
+                          const Isometry3<double>& base_to_world);
 
   /**
    * Sets end effector to @p end_effector_body.

attic/manipulation/planner/test/constraint_relaxing_ik_test.cc → attic/manipulation/planner/test/constraint_relaxing_ik_rbt_test.cc

@@ -1,4 +1,4 @@
-#include "drake/manipulation/planner/constraint_relaxing_ik.h"
+#include "drake/manipulation/planner/constraint_relaxing_ik_rbt.h"
 
 #include <gtest/gtest.h>
 
@@ -23,7 +23,7 @@ inline double get_orientation_difference(const Matrix3<double>& rot0,
 // joint angles that would achieve these poses. This test checks that
 // an IK solution can be computed, and that the resulting pose lies
 // within the given tolerance from the forward kinematics poses.
-GTEST_TEST(ConstraintRelaxingIkTest, SolveIkFromFk) {
+GTEST_TEST(ConstraintRelaxingIkRbtTest, SolveIkFromFk) {
   const std::string kModelPath = FindResourceOrThrow(
       "drake/manipulation/models/iiwa_description/urdf/"
       "iiwa14_polytope_collision.urdf");
@@ -38,13 +38,13 @@ GTEST_TEST(ConstraintRelaxingIkTest, SolveIkFromFk) {
   const RigidBody<double>* end_effector = iiwa->FindBody(kEndEffectorLinkName);
 
   IKResults ik_res;
-  ConstraintRelaxingIk ik_planner(kModelPath, kEndEffectorLinkName,
-                               Isometry3<double>::Identity());
-  ConstraintRelaxingIk::IkCartesianWaypoint wp;
+  ConstraintRelaxingIkRbt ik_planner(kModelPath, kEndEffectorLinkName,
+                                     Isometry3<double>::Identity());
+  ConstraintRelaxingIkRbt::IkCartesianWaypoint wp;
   wp.pos_tol = Vector3<double>(0.001, 0.001, 0.001);
   wp.rot_tol = 0.005;
   wp.constrain_orientation = true;
-  std::vector<ConstraintRelaxingIk::IkCartesianWaypoint> waypoints(1, wp);
+  std::vector<ConstraintRelaxingIkRbt::IkCartesianWaypoint> waypoints(1, wp);
 
   const VectorX<double> kQcurrent = iiwa->getZeroConfiguration();
   VectorX<double> q_fk;

examples/kuka_iiwa_arm/BUILD.bazel

@@ -42,7 +42,6 @@ drake_cc_library(
     name = "iiwa_lcm",
     srcs = ["iiwa_lcm.cc"],
     hdrs = ["iiwa_lcm.h"],
-    # TODO(hongkai.dai): move this package to manipulation folder.
     visibility = ["//visibility:public"],
     deps = [
         ":iiwa_common",
@@ -191,6 +190,8 @@ drake_cc_binary(
     deps = [
         ":iiwa_common",
         ":iiwa_lcm",
+        "//multibody/parsing",
+        "//multibody/plant",
         "@lcmtypes_bot2_core",
         "@lcmtypes_robotlocomotion",
     ],
@@ -205,11 +206,12 @@ drake_cc_binary(
     ],
     deps = [
         ":iiwa_common",
-        "//attic/manipulation/planner:constraint_relaxing_ik",
-        "//attic/multibody/parsers",
         "//common:add_text_logging_gflags",
         "//lcmtypes:iiwa",
+        "//manipulation/planner:constraint_relaxing_ik",
         "//math:geometric_transform",
+        "//multibody/parsing",
+        "//multibody/plant",
         "@lcm",
         "@lcmtypes_bot2_core",
         "@lcmtypes_robotlocomotion",
@@ -232,6 +234,7 @@ alias(
 
 drake_cc_googletest(
     name = "iiwa_lcm_test",
+    copts = ["-Wno-deprecated-declarations"],
     data = [
         "//manipulation/models/iiwa_description:models",
     ],

examples/kuka_iiwa_arm/iiwa_common.cc

@@ -177,7 +177,6 @@ void ApplyJointVelocityLimits(const MatrixX<double>& keyframes,
   }
 }
 
-
 robotlocomotion::robot_plan_t EncodeKeyFrames(
     const RigidBodyTree<double>& robot,
     const std::vector<double>& time,

examples/kuka_iiwa_arm/iiwa_common.h

@@ -38,6 +38,7 @@ Matrix6<T> ComputeLumpedGripperInertiaInEndEffectorFrame(
 
 /// Verifies that @p tree matches assumptions about joint indices.
 /// Aborts if the tree isn't as expected.
+DRAKE_DEPRECATED("2020-06-01", "This function is being removed.")
 void VerifyIiwaTree(const RigidBodyTree<double>& tree);
 
 /// Builds a RigidBodyTree at the specified @position and @orientation from
@@ -69,6 +70,7 @@ void ApplyJointVelocityLimits(const MatrixX<double>& keyframes,
 /// Makes a robotlocomotion::robot_plan_t message.  The number of
 /// columns in @p keyframes must match the size of @p time.  Times
 /// must be in strictly increasing order.
+DRAKE_DEPRECATED("2020-06-01", "This function is being removed.")
 robotlocomotion::robot_plan_t EncodeKeyFrames(
     const RigidBodyTree<double>& robot, const std::vector<double>& time,
     const std::vector<int>& info, const MatrixX<double>& keyframes);

examples/kuka_iiwa_arm/iiwa_lcm.h

@@ -34,7 +34,8 @@ namespace kuka_iiwa_arm {
  * that contact force is the only cause of external joint torque, no other
  * effects such as friction is considered.
  */
-class IiwaContactResultsToExternalTorque : public systems::LeafSystem<double> {
+class DRAKE_DEPRECATED("2020-06-01", "This class is being removed.")
+IiwaContactResultsToExternalTorque : public systems::LeafSystem<double> {
  public:
   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(IiwaContactResultsToExternalTorque)
 

examples/kuka_iiwa_arm/kuka_plan_runner.cc

@@ -20,12 +20,10 @@
 #include "drake/common/drake_assert.h"
 #include "drake/common/find_resource.h"
 #include "drake/common/trajectories/piecewise_polynomial.h"
-#include "drake/examples/kuka_iiwa_arm/iiwa_common.h"
 #include "drake/lcmt_iiwa_command.hpp"
 #include "drake/lcmt_iiwa_status.hpp"
-#include "drake/multibody/joints/floating_base_types.h"
-#include "drake/multibody/parsers/urdf_parser.h"
-#include "drake/multibody/rigid_body_tree.h"
+#include "drake/multibody/parsing/parser.h"
+#include "drake/multibody/plant/multibody_plant.h"
 
 using Eigen::MatrixXd;
 using Eigen::VectorXd;
@@ -52,10 +50,9 @@ typedef PPType::PolynomialMatrix PPMatrix;
 
 class RobotPlanRunner {
  public:
-  /// tree is aliased
-  explicit RobotPlanRunner(const RigidBodyTree<double>& tree)
-      : tree_(tree), plan_number_(0) {
-    VerifyIiwaTree(tree);
+  /// plant is aliased
+  explicit RobotPlanRunner(const multibody::MultibodyPlant<double>& plant)
+      : plant_(plant), plan_number_(0) {
     lcm_.subscribe(kLcmStatusChannel,
                     &RobotPlanRunner::HandleStatus, this);
     lcm_.subscribe(kLcmPlanChannel,
@@ -109,12 +106,12 @@ class RobotPlanRunner {
   }
 
  private:
-  void HandleStatus(const lcm::ReceiveBuffer*, const std::string&,
+  void HandleStatus(const ::lcm::ReceiveBuffer*, const std::string&,
                     const lcmt_iiwa_status* status) {
     iiwa_status_ = *status;
   }
 
-  void HandlePlan(const lcm::ReceiveBuffer*, const std::string&,
+  void HandlePlan(const ::lcm::ReceiveBuffer*, const std::string&,
                   const robotlocomotion::robot_plan_t* plan) {
     std::cout << "New plan received." << std::endl;
     if (iiwa_status_.utime == -1) {
@@ -128,24 +125,27 @@ class RobotPlanRunner {
 
     std::vector<Eigen::MatrixXd> knots(plan->num_states,
                                        Eigen::MatrixXd::Zero(kNumJoints, 1));
-    std::map<std::string, int> name_to_idx =
-        tree_.computePositionNameToIndexMap();
     for (int i = 0; i < plan->num_states; ++i) {
       const auto& state = plan->plan[i];
       for (int j = 0; j < state.num_joints; ++j) {
-        if (name_to_idx.count(state.joint_name[j]) == 0) {
+        if (!plant_.HasJointNamed(state.joint_name[j])) {
           continue;
         }
+        const multibody::Joint<double>& joint =
+            plant_.GetJointByName(state.joint_name[j]);
+        DRAKE_DEMAND(joint.num_positions() == 1);
+        const int idx = joint.position_start();
+        DRAKE_DEMAND(idx < kNumJoints);
+
         // Treat the matrix at knots[i] as a column vector.
         if (i == 0) {
           // Always start moving from the position which we're
           // currently commanding.
           DRAKE_DEMAND(iiwa_status_.utime != -1);
-          knots[0](name_to_idx[state.joint_name[j]], 0) =
-              iiwa_status_.joint_position_commanded[j];
+          knots[0](idx, 0) = iiwa_status_.joint_position_commanded[j];
+
         } else {
-          knots[i](name_to_idx[state.joint_name[j]], 0) =
-              state.joint_position[j];
+          knots[i](idx, 0) = state.joint_position[j];
         }
       }
     }
@@ -165,27 +165,29 @@ class RobotPlanRunner {
     ++plan_number_;
   }
 
-  void HandleStop(const lcm::ReceiveBuffer*, const std::string&,
-                    const robotlocomotion::robot_plan_t*) {
+  void HandleStop(const ::lcm::ReceiveBuffer*, const std::string&,
+                  const robotlocomotion::robot_plan_t*) {
     std::cout << "Received stop command. Discarding plan." << std::endl;
     plan_.reset();
   }
 
-  lcm::LCM lcm_;
-  const RigidBodyTree<double>& tree_;
+  ::lcm::LCM lcm_;
+  const multibody::MultibodyPlant<double>& plant_;
   int plan_number_{};
   std::unique_ptr<PiecewisePolynomial<double>> plan_;
   lcmt_iiwa_status iiwa_status_;
 };
 
 int do_main() {
-  auto tree = std::make_unique<RigidBodyTree<double>>();
-  parsers::urdf::AddModelInstanceFromUrdfFileToWorld(
+  multibody::MultibodyPlant<double> plant(0.0);
+  multibody::Parser(&plant).AddModelFromFile(
       FindResourceOrThrow("drake/manipulation/models/iiwa_description/urdf/"
-                          "iiwa14_primitive_collision.urdf"),
-      multibody::joints::kFixed, tree.get());
+                          "iiwa14_no_collision.urdf"));
+  plant.WeldFrames(plant.world_frame(),
+                   plant.GetBodyByName("base").body_frame());
+  plant.Finalize();
 
-  RobotPlanRunner runner(*tree);
+  RobotPlanRunner runner(plant);
   runner.Run();
   return 0;
 }