def visualize(self, vis, x_vec):
x_dict = self._state_vector_to_dict(x_vec)
for limb in self.moving_limbs:
translation = [0.] + list(
(self.nominal_limb_positions[limb] + x_dict[limb]).flatten())
vis[limb].settransform(
transformations.translation_matrix(translation))
for limb in self.fixed_limbs:
translation = [0.] + list(
(self.topology['fixed'][limb].position).flatten())
vis[limb].settransform(
transformations.translation_matrix(translation))
translation = [0.] + list(x_dict['bq'].flatten())
vis['bq'].settransform(transformations.translation_matrix(translation))
return vis
# messages, so we'll wait until it sends its first status message.
print("waiting for viewer to initialize")
lc = lcm.LCM()
lc.subscribe("DIRECTOR_TREE_VIEWER_RESPONSE", lambda c, d: None)
vis_process = subprocess.Popen([vis_binary, '--testing', '--interactive'])
# Wait for one LCM message to be received.
lc.handle()
# We can provide an initial path if we want
vis = Visualizer(path="/root/folder1")
vis["boxes"].setgeometry(
[GeometryData(Box([1, 1, 1]),
color=np.hstack((np.random.rand(3), 0.5)),
transform=transformations.translation_matrix([x, -2, 0]))
for x in range(10)])
# Index into the visualizer to get a sub-tree. vis.__getitem__ is lazily
# implemented, so these sub-visualizers come into being as soon as they're
# asked for
vis = vis["group1"]
box_vis = vis["box"]
sphere_vis = vis["sphere"]
box = Box([1, 1, 1])
geom = GeometryData(box, color=[0, 1, 0, 0.5])
box_vis.setgeometry(geom)
sphere_vis.setgeometry(Sphere(1.0))
# messages, so we'll wait until it sends its first status message.
print "waiting for viewer to initialize"
lc = lcm.LCM()
lc.subscribe("DIRECTOR_TREE_VIEWER_RESPONSE", lambda c, d: None)
vis_process = subprocess.Popen([vis_binary, '--testing', '--interactive'])
# Wait for one LCM message to be received.
lc.handle()
# We can provide an initial path if we want
vis = Visualizer(path="/root/folder1")
vis["boxes"].setgeometry(
[GeometryData(Box([1, 1, 1]),
color=np.hstack((np.random.rand(3), 0.5)),
transform=transformations.translation_matrix([x, -2, 0]))
for x in range(10)])
# Index into the visualizer to get a sub-tree. vis.__getitem__ is lazily
# implemented, so these sub-visualizers come into being as soon as they're
# asked for
vis = vis["group1"]
box_vis = vis["box"]
sphere_vis = vis["sphere"]
box = Box([1, 1, 1])
geom = GeometryData(box, color=[0, 1, 0, 0.5])
box_vis.setgeometry(geom)
sphere_vis.setgeometry(Sphere(1.0))
def pose_msg2homogeneous(pose): trans = transformations.translation_matrix((pose.position.x, pose.position.y, pose.position.z)) rot = transformations.quaternion_matrix((pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)) return transformations.concatenate_matrices(trans, rot)
