Skip to content

Commit

Permalink
moved /examples/kuka_iiwa_arm/dev/tools/simple_tree_visualizer to
Browse files Browse the repository at this point in the history
/manipulation/util/simple_tree_visualizer
  • Loading branch information
siyuanfeng-tri committed Jul 7, 2017
1 parent 656dc85 commit 07fedc5
Show file tree
Hide file tree
Showing 5 changed files with 55 additions and 65 deletions.
37 changes: 0 additions & 37 deletions drake/examples/kuka_iiwa_arm/dev/tools/BUILD
Original file line number Diff line number Diff line change
Expand Up @@ -4,47 +4,10 @@
load("//tools:cpplint.bzl", "cpplint")
load(
"//tools:drake.bzl",
"drake_cc_binary",
"drake_cc_library",
"drake_cc_googletest",
)

drake_cc_library(
name = "simple_tree_visualizer",
srcs = ["simple_tree_visualizer.cc"],
hdrs = ["simple_tree_visualizer.h"],
visibility = ["//drake/examples/kuka_iiwa_arm:__subpackages__"],
deps = [
"//drake/common",
"//drake/lcm",
"//drake/lcmtypes:viewer",
"//drake/multibody/rigid_body_plant:create_load_robot_message",
"//drake/multibody/rigid_body_plant:drake_visualizer",
"//drake/systems/framework",
"//drake/systems/rendering:drake_visualizer_client",
],
)

drake_cc_binary(
name = "simple_tree_visualizer_demo",
srcs = ["simple_tree_visualizer_demo.cc"],
add_test_rule = 1,
data = [
"//drake/examples/kuka_iiwa_arm:models",
"//drake/manipulation/models/iiwa_description:models",
],
test_rule_args = ["--num_configurations=1"],
deps = [
":simple_tree_visualizer",
"//drake/common",
"//drake/common:find_resource",
"//drake/lcm",
"//drake/multibody/parsers",
"//drake/systems/framework",
"@com_github_gflags_gflags//:gflags",
],
)

drake_cc_library(
name = "moving_average_filter",
srcs = ["moving_average_filter.cc"],
Expand Down
35 changes: 35 additions & 0 deletions drake/manipulation/util/BUILD
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
load("//tools:cpplint.bzl", "cpplint")
load(
"//tools:drake.bzl",
"drake_cc_binary",
"drake_cc_googletest",
"drake_cc_library",
)
Expand All @@ -22,6 +23,21 @@ drake_cc_library(
],
)

drake_cc_library(
name = "simple_tree_visualizer",
srcs = ["simple_tree_visualizer.cc"],
hdrs = ["simple_tree_visualizer.h"],
deps = [
"//drake/common",
"//drake/lcm",
"//drake/lcmtypes:viewer",
"//drake/multibody/rigid_body_plant:create_load_robot_message",
"//drake/multibody/rigid_body_plant:drake_visualizer",
"//drake/systems/framework",
"//drake/systems/rendering:drake_visualizer_client",
],
)

