From 535110755306ac6aa78ae247b920c3121336663c Mon Sep 17 00:00:00 2001 From: Sam Creasey Date: Tue, 9 Jan 2018 18:55:48 -0500 Subject: [PATCH] Add a simple demo to move the end effector based on some command line args --- examples/kuka_iiwa_arm/BUILD.bazel | 20 ++++ examples/kuka_iiwa_arm/README.md | 9 +- examples/kuka_iiwa_arm/move_iiwa_ee.cc | 160 +++++++++++++++++++++++++ 3 files changed, 186 insertions(+), 3 deletions(-) create mode 100644 examples/kuka_iiwa_arm/move_iiwa_ee.cc diff --git a/examples/kuka_iiwa_arm/BUILD.bazel b/examples/kuka_iiwa_arm/BUILD.bazel index e87363a9178b..0eeca06d51e8 100644 --- a/examples/kuka_iiwa_arm/BUILD.bazel +++ b/examples/kuka_iiwa_arm/BUILD.bazel @@ -148,6 +148,26 @@ drake_example_cc_binary( ], ) +drake_cc_binary( + name = "move_iiwa_ee", + srcs = ["move_iiwa_ee.cc"], + data = [ + ":models", + "//drake/manipulation/models/iiwa_description:models", + ], + deps = [ + ":iiwa_common", + "//drake/common:text_logging_gflags", + "//drake/lcmtypes:iiwa", + "//drake/manipulation/planner:constraint_relaxing_ik", + "//drake/math:geometric_transform", + "//drake/multibody/parsers", + "@lcm", + "@lcmtypes_bot2_core", + "@lcmtypes_robotlocomotion", + ], +) + alias( name = "models", actual = adjust_label_for_drake_hoist( diff --git a/examples/kuka_iiwa_arm/README.md b/examples/kuka_iiwa_arm/README.md index 70cee22f7639..451480a53b6f 100644 --- a/examples/kuka_iiwa_arm/README.md +++ b/examples/kuka_iiwa_arm/README.md @@ -1,7 +1,7 @@ IIWA Manipulation Examples ========================== -There are a number of examples contained in these directories. +There are a number of examples contained in these directories. The following instructions assume Drake was [built using bazel](http://drake.mit.edu/bazel.html?highlight=bazel). @@ -43,7 +43,10 @@ upon receiving a single plan message) bazel-bin/examples/kuka_iiwa_arm/kuka_plan_runner ``` +Command the robot to move the end effector +``` +bazel-bin/examples/kuka_iiwa_arm/move_iiwa_ee -x 0.8 -y 0.3 -z 0.25 -yaw 1.57 +``` + Coming back soon - generate plans using the graphical IK interface. See https://github.com/RobotLocomotion/drake/issues/7321 . - - diff --git a/examples/kuka_iiwa_arm/move_iiwa_ee.cc b/examples/kuka_iiwa_arm/move_iiwa_ee.cc new file mode 100644 index 000000000000..a1411e8241b4 --- /dev/null +++ b/examples/kuka_iiwa_arm/move_iiwa_ee.cc @@ -0,0 +1,160 @@ +/// @file +/// +/// Demo of moving the iiwa's end effector in cartesian space. This +/// program creates a plan to move the end effector from the current +/// position to the location specified on the command line. The +/// current calculated position of the end effector is printed before, +/// during, and after the commanded motion. + +#include +#include +#include "robotlocomotion/robot_plan_t.hpp" + +#include "drake/common/drake_assert.h" +#include "drake/common/find_resource.h" +#include "drake/common/text_logging.h" +#include "drake/common/text_logging_gflags.h" +#include "drake/examples/kuka_iiwa_arm/iiwa_common.h" +#include "drake/lcmt_iiwa_status.hpp" +#include "drake/manipulation/planner/constraint_relaxing_ik.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" + +DEFINE_string(urdf, "", "Name of urdf to load"); +DEFINE_string(lcm_status_channel, "IIWA_STATUS", + "Channel on which to listen for lcmt_iiwa_status messages."); +DEFINE_string(lcm_plan_channel, "COMMITTED_ROBOT_PLAN", + "Channel on which to send robot_plan_t messages."); +DEFINE_double(x, 0., "x coordinate to move to"); +DEFINE_double(y, 0., "y coordinate to move to"); +DEFINE_double(z, 0., "z coordinate to move to"); +DEFINE_double(roll, 0., "target roll about world x axis for end effector"); +DEFINE_double(pitch, 0., "target pitch about world y axis for end effector"); +DEFINE_double(yaw, 0., "target yaw about world z axis for end effector"); +DEFINE_string(ee_name, "iiwa_link_ee", "Name of the end effector link"); + +namespace drake { +namespace examples { +namespace kuka_iiwa_arm { +namespace { + +using manipulation::planner::ConstraintRelaxingIk; + +const char kIiwaUrdf[] = + "drake/manipulation/models/iiwa_description/urdf/" + "iiwa14_polytope_collision.urdf"; + +class MoveDemoRunner { + public: + MoveDemoRunner() { + urdf_ = + (!FLAGS_urdf.empty() ? FLAGS_urdf : FindResourceOrThrow(kIiwaUrdf)); + parsers::urdf::AddModelInstanceFromUrdfFileToWorld( + urdf_, multibody::joints::kFixed, &tree_); + + lcm_.subscribe(FLAGS_lcm_status_channel, + &MoveDemoRunner::HandleStatus, this); + } + + void Run() { + while (lcm_.handle() >= 0) { } + } + + private: + // Handle the incoming status message from the iiwa. This is a + // 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&, + 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); + + Isometry3 ee_pose = + tree_.CalcBodyPoseInWorldFrame(cache, *end_effector); + drake::log()->info("End effector at: {} {}", + ee_pose.translation().transpose(), + math::rotmat2rpy(ee_pose.rotation()).transpose()); + } + + // 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()); + + // Create a single waypoint for our plan (the destination). + // This results in a trajectory with two knot points (the + // current pose (read from the status message currently being + // 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); + Eigen::Vector3d rpy(FLAGS_roll, FLAGS_pitch, FLAGS_yaw); + wp.pose.linear() = math::rpy2rotmat(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]); + + if (ik_res.info[0] == 1) { + drake::log()->info("IK sol size {}", ik_res.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()); + + // 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]; + } + + // Ensure that the planned motion would not exceed the joint + // velocity limits. This function will scale the supplied + // times if needed. + ApplyJointVelocityLimits(q_mat, ×); + robotlocomotion::robot_plan_t plan = + EncodeKeyFrames(tree_, times, ik_res.info, q_mat); + lcm_.publish(FLAGS_lcm_plan_channel, &plan); + } + } + } + + lcm::LCM lcm_; + std::string urdf_; + RigidBodyTree tree_; + int status_count_{0}; +}; + +} // namespace +} // namespace kuka_iiwa_arm +} // namespace examples +} // namespace drake + +int main(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + drake::logging::HandleSpdlogGflags(); + drake::examples::kuka_iiwa_arm::MoveDemoRunner runner; + runner.Run(); +}