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)