drake_cc_library(
name = "trajectory_utils",
srcs = [
Expand All @@ -36,6 +52,25 @@ drake_cc_library(
],
)

drake_cc_binary(
name = "simple_tree_visualizer_demo",
srcs = ["simple_tree_visualizer_demo.cc"],
add_test_rule = 1,
data = [
"//drake/manipulation/models/iiwa_description:models",
],
test_rule_args = ["--num_configurations=1"],
deps = [
":simple_tree_visualizer",
"//drake/common",
"//drake/common:find_resource",
"//drake/lcm",
"//drake/multibody/parsers",
"//drake/systems/framework",
"@com_github_gflags_gflags//:gflags",
],
)

# === test/ ===

drake_cc_googletest(
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
#include "drake/examples/kuka_iiwa_arm/dev/tools/simple_tree_visualizer.h"
#include "drake/manipulation/util/simple_tree_visualizer.h"

#include <vector>

Expand All @@ -13,13 +13,14 @@
#include "drake/systems/framework/basic_vector.h"

namespace drake {
namespace examples {
namespace kuka_iiwa_arm {
namespace tools {
namespace manipulation {

SimpleTreeVisualizer::SimpleTreeVisualizer(const RigidBodyTreed& tree,
lcm::DrakeLcmInterface* lcm)
: tree_(tree), state_dimension_(tree_.get_num_positions() +
tree_.get_num_velocities()), draw_message_translator_(tree_), lcm_(lcm) {
: tree_(tree),
state_dimension_(tree_.get_num_positions() + tree_.get_num_velocities()),
draw_message_translator_(tree_),
lcm_(lcm) {
// Loads the robot.
const lcmt_viewer_load_robot load_message(
multibody::CreateLoadRobotMessage<double>(tree_));
Expand All @@ -42,15 +43,13 @@ void SimpleTreeVisualizer::visualize(const VectorX<double>& position_vector) {
state_vector.SetFromVector(state);

std::vector<uint8_t> message_bytes;
draw_message_translator_.Serialize(0 /* context get time */, state_vector,
&message_bytes);
constexpr double kTime = 0;
draw_message_translator_.Serialize(kTime, state_vector, &message_bytes);

// Publishes onto the specified LCM channel.
lcm_->Publish("DRAKE_VIEWER_DRAW", message_bytes.data(),
message_bytes.size());
}

} // namespace tools
} // namespace kuka_iiwa_arm
} // namespace examples
} // namespace manipulation
} // namespace drake
Original file line number Diff line number Diff line change
Expand Up @@ -4,15 +4,15 @@
#include "drake/multibody/rigid_body_plant/viewer_draw_translator.h"

namespace drake {
namespace examples {
namespace kuka_iiwa_arm {
namespace tools {
namespace manipulation {

/**
* A utility to render a `RigidBodyTree` in a specified configuration in the
* `drake-visualizer` app.
* A utility to render a `RigidBodyTree` in a specified configuration.
*/
class SimpleTreeVisualizer {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SimpleTreeVisualizer)

/**
* Constructs the `SimpleTreeVisualizer` and publishes a
* DRAKE_VIEWER_LOAD_ROBOT message. Note that the drake-visualizer must be
Expand All @@ -24,7 +24,6 @@ class SimpleTreeVisualizer {
*/
SimpleTreeVisualizer(const RigidBodyTreed& tree, lcm::DrakeLcmInterface* lcm);

DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SimpleTreeVisualizer)
/**
* Visualizes a given state position configuration.
* @param position_vector : A `VectorX` with the positions that are
Expand All @@ -41,7 +40,5 @@ class SimpleTreeVisualizer {
lcm::DrakeLcmInterface* lcm_{nullptr};
};

} // namespace tools
} // namespace kuka_iiwa_arm
} // namespace examples
} // namespace manipulation
} // namespace drake
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
/**
* @file test demo to visualize a given tree in a random set of configurations.
*/
#include "drake/examples/kuka_iiwa_arm/dev/tools/simple_tree_visualizer.h"
#include "drake/manipulation/util/simple_tree_visualizer.h"

#include <chrono>
#include <thread>
Expand All @@ -18,9 +18,7 @@ DEFINE_int32(num_configurations, 10,
"Number of random test configurations to display in the demo");

namespace drake {
namespace examples {
namespace kuka_iiwa_arm {
namespace tools {
namespace manipulation {
namespace {

int DoMain() {
Expand Down Expand Up @@ -57,12 +55,10 @@ int DoMain() {
}

} // namespace
} // namespace tools
} // namespace kuka_iiwa_arm
} // namespace examples
} // namespace manipulation
} // namespace drake

int main(int argc, char* argv[]) {
gflags::ParseCommandLineFlags(&argc, &argv, true);
return drake::examples::kuka_iiwa_arm::tools::DoMain();
return drake::manipulation::DoMain();
}

0 comments on commit 07fedc5

Please sign in to comment.