diff --git a/attic/manipulation/planner/BUILD.bazel b/attic/manipulation/planner/BUILD.bazel index 67d563ca228d..1baf6ea847af 100644 --- a/attic/manipulation/planner/BUILD.bazel +++ b/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", ], ) diff --git a/attic/manipulation/planner/constraint_relaxing_ik.cc b/attic/manipulation/planner/constraint_relaxing_ik_rbt.cc similarity index 96% rename from attic/manipulation/planner/constraint_relaxing_ik.cc rename to attic/manipulation/planner/constraint_relaxing_ik_rbt.cc index 6e6ec1219081..0e4a5b73226e 100644 --- a/attic/manipulation/planner/constraint_relaxing_ik.cc +++ b/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 @@ -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& base_to_world) @@ -32,7 +32,7 @@ ConstraintRelaxingIk::ConstraintRelaxingIk( SetEndEffector(end_effector_link_name); } -bool ConstraintRelaxingIk::PlanSequentialTrajectory( +bool ConstraintRelaxingIkRbt::PlanSequentialTrajectory( const std::vector& waypoints, const VectorX& 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& q0, const VectorX& q_nom, diff --git a/attic/manipulation/planner/constraint_relaxing_ik.h b/attic/manipulation/planner/constraint_relaxing_ik_rbt.h similarity index 89% rename from attic/manipulation/planner/constraint_relaxing_ik.h rename to attic/manipulation/planner/constraint_relaxing_ik_rbt.h index a34b8b345288..40091999a7d1 100644 --- a/attic/manipulation/planner/constraint_relaxing_ik.h +++ b/attic/manipulation/planner/constraint_relaxing_ik_rbt.h @@ -6,6 +6,7 @@ #include #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& base_to_world); + ConstraintRelaxingIkRbt(const std::string& model_path, + const std::string& end_effector_link_name, + const Isometry3& base_to_world); /** * Sets end effector to @p end_effector_body. diff --git a/attic/manipulation/planner/test/constraint_relaxing_ik_test.cc b/attic/manipulation/planner/test/constraint_relaxing_ik_rbt_test.cc similarity index 88% rename from attic/manipulation/planner/test/constraint_relaxing_ik_test.cc rename to attic/manipulation/planner/test/constraint_relaxing_ik_rbt_test.cc index e208929cde1c..c7e543848a69 100644 --- a/attic/manipulation/planner/test/constraint_relaxing_ik_test.cc +++ b/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 @@ -23,7 +23,7 @@ inline double get_orientation_difference(const Matrix3& 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* end_effector = iiwa->FindBody(kEndEffectorLinkName); IKResults ik_res; - ConstraintRelaxingIk ik_planner(kModelPath, kEndEffectorLinkName, - Isometry3::Identity()); - ConstraintRelaxingIk::IkCartesianWaypoint wp; + ConstraintRelaxingIkRbt ik_planner(kModelPath, kEndEffectorLinkName, + Isometry3::Identity()); + ConstraintRelaxingIkRbt::IkCartesianWaypoint wp; wp.pos_tol = Vector3(0.001, 0.001, 0.001); wp.rot_tol = 0.005; wp.constrain_orientation = true; - std::vector waypoints(1, wp); + std::vector waypoints(1, wp); const VectorX kQcurrent = iiwa->getZeroConfiguration(); VectorX q_fk; diff --git a/examples/kuka_iiwa_arm/BUILD.bazel b/examples/kuka_iiwa_arm/BUILD.bazel index d54ea4e2139c..e12956b4b69d 100644 --- a/examples/kuka_iiwa_arm/BUILD.bazel +++ b/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", ], diff --git a/examples/kuka_iiwa_arm/iiwa_common.cc b/examples/kuka_iiwa_arm/iiwa_common.cc index b541206b9b28..de824d241831 100644 --- a/examples/kuka_iiwa_arm/iiwa_common.cc +++ b/examples/kuka_iiwa_arm/iiwa_common.cc @@ -177,7 +177,6 @@ void ApplyJointVelocityLimits(const MatrixX& keyframes, } } - robotlocomotion::robot_plan_t EncodeKeyFrames( const RigidBodyTree& robot, const std::vector& time, diff --git a/examples/kuka_iiwa_arm/iiwa_common.h b/examples/kuka_iiwa_arm/iiwa_common.h index c2d155b7107d..ad6ef09fdeae 100644 --- a/examples/kuka_iiwa_arm/iiwa_common.h +++ b/examples/kuka_iiwa_arm/iiwa_common.h @@ -38,6 +38,7 @@ Matrix6 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& tree); /// Builds a RigidBodyTree at the specified @position and @orientation from @@ -69,6 +70,7 @@ void ApplyJointVelocityLimits(const MatrixX& 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& robot, const std::vector& time, const std::vector& info, const MatrixX& keyframes); diff --git a/examples/kuka_iiwa_arm/iiwa_lcm.h b/examples/kuka_iiwa_arm/iiwa_lcm.h index ae9363e8e1a5..1229e76c3520 100644 --- a/examples/kuka_iiwa_arm/iiwa_lcm.h +++ b/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 { +class DRAKE_DEPRECATED("2020-06-01", "This class is being removed.") +IiwaContactResultsToExternalTorque : public systems::LeafSystem { public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(IiwaContactResultsToExternalTorque) diff --git a/examples/kuka_iiwa_arm/kuka_plan_runner.cc b/examples/kuka_iiwa_arm/kuka_plan_runner.cc index a60cfddc72d5..3413fc9a6e3e 100644 --- a/examples/kuka_iiwa_arm/kuka_plan_runner.cc +++ b/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& tree) - : tree_(tree), plan_number_(0) { - VerifyIiwaTree(tree); + /// plant is aliased + explicit RobotPlanRunner(const multibody::MultibodyPlant& 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 knots(plan->num_states, Eigen::MatrixXd::Zero(kNumJoints, 1)); - std::map 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& 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& tree_; + ::lcm::LCM lcm_; + const multibody::MultibodyPlant& plant_; int plan_number_{}; std::unique_ptr> plan_; lcmt_iiwa_status iiwa_status_; }; int do_main() { - auto tree = std::make_unique>(); - parsers::urdf::AddModelInstanceFromUrdfFileToWorld( + multibody::MultibodyPlant 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; } diff --git a/examples/kuka_iiwa_arm/move_iiwa_ee.cc b/examples/kuka_iiwa_arm/move_iiwa_ee.cc index 8b90da11720b..86ffd9c41356 100644 --- a/examples/kuka_iiwa_arm/move_iiwa_ee.cc +++ b/examples/kuka_iiwa_arm/move_iiwa_ee.cc @@ -6,6 +6,8 @@ /// current calculated position of the end effector is printed before, /// during, and after the commanded motion. +#include + #include #include "lcm/lcm-cpp.hpp" #include "robotlocomotion/robot_plan_t.hpp" @@ -19,8 +21,8 @@ #include "drake/math/rigid_transform.h" #include "drake/math/roll_pitch_yaw.h" #include "drake/math/rotation_matrix.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" DEFINE_string(urdf, "", "Name of urdf to load"); DEFINE_string(lcm_status_channel, "IIWA_STATUS", @@ -48,14 +50,39 @@ const char kIiwaUrdf[] = class MoveDemoRunner { public: - MoveDemoRunner() { + MoveDemoRunner() + : plant_(0.0) { urdf_ = (!FLAGS_urdf.empty() ? FLAGS_urdf : FindResourceOrThrow(kIiwaUrdf)); - parsers::urdf::AddModelInstanceFromUrdfFileToWorld( - urdf_, multibody::joints::kFixed, &tree_); + multibody::Parser(&plant_).AddModelFromFile(urdf_); + plant_.WeldFrames(plant_.world_frame(), + plant_.GetBodyByName("base").body_frame()); + plant_.Finalize(); + context_ = plant_.CreateDefaultContext(); lcm_.subscribe(FLAGS_lcm_status_channel, &MoveDemoRunner::HandleStatus, this); + + // TODO(sammy-tri) Move this code somewhere more generic if we get a + // second example which wants to encode robot plans. + std::map position_names; + const int num_positions = plant_.num_positions(); + for (int i = 0; i < plant_.num_joints(); ++i) { + const multibody::Joint& joint = + plant_.get_joint(multibody::JointIndex(i)); + if (joint.num_positions() == 0) { + continue; + } + DRAKE_DEMAND(joint.num_positions() == 1); + DRAKE_DEMAND(joint.position_start() < num_positions); + + position_names[joint.position_start()] = joint.name(); + } + + DRAKE_DEMAND(static_cast(position_names.size()) == num_positions); + for (int i = 0; i < num_positions; ++i) { + joint_names_.push_back(position_names[i]); + } } void Run() { @@ -67,25 +94,22 @@ class MoveDemoRunner { // fairly high rate operation (200Hz is typical). The plan is // calculated on the first status message received, after that // periodically display the current position of the end effector. - void HandleStatus(const lcm::ReceiveBuffer*, const std::string&, + void HandleStatus(const ::lcm::ReceiveBuffer*, const std::string&, const lcmt_iiwa_status* status) { status_count_++; Eigen::VectorXd iiwa_q(status->num_joints); - Eigen::VectorXd iiwa_v(status->num_joints); for (int i = 0; i < status->num_joints; i++) { - iiwa_v[i] = status->joint_velocity_estimated[i]; iiwa_q[i] = status->joint_position_measured[i]; } // Only print the position every 100 messages (this results in an // 0.5s period in a typical configuration). if (status_count_ % 100 == 1) { - // Estimate the end effector position through forward kinematics. - KinematicsCache cache = tree_.doKinematics(iiwa_q, iiwa_v, true); - const RigidBody* end_effector = tree_.FindBody(FLAGS_ee_name); + plant_.SetPositions(context_.get(), iiwa_q); - const math::RigidTransform ee_pose( - tree_.CalcBodyPoseInWorldFrame(cache, *end_effector)); + const math::RigidTransform ee_pose = + plant_.EvalBodyPoseInWorld( + *context_, plant_.GetBodyByName(FLAGS_ee_name)); const math::RollPitchYaw rpy(ee_pose.rotation()); drake::log()->info("End effector at: {} {}", ee_pose.translation().transpose(), @@ -95,8 +119,7 @@ class MoveDemoRunner { // If this is the first status we've received, calculate a plan // and send it (if it succeeds). if (status_count_ == 1) { - ConstraintRelaxingIk ik( - urdf_, FLAGS_ee_name, Isometry3::Identity()); + ConstraintRelaxingIk ik(urdf_, FLAGS_ee_name); // Create a single waypoint for our plan (the destination). // This results in a trajectory with two knot points (the @@ -104,47 +127,51 @@ class MoveDemoRunner { // processes and passed directly to PlanSequentialTrajectory as // iiwa_q) and the calculated final pose). ConstraintRelaxingIk::IkCartesianWaypoint wp; - wp.pose.translation() = Eigen::Vector3d(FLAGS_x, FLAGS_y, FLAGS_z); + wp.pose.set_translation(Eigen::Vector3d(FLAGS_x, FLAGS_y, FLAGS_z)); const math::RollPitchYaw rpy(FLAGS_roll, FLAGS_pitch, FLAGS_yaw); - wp.pose.linear() = rpy.ToMatrix3ViaRotationMatrix(); + wp.pose.set_rotation(rpy); wp.constrain_orientation = true; std::vector waypoints; waypoints.push_back(wp); - IKResults ik_res; - ik.PlanSequentialTrajectory(waypoints, iiwa_q, &ik_res); - drake::log()->info("IK result: {}", ik_res.info[0]); + std::vector q_sol; + const bool result = + ik.PlanSequentialTrajectory(waypoints, iiwa_q, &q_sol); + drake::log()->info("IK result: {}", result); - if (ik_res.info[0] == 1) { - drake::log()->info("IK sol size {}", ik_res.q_sol.size()); + if (result) { + drake::log()->info("IK sol size {}", q_sol.size()); // Run the resulting plan over 2 seconds (which is a fairly // arbitrary choice). This may be slowed down if executing // the plan in that time would exceed any joint velocity // limits. std::vector times{0, 2}; - DRAKE_DEMAND(ik_res.q_sol.size() == times.size()); + DRAKE_DEMAND(q_sol.size() == times.size()); // Convert the resulting waypoints into the format expected by // ApplyJointVelocityLimits and EncodeKeyFrames. - MatrixX q_mat(ik_res.q_sol.front().size(), ik_res.q_sol.size()); - for (size_t i = 0; i < ik_res.q_sol.size(); ++i) { - q_mat.col(i) = ik_res.q_sol[i]; + MatrixX q_mat(q_sol.front().size(), q_sol.size()); + for (size_t i = 0; i < q_sol.size(); ++i) { + q_mat.col(i) = q_sol[i]; } // Ensure that the planned motion would not exceed the joint // velocity limits. This function will scale the supplied // times if needed. ApplyJointVelocityLimits(q_mat, ×); + std::vector info{1, 1}; robotlocomotion::robot_plan_t plan = - EncodeKeyFrames(tree_, times, ik_res.info, q_mat); + EncodeKeyFrames(joint_names_, times, info, q_mat); lcm_.publish(FLAGS_lcm_plan_channel, &plan); } } } - lcm::LCM lcm_; + ::lcm::LCM lcm_; std::string urdf_; - RigidBodyTree tree_; + multibody::MultibodyPlant plant_; + std::unique_ptr> context_; + std::vector joint_names_; int status_count_{0}; }; diff --git a/manipulation/planner/BUILD.bazel b/manipulation/planner/BUILD.bazel index 4904908b9d41..cb6200246505 100644 --- a/manipulation/planner/BUILD.bazel +++ b/manipulation/planner/BUILD.bazel @@ -15,11 +15,25 @@ package( drake_cc_package_library( name = "planner", deps = [ + ":constraint_relaxing_ik", ":differential_inverse_kinematics", ":robot_plan_interpolator", ], ) +drake_cc_library( + name = "constraint_relaxing_ik", + srcs = ["constraint_relaxing_ik.cc"], + hdrs = ["constraint_relaxing_ik.h"], + deps = [ + "//common/trajectories:piecewise_polynomial", + "//multibody/inverse_kinematics", + "//multibody/parsing", + "//multibody/plant", + "//solvers:solve", + ], +) + drake_cc_library( name = "differential_inverse_kinematics", srcs = ["differential_inverse_kinematics.cc"], @@ -46,6 +60,19 @@ drake_cc_library( # === test/ === +drake_cc_googletest( + name = "constraint_relaxing_ik_test", + srcs = ["test/constraint_relaxing_ik_test.cc"], + data = [ + "//manipulation/models/iiwa_description:models", + ], + tags = ["snopt"], + deps = [ + ":constraint_relaxing_ik", + "//common:find_resource", + ], +) + drake_cc_googletest( name = "differential_inverse_kinematics_test", data = [ diff --git a/manipulation/planner/constraint_relaxing_ik.cc b/manipulation/planner/constraint_relaxing_ik.cc new file mode 100644 index 000000000000..9ecd11b0670b --- /dev/null +++ b/manipulation/planner/constraint_relaxing_ik.cc @@ -0,0 +1,198 @@ +#include "drake/manipulation/planner/constraint_relaxing_ik.h" + +#include +#include + +#include "drake/common/text_logging.h" +#include "drake/multibody/inverse_kinematics/inverse_kinematics.h" +#include "drake/multibody/parsing/parser.h" +#include "drake/solvers/solve.h" + +namespace drake { +namespace manipulation { +namespace planner { +namespace { +constexpr int kDefaultRandomSeed = 1234; +} // namespace + +ConstraintRelaxingIk::ConstraintRelaxingIk( + const std::string& model_path, + const std::string& end_effector_link_name) + : rand_generator_(kDefaultRandomSeed), + plant_(0) { + const auto model_instance = + multibody::Parser(&plant_).AddModelFromFile(model_path); + + // Check if our robot is welded to the world. If not, try welding the first + // link. + if (plant_.GetBodiesWeldedTo(plant_.world_body()).size() <= 1) { + const std::vector bodies = + plant_.GetBodyIndices(model_instance); + plant_.WeldFrames(plant_.world_frame(), + plant_.get_body(bodies[0]).body_frame()); + } + plant_.Finalize(); + + SetEndEffector(end_effector_link_name); +} + +ConstraintRelaxingIk::ConstraintRelaxingIk( + const std::string&, + const std::string&, + const Isometry3&) + : rand_generator_(kDefaultRandomSeed), + plant_(0) { + throw std::runtime_error( + "Use the two-argument constructor or ConstraintRelaxingIkRbt"); +} + +bool ConstraintRelaxingIk::PlanSequentialTrajectory( + const std::vector& waypoints, + const VectorX& q_current, + std::vector* q_sol_out) { + DRAKE_DEMAND(q_sol_out); + int num_steps = static_cast(waypoints.size()); + + VectorX q0 = q_current; + VectorX q_sol = q_current; + + q_sol_out->clear(); + q_sol_out->push_back(q_current); + + int step_ctr = 0; + int relaxed_ctr = 0; + int random_ctr = 0; + + enum class RelaxMode { kRelaxPosTol = 0, kRelaxRotTol = 1 }; + + // These numbers are arbitrarily picked by siyuan. + const int kMaxNumInitialGuess = 50; + const int kMaxNumConstraintRelax = 10; + const Vector3 kInitialPosTolerance(0.01, 0.01, 0.01); + const double kInitialRotTolerance = 0.01; + const double kConstraintShrinkFactor = 0.5; + const double kConstraintGrowFactor = 1.5; + + for (const auto& waypoint : waypoints) { + // Sets the initial constraints guess bigger than the desired tolerance. + Vector3 pos_tol = kInitialPosTolerance; + double rot_tol = kInitialRotTolerance; + + if (!waypoint.constrain_orientation) rot_tol = 0; + + // Sets mode to reduce position tolerance. + RelaxMode mode = RelaxMode::kRelaxPosTol; + + // Solves point IK with constraint fiddling and random start. + while (true) { + if (!waypoint.constrain_orientation) + DRAKE_DEMAND(mode == RelaxMode::kRelaxPosTol); + + std::vector info; + std::vector infeasible_constraints; + bool res = SolveIk(waypoint, q0, pos_tol, rot_tol, &q_sol); + + if (res) { + // Breaks if the current tolerance is below given threshold. + if ((rot_tol <= waypoint.rot_tol) && + (pos_tol.array() <= waypoint.pos_tol.array()).all()) { + break; + } + + // Alternates between kRelaxPosTol and kRelaxRotTol + if (mode == RelaxMode::kRelaxPosTol && waypoint.constrain_orientation) { + rot_tol *= kConstraintShrinkFactor; + mode = RelaxMode::kRelaxRotTol; + } else { + pos_tol *= kConstraintShrinkFactor; + mode = RelaxMode::kRelaxPosTol; + } + // Sets the initial guess to the current solution. + q0 = q_sol; + } else { + // Relaxes the constraints no solution is found. + if (mode == RelaxMode::kRelaxRotTol && waypoint.constrain_orientation) { + rot_tol *= kConstraintGrowFactor; + } else { + pos_tol *= kConstraintGrowFactor; + } + relaxed_ctr++; + } + + // Switches to a different initial guess and start over if we have relaxed + // the constraints for max times. + if (relaxed_ctr > kMaxNumConstraintRelax) { + // Make a random initial guess. + std::unique_ptr> context = + plant_.CreateDefaultContext(); + plant_.SetRandomState(*context, &context->get_mutable_state(), + &rand_generator_); + q0 = plant_.GetPositions(*context); + // Resets constraints tolerance. + pos_tol = kInitialPosTolerance; + rot_tol = kInitialRotTolerance; + if (!waypoint.constrain_orientation) rot_tol = 0; + mode = RelaxMode::kRelaxPosTol; + drake::log()->warn("IK FAILED at max constraint relaxing iter: " + + std::to_string(relaxed_ctr)); + relaxed_ctr = 0; + random_ctr++; + } + + // Admits failure and returns false. + if (random_ctr > kMaxNumInitialGuess) { + drake::log()->error("IK FAILED at max random starts: " + + std::to_string(random_ctr)); + // Returns information about failure. + q_sol_out->push_back(q_sol); + return false; + } + } + + // Sets next IK's initial and bias to current solution. + q0 = q_sol; + q_sol_out->push_back(q_sol); + step_ctr++; + } + + DRAKE_DEMAND(static_cast(q_sol_out->size()) == num_steps + 1); + return true; +} + +bool ConstraintRelaxingIk::SolveIk( + const IkCartesianWaypoint& waypoint, + const VectorX& q0, + const Vector3& pos_tol, double rot_tol, + VectorX* q_res) { + DRAKE_DEMAND(q_res); + + multibody::InverseKinematics ik(plant_); + + // Adds a position constraint. + Vector3 pos_lb = waypoint.pose.translation() - pos_tol; + Vector3 pos_ub = waypoint.pose.translation() + pos_tol; + + ik.AddPositionConstraint( + plant_.get_body(end_effector_body_idx_).body_frame(), + Vector3::Zero(), + plant_.world_frame(), pos_lb, pos_ub); + + if (waypoint.constrain_orientation) { + ik.AddOrientationConstraint( + plant_.world_frame(), waypoint.pose.rotation(), + plant_.get_body(end_effector_body_idx_).body_frame(), + math::RotationMatrixd(), rot_tol); + } + + const auto result = solvers::Solve(ik.prog(), q0); + if (!result.is_success()) { + return false; + } + + *q_res = result.get_x_val(); + return true; +} + +} // namespace planner +} // namespace manipulation +} // namespace drake diff --git a/manipulation/planner/constraint_relaxing_ik.h b/manipulation/planner/constraint_relaxing_ik.h new file mode 100644 index 000000000000..c0f3086f2d92 --- /dev/null +++ b/manipulation/planner/constraint_relaxing_ik.h @@ -0,0 +1,96 @@ +#pragma once + +#include +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" +#include "drake/common/random.h" +#include "drake/math/rigid_transform.h" +#include "drake/multibody/plant/multibody_plant.h" + +namespace drake { +namespace manipulation { +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 { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ConstraintRelaxingIk); + + /** + * Cartesian waypoint. Input to the IK solver. + */ + struct IkCartesianWaypoint { + /// Desired end effector pose in the world frame. + math::RigidTransformd pose; + /// Bounding box for the end effector in the world frame. + Vector3 pos_tol{Vector3(0.005, 0.005, 0.005)}; + /// Max angle difference (in radians) between solved end effector's + /// orientation and the desired. + double rot_tol{0.05}; + /// Signals if orientation constraint is enabled. + bool constrain_orientation{false}; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** + * Constructor. Instantiates an internal MultibodyPlant from @p model_path. + * @param model_path Path to the model file. + * @param end_effector_link_name Link name of the end effector. + */ + ConstraintRelaxingIk(const std::string& model_path, + const std::string& end_effector_link_name); + + + DRAKE_DEPRECATED("2020-06-01", + "Use the two-argument constructor or " + "ConstraintRelaxingIkRbt (which is also deprecated)") + ConstraintRelaxingIk(const std::string& model_path, + const std::string& end_effector_link_name, + const Isometry3& base_to_world); + + /** + * Sets end effector to @p link_name. + */ + void SetEndEffector(const std::string& link_name) { + end_effector_body_idx_ = plant_.GetBodyByName(link_name).index(); + } + + /** + * Generates IK solutions for each waypoint sequentially. For waypoint wp_i, + * the IK tries to solve q_i that satisfies the end effector constraints in + * wp_i and minimizes the squared difference to q_{i-1}, where q_{i-1} is the + * solution to the previous wp_{i-1}. q_{i-1} = @p q_current when i = 0. This + * function internally does constraint relaxing and initial condition + * guessing if necessary. + * + * Note that @p q_current is inserted at the beginning of @p q_sol. + * + * @param waypoints A sequence of desired waypoints. + * @param q_current The initial generalized position. + * @param[out] q_sol Results. + * @return True if solved successfully. + */ + bool PlanSequentialTrajectory( + const std::vector& waypoints, + const VectorX& q_current, + std::vector* q_sol); + + private: + bool SolveIk(const IkCartesianWaypoint& waypoint, const VectorX& q0, + const Vector3& pos_tol, double rot_tol, + VectorX* q_res); + + RandomGenerator rand_generator_; + multibody::MultibodyPlant plant_; + multibody::BodyIndex end_effector_body_idx_{}; +}; + +} // namespace planner +} // namespace manipulation +} // namespace drake diff --git a/manipulation/planner/test/constraint_relaxing_ik_test.cc b/manipulation/planner/test/constraint_relaxing_ik_test.cc new file mode 100644 index 000000000000..add7b5d47943 --- /dev/null +++ b/manipulation/planner/test/constraint_relaxing_ik_test.cc @@ -0,0 +1,86 @@ +#include "drake/manipulation/planner/constraint_relaxing_ik.h" + +#include + +#include "drake/common/find_resource.h" +#include "drake/common/random.h" +#include "drake/multibody/parsing/parser.h" + +namespace drake { +namespace manipulation { +namespace planner { + +// N random samples are taken from the configuration space (q), and +// the corresponding end effector poses are computed with forward +// kinematics. We use inverse kinematics to try to recover a set of +// 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) { + const std::string kModelPath = FindResourceOrThrow( + "drake/manipulation/models/iiwa_description/urdf/" + "iiwa14_polytope_collision.urdf"); + multibody::MultibodyPlant iiwa(0); + multibody::Parser(&iiwa).AddModelFromFile(kModelPath); + iiwa.WeldFrames(iiwa.world_frame(), + iiwa.GetBodyByName("base").body_frame()); + iiwa.Finalize(); + + const std::string kEndEffectorLinkName = "iiwa_link_ee"; + const multibody::Body& end_effector = + iiwa.GetBodyByName(kEndEffectorLinkName); + + ConstraintRelaxingIk ik_planner(kModelPath, kEndEffectorLinkName); + + ConstraintRelaxingIk::IkCartesianWaypoint wp; + wp.pos_tol = Vector3(0.001, 0.001, 0.001); + wp.rot_tol = 0.005; + wp.constrain_orientation = true; + std::vector waypoints(1, wp); + + std::unique_ptr> context = + iiwa.CreateDefaultContext(); + + const VectorX kQcurrent = iiwa.GetPositions(*context); + + const double kEpsilon = 1e-8; + const Vector3 kUpperBound = + wp.pos_tol + kEpsilon * Vector3::Ones(); + const Vector3 kLowerBound = + -wp.pos_tol - kEpsilon * Vector3::Ones(); + RandomGenerator rand_generator(1234); + + for (int i = 0; i < 100; ++i) { + iiwa.SetRandomState( + *context, &context->get_mutable_state(), &rand_generator); + + math::RigidTransformd fk_pose = iiwa.EvalBodyPoseInWorld( + *context, end_effector); + waypoints[0].pose = fk_pose; + + std::vector q_sol; + const bool ret = + ik_planner.PlanSequentialTrajectory(waypoints, kQcurrent, &q_sol); + ASSERT_TRUE(ret); + + iiwa.SetPositions(context.get(), q_sol.front()); + const math::RigidTransformd ik_pose = iiwa.EvalBodyPoseInWorld( + *context, end_effector); + + const Vector3 pos_diff = + ik_pose.translation() - fk_pose.translation(); + const math::RotationMatrix R_diff = + ik_pose.rotation().transpose() * fk_pose.rotation(); + const double rot_diff = R_diff.ToAngleAxis().angle(); + + EXPECT_TRUE((pos_diff.array() <= kUpperBound.array()).all()); + EXPECT_TRUE((pos_diff.array() >= kLowerBound.array()).all()); + + // cos(ang_diff) >= cos(tol) is the actual constraint in the IK. + EXPECT_TRUE(std::cos(rot_diff) + kEpsilon >= std::cos(wp.rot_tol)); + } +} + +} // namespace planner +} // namespace manipulation +} // namespace drake