def test_pose_vector(self): value = PoseVector() self.assertTrue(isinstance(value, BasicVector)) self.assertTrue(isinstance(copy.copy(value), PoseVector)) self.assertTrue(isinstance(value.Clone(), PoseVector)) self.assertEqual(value.size(), PoseVector.kSize) # - Accessors. with catch_drake_warnings(expected_count=1): self.assertTrue(isinstance(value.get_isometry(), Isometry3)) self.assertTrue(isinstance(value.get_transform(), RigidTransform)) self.assertTrue(isinstance( value.get_rotation(), Quaternion)) self.assertTrue(isinstance( value.get_translation(), np.ndarray)) # - Value. with catch_drake_warnings(expected_count=1): self.assertTrue(np.allclose( value.get_isometry().matrix(), np.eye(4, 4))) self.assertTrue(np.allclose( value.get_transform().GetAsMatrix4(), np.eye(4, 4))) # - Mutators. p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) X_expected = RigidTransform(quaternion=q, p=p) value.set_translation(p) value.set_rotation(q) with catch_drake_warnings(expected_count=1): self.assertTrue(np.allclose( value.get_isometry().matrix(), X_expected.GetAsMatrix4())) self.assertTrue(np.allclose( value.get_transform().GetAsMatrix4(), X_expected.GetAsMatrix4())) # - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz)) vector_expected = np.hstack((p, q.wxyz())) vector_actual = value.get_value() self.assertTrue(np.allclose(vector_actual, vector_expected)) # - Fully-parameterized constructor. value1 = PoseVector(rotation=q, translation=p) with catch_drake_warnings(expected_count=1): self.assertTrue(np.allclose( value1.get_isometry().matrix(), X_expected.GetAsMatrix4())) self.assertTrue(np.allclose( value1.get_transform().GetAsMatrix4(), X_expected.GetAsMatrix4())) # Test mutation via RigidTransform p2 = [10, 20, 30] q2 = Quaternion(wxyz=normalized([0.2, 0.3, 0.5, 0.8])) X2_expected = RigidTransform(quaternion=q2, p=p2) value.set_transform(X2_expected) self.assertTrue(np.allclose( value.get_transform().GetAsMatrix4(), X2_expected.GetAsMatrix4()))
class TestTransformPoints(unittest.TestCase): def setUp(self): self.points = np.array([[1, 1, 0], [2, 1, 0]]).T self.translation = RigidTransform(p=[1, 2, 3]) self.rotation = RigidTransform( RotationMatrix(RollPitchYaw(0, 0, np.pi / 2))) def test_translation(self): transformed_points = _TransformPoints(self.points, self.translation.GetAsMatrix4()) expected_translated_points = np.array([[2, 3, 3], [3, 3, 3]]).T self.assertTrue( np.allclose(transformed_points, expected_translated_points)) def test_rotation(self): transformed_points = _TransformPoints(self.points, self.rotation.GetAsMatrix4()) expected_rotated_points = np.array([[-1, 1, 0], [-1, 2, 0]]).T self.assertTrue( np.allclose(transformed_points, expected_rotated_points))
def buildViewPatches(self, use_random_colors): ''' Generates view patches. self.viewPatches stores a list of viewPatches for each body (starting at body id 1). A viewPatch is a list of all 3D vertices of a piece of visual geometry. ''' self.viewPatches = {} self.viewPatchColors = {} mock_lcm = DrakeMockLcm() mock_lcm_subscriber = Subscriber(lcm=mock_lcm, channel="DRAKE_VIEWER_LOAD_ROBOT", lcm_type=lcmt_viewer_load_robot) DispatchLoadMessage(self._scene_graph, mock_lcm) mock_lcm.HandleSubscriptions(0) assert mock_lcm_subscriber.count > 0 load_robot_msg = mock_lcm_subscriber.message # Spawn a random color generator, in case we need to pick # random colors for some bodies. Each body will be given # a unique color when using this random generator, with # each visual element of the body colored the same. color = iter( plt.cm.rainbow(np.linspace(0, 1, load_robot_msg.num_links))) for i in range(load_robot_msg.num_links): link = load_robot_msg.link[i] this_body_patches = [] this_body_colors = [] this_color = next(color) for j in range(link.num_geom): geom = link.geom[j] # MultibodyPlant currently sets alpha=0 to make collision # geometry "invisible". Ignore those geometries here. if geom.color[3] == 0: continue element_local_tf = RigidTransform( RotationMatrix(Quaternion(geom.quaternion)), geom.position) if geom.type == geom.BOX: assert geom.num_float_data == 3 # Draw a bounding box. patch = np.vstack((geom.float_data[0] / 2. * np.array([-1, -1, 1, 1, -1, -1, 1, 1]), geom.float_data[1] / 2. * np.array([-1, 1, -1, 1, -1, 1, -1, 1]), geom.float_data[2] / 2. * np.array([-1, -1, -1, -1, 1, 1, 1, 1]))) elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 radius = geom.float_data[0] lati, longi = np.meshgrid(np.arange(0., 2. * math.pi, 0.5), np.arange(0., 2. * math.pi, 0.5)) lati = lati.ravel() longi = longi.ravel() patch = np.vstack([ np.sin(longi) * np.cos(lati), np.sin(longi) * np.sin(lati), np.cos(lati) ]) patch *= radius elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 radius = geom.float_data[0] length = geom.float_data[1] # In the lcm geometry, cylinders are along +z # https://github.com/RobotLocomotion/drake/blob/last_sha_with_original_matlab/drake/matlab/systems/plants/RigidBodyCylinder.m # Two circles: one at bottom, one at top. sample_pts = np.arange(0., 2. * math.pi, 0.25) patch = np.hstack([ np.array([[ radius * math.cos(pt), radius * math.sin(pt), -length / 2. ], [ radius * math.cos(pt), radius * math.sin(pt), length / 2. ]]).T for pt in sample_pts ]) elif geom.type == geom.MESH: # TODO(gizatt): Remove trimesh and shapely dependency when # vertex information is accessible from the SceneGraph # interface. mesh = trimesh.load(geom.string_data) patch = mesh.vertices.T # Apply scaling for i in range(3): patch[i, :] *= geom.float_data[i] else: print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format( geom.type)) continue patch = np.vstack((patch, np.ones((1, patch.shape[1])))) patch = np.dot(element_local_tf.GetAsMatrix4(), patch) # Close path if not closed if (patch[:, -1] != patch[:, 0]).any(): patch = np.hstack((patch, patch[:, 0][np.newaxis].T)) this_body_patches.append(patch) if use_random_colors: this_body_colors.append(this_color) else: this_body_colors.append(geom.color) self.viewPatches[link.name] = this_body_patches self.viewPatchColors[link.name] = this_body_colors
class MeshcatPointCloudVisualizer(LeafSystem): """ MeshcatPointCloudVisualizer is a System block that visualizes a PointCloud in meshcat. The PointCloud: * Must have XYZ values. Assumed to be in point cloud frame, ``P``. * RGB values are optional; if provided, they must be on the range [0..255]. An example using a pydrake MeshcatVisualizer:: viz = builder.AddSystem(MeshcatVisualizer(scene_graph)) pc_viz = builder.AddSystem( MeshcatPointCloudVisualizer(viz, viz.draw_period)) Using a native meshcat.Visualizer:: viz = meshcat.Visualizer() pc_viz = builder.AddSystem(MeshcatPointCloudVisualizer(viz)) .. pydrake_system:: name: MeshcatPointCloudVisualizer input_ports: - point_cloud_P """ def __init__(self, meshcat_viz, draw_period=_DEFAULT_PUBLISH_PERIOD, name="point_cloud", X_WP=RigidTransform.Identity(), default_rgb=[255., 255., 255.]): """ Args: meshcat_viz: Either a native meshcat.Visualizer or a pydrake MeshcatVisualizer object. draw_period: The rate at which this class publishes to the visualizer. name: The string name of the meshcat object. X_WP: Pose of point cloud frame ``P`` in meshcat world frame ``W``. Default is identity. default_rgb: RGB value for published points if the PointCloud does not provide RGB values. """ LeafSystem.__init__(self) self._meshcat_viz = _get_native_visualizer(meshcat_viz) self._X_WP = RigidTransform(X_WP) self._default_rgb = np.array(default_rgb) self._name = name self.set_name('meshcat_point_cloud_visualizer_' + name) self.DeclarePeriodicPublish(draw_period, 0.0) self.DeclareAbstractInputPort("point_cloud_P", AbstractValue.Make(mut.PointCloud())) def DoPublish(self, context, event): # TODO(SeanCurtis-TRI) We want to be able to use this visualizer to # draw without having it part of a Simulator. That means we'd like # vis.Publish(context) to work. Currently, pydrake offers no mechanism # to declare a forced event. However, by overriding DoPublish and # putting the forced event callback code in the override, we can # simulate it. # We need to bind a mechanism for declaring forced events so we don't # have to resort to overriding the dispatcher. LeafSystem.DoPublish(self, context, event) point_cloud_P = self.EvalAbstractInput(context, 0).get_value() # `Q` is a point in the point cloud. p_PQs = point_cloud_P.xyzs() # Use only valid points. valid = np.logical_not(np.isnan(p_PQs)) valid = np.all(valid, axis=0) # Reduce along XYZ axis. p_PQs = p_PQs[:, valid] if point_cloud_P.has_rgbs(): rgbs = point_cloud_P.rgbs()[:, valid] else: # Need manual broadcasting. count = p_PQs.shape[1] rgbs = np.tile(np.array([self._default_rgb]).T, (1, count)) # pydrake `PointCloud.rgbs()` are on [0..255], while meshcat # `PointCloud` colors are on [0..1]. rgbs = rgbs / 255. # Do not use in-place so we can promote types. # Send to meshcat. self._meshcat_viz[self._name].set_object(g.PointCloud(p_PQs, rgbs)) self._meshcat_viz[self._name].set_transform(self._X_WP.GetAsMatrix4())
def DoPublish(self, context, event): # TODO(SeanCurtis-TRI) We want to be able to use this visualizer to # draw without having it part of a Simulator. That means we'd like # vis.Publish(context) to work. Currently, pydrake offers no mechanism # to declare a forced event. However, by overriding DoPublish and # putting the forced event callback code in the override, we can # simulate it. # We need to bind a mechanism for declaring forced events so we don't # have to resort to overriding the dispatcher. LeafSystem.DoPublish(self, context, event) contact_results = self.EvalAbstractInput(context, 0).get_value() vis = self._meshcat_viz.vis[self._meshcat_viz.prefix]["contact_forces"] contacts = [] for i_contact in range(contact_results.num_point_pair_contacts()): contact_info = contact_results.point_pair_contact_info(i_contact) # Do not display small forces. force_norm = np.linalg.norm(contact_info.contact_force()) if force_norm < self._force_threshold: continue point_pair = contact_info.point_pair() key = (point_pair.id_A.get_value(), point_pair.id_B.get_value()) cvis = vis[str(key)] contacts.append(key) arrow_height = self._radius * 2.0 if key not in self._published_contacts: # New key, so create the geometry. Note: the height of the # cylinder is 2 and gets scaled to twice the contact force # length, because I am drawing both (equal and opposite) # forces. Note also that meshcat (following three.js) puts # the height of the cylinder along the y axis. cvis["cylinder"].set_object( meshcat.geometry.Cylinder(height=2.0, radius=self._radius), meshcat.geometry.MeshLambertMaterial(color=0x33cc33)) cvis["head"].set_object( meshcat.geometry.Cylinder(height=arrow_height, radiusTop=0, radiusBottom=self._radius * 2.0), meshcat.geometry.MeshLambertMaterial(color=0x00dd00)) cvis["tail"].set_object( meshcat.geometry.Cylinder(height=arrow_height, radiusTop=self._radius * 2.0, radiusBottom=0), meshcat.geometry.MeshLambertMaterial(color=0x00dd00)) height = force_norm / self._contact_force_scale cvis["cylinder"].set_transform( tf.scale_matrix(height, direction=[0, 1, 0])) cvis["head"].set_transform( tf.translation_matrix([0, height + arrow_height / 2.0, 0.0])) cvis["tail"].set_transform( tf.translation_matrix([0, -height - arrow_height / 2.0, 0.0])) # The contact frame's origin is located at contact_point and is # oriented such that Cy is aligned with the contact force. p_WC = contact_info.contact_point() # documented as in world. if force_norm < 1e-6: # We cannot rely on self._force_threshold to determine if the # force can be normalized; that threshold can be zero. R_WC = RotationMatrix() else: fhat_C_W = contact_info.contact_force() / force_norm R_WC = RotationMatrix.MakeFromOneVector(b_A=fhat_C_W, axis_index=1) X_WC = RigidTransform(R=R_WC, p=p_WC) cvis.set_transform(X_WC.GetAsMatrix4()) # We only delete any contact vectors that did not persist into this # publish. It is tempting to just delete() the root branch at the # beginning of this publish, but this leads to visual artifacts # (flickering) in the browser. for key in set(self._published_contacts) - set(contacts): vis[str(key)].delete() self._published_contacts = contacts
def buildViewPatches(self, use_random_colors): ''' Generates view patches. self.viewPatches stores a list of viewPatches for each body (starting at body id 1). A viewPatch is a list of 2D coordinates in counterclockwise order forming the boundary of a filled polygon representing a piece of visual geometry. ''' self.viewPatches = {} self.viewPatchColors = {} mock_lcm = DrakeMockLcm() mock_lcm_subscriber = Subscriber(lcm=mock_lcm, channel="DRAKE_VIEWER_LOAD_ROBOT", lcm_type=lcmt_viewer_load_robot) DispatchLoadMessage(self._scene_graph, mock_lcm) mock_lcm.HandleSubscriptions(0) assert mock_lcm_subscriber.count > 0 load_robot_msg = mock_lcm_subscriber.message # Spawn a random color generator, in case we need to pick # random colors for some bodies. Each body will be given # a unique color when using this random generator, with # each visual element of the body colored the same. color = iter(plt.cm.rainbow(np.linspace(0, 1, load_robot_msg.num_links))) for i in range(load_robot_msg.num_links): link = load_robot_msg.link[i] this_body_patches = [] this_body_colors = [] this_color = next(color) for j in range(link.num_geom): geom = link.geom[j] # MultibodyPlant currently sets alpha=0 to make collision # geometry "invisible". Ignore those geometries here. if geom.color[3] == 0: continue element_local_tf = RigidTransform( RotationMatrix(Quaternion(geom.quaternion)), geom.position) if geom.type == geom.BOX: assert geom.num_float_data == 3 # Draw a bounding box. patch = np.vstack(( geom.float_data[0]/2.*np.array([1, 1, 1, 1, -1, -1, -1, -1]), geom.float_data[1]/2.*np.array([1, 1, 1, 1, -1, -1, -1, -1]), geom.float_data[2]/2.*np.array([1, 1, -1, -1, -1, -1, 1, 1]))) elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 radius = geom.float_data[0] sample_pts = np.arange(0., 2.*math.pi, 0.25) patch = np.vstack([math.cos(pt)*self.Tview[0, 0:3] + math.sin(pt)*self.Tview[1, 0:3] for pt in sample_pts]) patch = np.transpose(patch) patch *= radius elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 radius = geom.float_data[0] length = geom.float_data[1] # In the lcm geometry, cylinders are along +z # https://github.com/RobotLocomotion/drake/blob/last_sha_with_original_matlab/drake/matlab/systems/plants/RigidBodyCylinder.m # I don't have access to the body to world transform # yet; decide between drawing a box and circle assuming the # T_body_to_world is will not rotate us out of the # viewing plane. z_axis = np.matmul(self.Tview[0:2, 0:3], element_local_tf.multiply([0, 0, 1])) if np.linalg.norm(z_axis) < 0.01: # Draw a circle. sample_pts = np.arange(0., 2.*math.pi, 0.25) patch = np.vstack([math.cos(pt)*self.Tview[0, 0:3] + math.sin(pt)*self.Tview[1, 0:3] for pt in sample_pts]) patch = np.transpose(patch) patch *= radius else: # Draw a bounding box. patch = np.vstack(( radius*np.array([1, 1, 1, 1, -1, -1, -1, -1]), radius*np.array([1, 1, 1, 1, -1, -1, -1, -1]), (length/2)*np.array([1, 1, -1, -1, -1, -1, 1, 1]))) else: print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format( geom.type)) continue patch = np.vstack((patch, np.ones((1, patch.shape[1])))) patch = np.dot(element_local_tf.GetAsMatrix4(), patch) # Project into 2D patch = np.dot(self.Tview, patch) # Close path if not closed if (patch[:, -1] != patch[:, 0]).any(): patch = np.hstack((patch, patch[:, 0][np.newaxis].T)) this_body_patches.append(patch) if use_random_colors: this_body_colors.append(this_color) else: this_body_colors.append(geom.color) self.viewPatches[link.name] = this_body_patches self.viewPatchColors[link.name] = this_body_colors