Drake Tutorial
Search…
Things to do in drake
Drake Controllers
Drake Optimization
Deprecated
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!
​
Copy link