Visualize trajectory
Drake does not support visualizing the trajectory yet. To show any customized data format in visualizer, we need a plugin.
Visualize trajectory

Create plugin

You could place your plugin anywhere as long as they load correctly. I would put the plugin file show_trajectory.py under drake/tools/workspace/drake_visualizer/plugin/
show_trajectory.py
1
# Note that this script runs in the main context of drake-visulizer,
2
# where many modules and variables already exist in the global scope.
3
from sets import Set
4
from director import lcmUtils
5
from director import applogic
6
from director import objectmodel as om
7
from director import transformUtils
8
from director import visualization as vis
9
from director.debugVis import DebugData
10
from six import iteritems
11
import numpy as np
12
import robotlocomotion as lcmrobotlocomotion
13
14
from drake.tools.workspace.drake_visualizer.plugin import scoped_singleton_func
15
16
17
class FrameChannel(object):
18
19
def __init__(self, parent_folder, channel):
20
self._parent_folder = parent_folder
21
self._channel = channel
22
# Link names that were previously published.
23
self._link_name_published = []
24
25
def handle_message(self, msg):
26
print("frame channel was called")
27
if set(self._link_name_published) != set(msg.link_name):
28
# Removes the folder completely.
29
self.remove_folder()
30
self._link_name_published = msg.link_name
31
32
folder = self._get_folder()
33
34
for i in range(0, msg.num_links):
35
name = msg.link_name[i]
36
transform = transformUtils.transformFromPose(
37
msg.position[i], msg.quaternion[i])
38
# `vis.updateFrame` will either create or update the frame
39
# according to its name within its parent folder.
40
vis.updateFrame(transform, name, parent=folder, scale=0.1)
41
42
# Create map of body names to a list of contact forces
43
collision_pair_to_forces = {}
44
if msg.num_links > 1:
45
for i in range(1, msg.num_links):
46
name = msg.link_name[i]
47
# msg.position[i] is tuple and can be transformed into np array.
48
point1 = np.array(msg.position[i-1])
49
point2 = np.array(msg.position[i])
50
collision_pair_to_forces[name] = [(point1, point2)]
51
52
for key, list_of_forces in iteritems(collision_pair_to_forces):
53
d = DebugData()
54
for force_pair in list_of_forces:
55
d.addArrow(start=force_pair[0],
56
end=force_pair[1],
57
tubeRadius=0.005,
58
headRadius=0.01)
59
60
vis.showPolyData(
61
d.getPolyData(), str(key), parent=folder, color=[0, 1, 0])
62
63
def _get_folder(self):
64
return om.getOrCreateContainer(
65
self._channel, parentObj=self._parent_folder)
66
67
def remove_folder(self):
68
om.removeFromObjectModel(self._get_folder())
69
70
71
class FramesVisualizer(object):
72
73
def __init__(self):
74
self._name = "Frame Visualizer"
75
self._subscriber = None
76
self._frame_channels = {}
77
self.set_enabled(True)
78
79
def _add_subscriber(self):
80
if (self._subscriber is not None):
81
return
82
83
self._subscriber = lcmUtils.addSubscriber(
84
'DRAKE_DRAW_TRAJECTORY.*',
85
messageClass=lcmrobotlocomotion.viewer_draw_t,
86
callback=self._handle_message,
87
callbackNeedsChannel=True)
88
self._subscriber.setNotifyAllMessagesEnabled(True)
89
90
def _get_folder(self):
91
return om.getOrCreateContainer(self._name)
92
93
def _remove_subscriber(self):
94
if (self._subscriber is None):
95
return
96
lcmUtils.removeSubscriber(self._subscriber)
97
for frame_channel in self._frame_channels:
98
frame_channel.remove_folder()
99
self._frame_channels.clear()
100
self._subscriber = None
101
om.removeFromObjectModel(self._get_folder())
102
103
def is_enabled(self):
104
return self._subscriber is not None
105
106
def set_enabled(self, enable):
107
if enable:
108
self._add_subscriber()
109
else:
110
self._remove_subscriber()
111
112
def _handle_message(self, msg, channel):
113
print("trajectory service was called")
114
frame_channel = self._frame_channels.get(channel)
115
if not frame_channel:
116
frame_channel = FrameChannel(
117
parent_folder=self._get_folder(), channel=channel)
118
self._frame_channels[channel] = frame_channel
119
frame_channel.handle_message(msg)
120
121
122
@scoped_singleton_func
123
def init_visualizer():
124
frame_viz = FramesVisualizer()
125
126
# Adds to the "Tools" menu.
127
applogic.MenuActionToggleHelper(
128
'Tools', frame_viz._name,
129
frame_viz.is_enabled, frame_viz.set_enabled)
130
return frame_viz
131
132
133
# Activate the plugin if this script is run directly; store the results to keep
134
# the plugin objects in scope.
135
if __name__ == "__main__":
136
frame_viz = init_visualizer()
Copied!
The plugin could be loaded by the drake_visualizer, it will call the _handle_message every time it gets a message data. The message data is defined as map{name, transformation}. The plugin will display all the points as frames and connect the points with arrows.

Create and send trajectory to visualizer

Add publisher code and BUILD.bazel under drake/examples/viz.
This code would create a piecewise cubic polynomial trajectory given several key points and sample points on the trajectory. The points are then packed into message and sent over to the drake_visualizer.
trajectory_publisher.cc
1
/// @file
2
///
3
/// This file creates a simple trajectory and visualize it.
4
5
/* Examples
6
7
PublishFramesToLcm("DRAKE_DRAW_TRAJECTORY", {
8
{"X_WF", Eigen::Isometry3d::Identity()},
9
{"X_WG", Eigen::Isometry3d::Identity()},
10
}, &lcm);
11
*/
12
13
#include <gflags/gflags.h>
14
15
#include "drake/common/drake_assert.h"
16
#include "drake/common/text_logging_gflags.h"
17
#include "drake/geometry/geometry_visualization.h"
18
#include "drake/geometry/scene_graph.h"
19
#include "drake/lcm/drake_lcm.h"
20
#include "drake/lcmt_viewer_draw.hpp"
21
#include "drake/multibody/parsing/parser.h"
22
#include "drake/multibody/plant/multibody_plant.h"
23
#include "drake/systems/analysis/simulator.h"
24
#include "drake/systems/framework/diagram.h"
25
#include "drake/systems/framework/diagram_builder.h"
26
27
namespace drake {
28
namespace examples {
29
namespace eve {
30
31
void PublishFramesToLcm(const std::string& channel_name,
32
const std::vector<Eigen::Isometry3d>& poses,
33
const std::vector<std::string>& names,
34
drake::lcm::DrakeLcmInterface* dlcm) {
35
DRAKE_DEMAND(poses.size() == names.size());
36
drake::lcmt_viewer_draw frame_msg{};
37
frame_msg.timestamp = 0;
38
int32_t vsize = poses.size();
39
frame_msg.num_links = vsize;
40
frame_msg.link_name.resize(vsize);
41
frame_msg.robot_num.resize(vsize, 0);
42
43
for (size_t i = 0; i < poses.size(); i++) {
44
Eigen::Isometry3f pose = poses[i].cast<float>();
45
// Create a frame publisher
46
Eigen::Vector3f goal_pos = pose.translation();
47
Eigen::Quaternion<float> goal_quat =
48
Eigen::Quaternion<float>(pose.linear());
49
frame_msg.link_name[i] = names[i];
50
frame_msg.position.push_back({goal_pos(0), goal_pos(1), goal_pos(2)});
51
frame_msg.quaternion.push_back(
52
{goal_quat.w(), goal_quat.x(), goal_quat.y(), goal_quat.z()});
53
}
54
55
const int num_bytes = frame_msg.getEncodedSize();
56
const size_t size_bytes = static_cast<size_t>(num_bytes);
57
std::vector<uint8_t> bytes(size_bytes);
58
frame_msg.encode(bytes.data(), 0, num_bytes);
59
dlcm->Publish("DRAKE_DRAW_TRAJECTORY_" + channel_name, bytes.data(),
60
num_bytes, {});
61
}
62
63
void PublishFramesToLcm(
64
const std::string& channel_name,
65
const std::unordered_map<std::string, Eigen::Isometry3d>& name_to_frame_map,
66
drake::lcm::DrakeLcmInterface* lcm) {
67
std::vector<Eigen::Isometry3d> poses;
68
std::vector<std::string> names;
69
for (const auto& pair : name_to_frame_map) {
70
poses.push_back(pair.second);
71
names.push_back(pair.first);
72
}
73
PublishFramesToLcm(channel_name, poses, names, lcm);
74
}
75
76
void DoMain() {
77
// Design the trajectory to follow.
78
const std::vector<double> kTimes{0.0, 2.0, 5.0, 10.0};
79
std::vector<Eigen::MatrixXd> knots(kTimes.size());
80
Eigen::VectorXd tmp1(3);
81
tmp1 << 0, 0, 0;
82
knots[0] = tmp1;
83
Eigen::VectorXd tmp2(3);
84
tmp2 << 1, 1, 0;
85
knots[1] = tmp2;
86
Eigen::VectorXd tmp3(3);
87
tmp3 << 2, -1, 0;
88
knots[2] = tmp3;
89
Eigen::VectorXd tmp4(3);
90
tmp4 << 3, 0, 0;
91
knots[3] = tmp4;
92
Eigen::VectorXd knot_dot_start = Eigen::VectorXd::Zero(3);
93
Eigen::MatrixXd knot_dot_end = Eigen::VectorXd::Zero(3);
94
trajectories::PiecewisePolynomial<double> trajectory =
95
trajectories::PiecewisePolynomial<double>::Cubic(
96
kTimes, knots, knot_dot_start, knot_dot_end);
97
98
std::vector<std::string> names;
99
std::vector<Eigen::Isometry3d> poses;
100
for (double t = 0.0; t < 10.0; t += 0.1) {
101
names.push_back("X" + std::to_string(int(t * 100)));
102
Eigen::Isometry3d pose = Eigen::Isometry3d::Identity();
103
pose.translation() = trajectory.value(t);
104
poses.push_back(pose);
105
}
106
lcm::DrakeLcm lcm;
107
108
// std::vector<std::string> names = {"X_WF", "X_WG"};
109
// Eigen::Isometry3d pose1 = Eigen::Isometry3d::Identity();
110
// pose1.translation() = Eigen::Vector3d::Ones()*0.5;
111
// Eigen::Isometry3d pose2 = Eigen::Isometry3d::Identity();
112
// Eigen::Vector3d translation2; translation2 << 1,2,3;
113
// pose1.translation() = translation2;
114
// std::vector<Eigen::Isometry3d> poses = {pose1, pose2};
115
116
PublishFramesToLcm("DRAKE_DRAW_TRAJECTORY", poses, names, &lcm);
117
}
118
} // namespace eve
119
} // namespace examples
120
} // namespace drake
121
122
int main(int argc, char* argv[]) {
123
gflags::SetUsageMessage(
124
"A simple dynamic simulation for the Allegro hand moving under constant"
125
" torques.");
126
gflags::ParseCommandLineFlags(&argc, &argv, true);
127
drake::examples::eve::DoMain();
128
return 0;
129
}
130
Copied!
BUILD.bazel
1
drake_cc_binary(
2
name = "trajectory_publisher",
3
srcs = ["trajectory_publisher.cc"],
4
data = [
5
"//manipulation/models/eve:models",
6
"//manipulation/models/qb_hand_description:models",
7
],
8
deps = [
9
"//common:find_resource",
10
"//common:text_logging_gflags",
11
"//geometry:geometry_visualization",
12
"//lcm",
13
"//lcmtypes:viewer",
14
"//multibody/benchmarks/inclined_plane",
15
"//multibody/parsing",
16
"//multibody/plant",
17
"//multibody/plant:contact_results_to_lcm",
18
"//systems/analysis",
19
"//systems/controllers",
20
"//systems/primitives",
21
"//systems/rendering:pose_bundle_to_draw_message",
22
"@gflags",
23
],
24
)
Copied!

Run the demo

Compile the visualizer again, and open visualizer with plugin.
1
bazel build //tools:drake_visualizer
2
bazel-bin/tools/drake_visualizer --script=tools/workspace/drake_visualizer/plugin/show_trajectory.py
Copied!
Run the trajectory publisher.
1
bazel run //examples/viz:trajectory_publisher
Copied!