def addTestData(): d = DebugData() # d.addSphere((0,0,0), radius=0.5) d.addArrow((0, 0, 0), (0, 0, 1), color=[1, 0, 0]) d.addArrow((0, 0, 1), (0, .5, 1), color=[0, 1, 0]) vis.showPolyData(d.getPolyData(), 'debug data', colorByName='RGB255') view.resetCamera()
def newMesh(): d = DebugData() d.addArrow((0,0,0), (0,0,0.3)) pd = d.getPolyData() meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd) desc = dict(classname='MeshAffordanceItem', Name='test mesh', Filename=meshId, pose=((0.5,0.0,1.0), (1,0,0,0))) return affordanceManager.newAffordanceFromDescription(desc)
def handle_message(self, msg): # Limits the rate of message handling, since redrawing is done in the # message handler. self._sub.setSpeedLimit(0.1) # Removes the folder completely. om.removeFromObjectModel(om.findObjectByName(self._folder_name)) # Recreates folder. folder = om.getOrCreateContainer(self._folder_name) # A map from pair of body names to a list of contact forces collision_pair_to_forces = {} for arrow in msg.arrow_info: point = np.array([ arrow.arrow_origin[0], arrow.arrow_origin[1], arrow.arrow_origin[2] ]) vec = np.array([ arrow.arrow_vector[0], arrow.arrow_vector[1], arrow.arrow_vector[2] ]) d = DebugData() d.addArrow(start=point, end=vec + point, tubeRadius=0.005, headRadius=0.01) vis.showPolyData(d.getPolyData(), str(0), parent=folder, color=[arrow.rgb[0], arrow.rgb[1], arrow.rgb[2]])
def addTestData(): d = DebugData() # d.addSphere((0,0,0), radius=0.5) d.addArrow((0,0,0), (0,0,1), color=[1,0,0]) d.addArrow((0,0,1), (0,.5,1), color=[0,1,0]) vis.showPolyData(d.getPolyData(), 'debug data', colorByName='RGB255') view.resetCamera()
def onSpawnMesh(self): d = DebugData() d.addArrow((0,0,0), (0,0,0.3)) pd = d.getPolyData() meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd) pose = transformUtils.poseFromTransform(self.getSpawnFrame()) desc = dict(classname='MeshAffordanceItem', Name='mesh', Filename=meshId, uuid=newUUID(), pose=pose) return self.affordanceManager.newAffordanceFromDescription(desc)
def onSpawnMesh(self): d = DebugData() d.addArrow((0, 0, 0), (0, 0, 0.3)) pd = d.getPolyData() meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd) pose = transformUtils.poseFromTransform(self.getSpawnFrame()) desc = dict(classname='MeshAffordanceItem', Name='mesh', Filename=meshId, uuid=newUUID(), pose=pose) return self.affordanceManager.newAffordanceFromDescription(desc)
def handle_message(self, msg): # Limits the rate of message handling, since redrawing is done in the # message handler. self._sub.setSpeedLimit(30) # Removes the folder completely. om.removeFromObjectModel(om.findObjectByName(self._folder_name)) # Recreates folder. folder = om.getOrCreateContainer(self._folder_name) # A map from pair of body names to a list of contact forces collision_pair_to_forces = {} for contact in msg.contact_info: point = np.array([ contact.contact_point[0], contact.contact_point[1], contact.contact_point[2] ]) force = np.array([ contact.contact_force[0], contact.contact_force[1], contact.contact_force[2] ]) mag = np.linalg.norm(force) if mag > 1e-4: mag = 0.3 / mag key1 = (str(contact.body1_name), str(contact.body2_name)) key2 = (str(contact.body2_name), str(contact.body1_name)) if key1 in collision_pair_to_forces: collision_pair_to_forces[key1].append( (point, point + mag * force)) elif key2 in collision_pair_to_forces: collision_pair_to_forces[key2].append( (point, point + mag * force)) else: collision_pair_to_forces[key1] = [(point, point + mag * force)] for key, list_of_forces in collision_pair_to_forces.iteritems(): d = DebugData() for force_pair in list_of_forces: d.addArrow(start=force_pair[0], end=force_pair[1], tubeRadius=0.005, headRadius=0.01) vis.showPolyData(d.getPolyData(), str(key), parent=folder, color=[0, 1, 0])
def doFTDraw(self, unusedrobotstate): frames = [] fts = [] vis_names = [] if (hasattr(self.robotStateJointController, 'lastRobotStateMessage') and self.robotStateJointController.lastRobotStateMessage): if self.draw_right: rft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_force + self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_torque) rft[0] = -1.*rft[0] # right FT looks to be rotated 180deg around the z axis rft[1] = -1.*rft[1] rft[3] = -1.*rft[3] rft[4] = -1.*rft[4] rft -= self.ft_right_bias fts.append(rft) frames.append(self.robotStateModel.getLinkFrame('r_hand_force_torque')) vis_names.append('ft_right') if self.draw_left: lft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_force + self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_torque) lft -= self.ft_left_bias fts.append(lft) frames.append(self.robotStateModel.getLinkFrame('l_hand_force_torque')) vis_names.append('ft_left') for i in range(0, len(frames)): frame = frames[i] ft = fts[i] offset = [0., 0., 0.] p1 = frame.TransformPoint(offset) scale = 1.0/25.0 # todo add slider for this? scalet = 1.0 / 2.5 p2f = frame.TransformPoint(offset + ft[0:3]*scale) p2t = frame.TransformPoint(offset + ft[3:6]) normalt = (np.array(p2t) - np.array(p1)) normalt = normalt / np.linalg.norm(normalt) d = DebugData() # force if np.linalg.norm(np.array(p2f) - np.array(p1)) < 0.1: d.addLine(p1, p2f, color=[1.0, 0.0, 0.0]) else: d.addArrow(p1, p2f, color=[1.,0.,0.]) # torque if self.draw_torque: d.addCircle(p1, normalt, scalet*np.linalg.norm(ft[3:6])) # frame (largely for debug) vis.updateFrame(frame, vis_names[i]+'frame', view=self.view, parent='wristft', visible=False, scale=0.2) vis.updatePolyData(d.getPolyData(), vis_names[i], view=self.view, parent='wristft')
def buildAccelArrow(center, accelThrust, roll, pitch, color=[1,0.3,0.0], alpha=0.8): d = DebugData() a_x = accelThrust*np.sin(pitch) a_y = -accelThrust*np.cos(pitch)*np.sin(roll) a_z = accelThrust*np.cos(pitch)*np.cos(roll)-9.8 end = [center[0] + a_x*0.125, center[1] + a_y*0.125, center[2] + a_z*0.125] d.addArrow(center, end, tubeRadius=0.03, headRadius=0.15, color=color) polyData = d.getPolyData() #t = vtk.vtkTransform() #t.Translate(center) #polyData = filterUtils.transformPolyData(polyData, t) obj = vis.updatePolyData(polyData, 'accel_arrow', color=color, alpha=alpha) return polyData
def handle_message(self, msg): # Limits the rate of message handling, since redrawing is done in the # message handler. self._sub.setSpeedLimit(30) # Removes the folder completely. om.removeFromObjectModel(om.findObjectByName(self._folder_name)) # Recreates folder. folder = om.getOrCreateContainer(self._folder_name) # A map from pair of body names to a list of contact forces collision_pair_to_forces = {} for contact in msg.contact_info: point = np.array([contact.contact_point[0], contact.contact_point[1], contact.contact_point[2]]) force = np.array([contact.contact_force[0], contact.contact_force[1], contact.contact_force[2]]) mag = np.linalg.norm(force) if mag > 1e-4: mag = 0.3 / mag key1 = (str(contact.body1_name), str(contact.body2_name)) key2 = (str(contact.body2_name), str(contact.body1_name)) if key1 in collision_pair_to_forces: collision_pair_to_forces[key1].append( (point, point + mag * force)) elif key2 in collision_pair_to_forces: collision_pair_to_forces[key2].append( (point, point + mag * force)) else: collision_pair_to_forces[key1] = [(point, point + mag * force)] for key, list_of_forces in iteritems(collision_pair_to_forces): d = DebugData() for force_pair in list_of_forces: d.addArrow(start=force_pair[0], end=force_pair[1], tubeRadius=0.005, headRadius=0.01) vis.showPolyData( d.getPolyData(), str(key), parent=folder, color=[0, 1, 0])
def handle_message(self, msg): print("frame channel was called") if set(self._link_name_published) != set(msg.link_name): # Removes the folder completely. self.remove_folder() self._link_name_published = msg.link_name folder = self._get_folder() for i in range(0, msg.num_links): name = msg.link_name[i] transform = transformUtils.transformFromPose( msg.position[i], msg.quaternion[i]) # `vis.updateFrame` will either create or update the frame # according to its name within its parent folder. vis.updateFrame(transform, name, parent=folder, scale=0.1) # Create map of body names to a list of contact forces collision_pair_to_forces = {} if msg.num_links > 1: for i in range(1, msg.num_links): name = msg.link_name[i] # msg.position[i] is tuple and can be transformed into np array. point1 = np.array(msg.position[i - 1]) point2 = np.array(msg.position[i]) collision_pair_to_forces[name] = [(point1, point2)] for key, list_of_forces in iteritems(collision_pair_to_forces): d = DebugData() for force_pair in list_of_forces: d.addArrow(start=force_pair[0], end=force_pair[1], tubeRadius=0.005, headRadius=0.01) vis.showPolyData(d.getPolyData(), str(key), parent=folder, color=[0, 1, 0])
def segmentTable(scenePolyData=None, searchRadius=0.3, visualize=True, thickness=0.01, pointOnTable=None, pointAboveTable=None, computeAboveTablePolyData=False): """ This requires two clicks using measurement panel. One on the table, one above the table on one of the objects. Call them point0, point1. Then we will attempt to fit a plane that passes through point0 with approximate normal point1 - point0 :param scenePolyData: :param searchRadius: :return: """ if scenePolyData is None: scenePolyData = om.findObjectByName('reconstruction').polyData assert scenePolyData is not None assert pointOnTable is not None assert pointAboveTable is not None expectedNormal = pointAboveTable - pointOnTable expectedNormal = expectedNormal / np.linalg.norm(expectedNormal) polyData, normal = segmentation.applyPlaneFit( scenePolyData, searchOrigin=pointOnTable, searchRadius=searchRadius, expectedNormal=expectedNormal) # get points above plane abovePolyData = None belowPolyData = None if computeAboveTablePolyData: abovePolyData = filterUtils.thresholdPoints( polyData, 'dist_to_plane', [thickness / 2.0, np.inf]) belowPolyData = filterUtils.thresholdPoints( polyData, 'dist_to_plane', [-np.inf, -thickness / 2.0]) # some debugging visualization if visualize: visFolder = om.getOrCreateContainer('debug') if abovePolyData is not None: vis.showPolyData(abovePolyData, 'above table segmentation', color=[0, 1, 0], parent=visFolder) arrowLength = 0.3 headRadius = 0.02 d = DebugData() visFolder = om.getOrCreateContainer('debug') d.addArrow(pointOnTable, pointOnTable + arrowLength * expectedNormal, headRadius=headRadius) vis.showPolyData(d.getPolyData(), 'expected normal', color=[1, 0, 0], parent=visFolder) d = DebugData() d.addArrow(pointOnTable, pointOnTable + arrowLength * normal, headRadius=headRadius) vis.showPolyData(d.getPolyData(), 'computed normal', color=[0, 1, 0], parent=visFolder) returnData = dict() returnData['abovePolyData'] = abovePolyData returnData['polyData'] = polyData returnData['normal'] = normal returnData['pointOnTable'] = pointOnTable return returnData
def handle_message(self, msg): # Limits the rate of message handling, since redrawing is done in the # message handler. self._sub.setSpeedLimit(30) # Removes the folder completely. om.removeFromObjectModel(om.findObjectByName(self._folder_name)) # Recreates folder. folder = om.getOrCreateContainer(self._folder_name) # The scale value attributable to auto-scale. auto_scale = 1.0 max_force = -1 # A map from pair of body names to a list of contact forces collision_pair_to_forces = {} for contact in msg.point_pair_contact_info: point = np.array([ contact.contact_point[0], contact.contact_point[1], contact.contact_point[2] ]) force = np.array([ contact.contact_force[0], contact.contact_force[1], contact.contact_force[2] ]) mag = np.linalg.norm(force) if mag < self.min_magnitude: continue if mag > max_force: max_force = mag scale = self.global_scale if self.magnitude_mode == ContactVisModes.kFixedLength: # mag must be > 0 otherwise this force would be skipped. scale /= mag vis_force = force * scale key1 = (str(contact.body1_name), str(contact.body2_name)) key2 = (str(contact.body2_name), str(contact.body1_name)) if key1 in collision_pair_to_forces: collision_pair_to_forces[key1].append((point, vis_force)) elif key2 in collision_pair_to_forces: collision_pair_to_forces[key2].append((point, vis_force)) else: collision_pair_to_forces[key1] = [(point, vis_force)] if self.magnitude_mode == ContactVisModes.kAutoScale: auto_scale = 1.0 / max_force for key, list_of_forces in collision_pair_to_forces.items(): d = DebugData() for p, v in list_of_forces: d.addArrow(start=p, end=p + auto_scale * v, tubeRadius=0.005, headRadius=0.01) vis.showPolyData(d.getPolyData(), str(key), parent=folder, color=[0.2, 0.8, 0.2]) self.update_screen_text()
def handle_message(self, msg): # Limits the rate of message handling, since redrawing is done in the # message handler. self._sub.setSpeedLimit(30) # Removes the folder completely. om.removeFromObjectModel(om.findObjectByName(self._folder_name)) # Recreates folder. folder = om.getOrCreateContainer(self._folder_name) # Set the color map. color_map = self.create_color_map() # The scale value attributable to auto-scale. auto_force_scale = 1.0 auto_moment_scale = 1.0 auto_traction_scale = 1.0 auto_slip_velocity_scale = 1.0 max_force = -1 max_moment = -1 max_traction = -1 max_slip_speed = -1 # Determine scaling magnitudes if autoscaling is activated. if self.magnitude_mode == ContactVisModes.kAutoScale: for surface in msg.hydroelastic_contacts: if self.show_spatial_force: force = np.array([ surface.force_C_W[0], surface.force_C_W[1], surface.force_C_W[2] ]) moment = np.array([ surface.moment_C_W[0], surface.moment_C_W[1], surface.moment_C_W[2] ]) force_mag = np.linalg.norm(force) moment_mag = np.linalg.norm(moment) if force_mag > max_force: max_force = force_mag if moment_mag > max_moment: max_moment = moment_mag # Prepare scaling information for the traction vectors. if self.show_traction_vectors: for quad_point_data in surface.quadrature_point_data: traction = np.array([ quad_point_data.traction_Aq_W[0], quad_point_data.traction_Aq_W[1], quad_point_data.traction_Aq_W[2] ]) max_traction = max(max_traction, np.linalg.norm(traction)) # Prepare scaling information for the slip velocity vectors. if self.show_slip_velocity_vectors: for quad_point_data in surface.quadrature_point_data: slip_speed = np.array([ quad_point_data.vt_BqAq_W[0], quad_point_data.vt_BqAq_W[1], quad_point_data.vt_BqAq_W[2] ]) max_slip_speed = max(max_slip_speed, np.linalg.norm(slip_speed)) # Compute scaling factors. We don't want division by zero. # We don't want division by negative numbers. if max_force > 0: auto_force_scale = 1.0 / max_force if max_moment > 0: auto_moment_scale = 1.0 / max_moment if max_traction > 0: auto_traction_scale = 1.0 / max_traction if max_slip_speed > 0: auto_slip_velocity_scale = 1.0 / max_slip_speed # TODO(drum) Consider exiting early if no visualization options are # enabled. view = applogic.getCurrentRenderView() for surface in msg.hydroelastic_contacts: contact_data_folder = om.getOrCreateContainer( f'Contact data between {surface.body1_name} and ' f'{surface.body2_name}', folder) # Adds a collection of debug data to the console with the given # item name. def add_contact_data(data, item_name): # Exploit the fact that data.append is a vtkAppendPolyData # instance. The number of input connections on port zero is the # number of *actual* geometries added. If zero have been added, # do no work. if (data is None or data.append.GetNumberOfInputConnections(0) == 0): return item = vis.PolyDataItem(item_name, data.getPolyData(), view) om.addToObjectModel(item, contact_data_folder) item.setProperty('Visible', True) item.setProperty('Alpha', 1.0) item.colorBy('RGB255') # Draw the spatial force. if self.show_spatial_force: force_data = DebugData() point = np.array([ surface.centroid_W[0], surface.centroid_W[1], surface.centroid_W[2] ]) force = np.array([ surface.force_C_W[0], surface.force_C_W[1], surface.force_C_W[2] ]) moment = np.array([ surface.moment_C_W[0], surface.moment_C_W[1], surface.moment_C_W[2] ]) force_mag = np.linalg.norm(force) moment_mag = np.linalg.norm(moment) # Draw the force arrow if it's of sufficient magnitude. if force_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode == ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this force would be # skipped. scale /= force_mag force_data.addArrow(start=point, end=point + auto_force_scale * force * scale, tubeRadius=0.001, headRadius=0.002, color=[1, 0, 0]) # Draw the moment arrow if it's of sufficient magnitude. if moment_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode == ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this moment would be # skipped. scale /= moment_mag force_data.addArrow(start=point, end=point + auto_moment_scale * moment * scale, tubeRadius=0.001, headRadius=0.002, color=[0, 0, 1]) add_contact_data(force_data, "Spatial force") # Iterate over all quadrature points, drawing traction and slip # velocity vectors. if self.show_traction_vectors or self.show_slip_velocity_vectors: traction_data = DebugData() slip_data = DebugData() for quad_point_data in surface.quadrature_point_data: origin = np.array([ quad_point_data.p_WQ[0], quad_point_data.p_WQ[1], quad_point_data.p_WQ[2] ]) if self.show_traction_vectors: traction = np.array([ quad_point_data.traction_Aq_W[0], quad_point_data.traction_Aq_W[1], quad_point_data.traction_Aq_W[2] ]) traction_mag = np.linalg.norm(traction) # Draw the arrow only if it's of sufficient magnitude. if traction_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode ==\ ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this traction # would be skipped. scale /= traction_mag offset = auto_traction_scale * traction * scale traction_data.addArrow(start=origin, end=origin + offset, tubeRadius=0.000125, headRadius=0.00025, color=[1, 0, 1]) else: traction_data.addSphere(center=origin, radius=0.000125, color=[1, 0, 1]) if self.show_slip_velocity_vectors: slip = np.array([ quad_point_data.vt_BqAq_W[0], quad_point_data.vt_BqAq_W[1], quad_point_data.vt_BqAq_W[2] ]) slip_mag = np.linalg.norm(slip) # Draw the arrow only if it's of sufficient magnitude. if slip_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode ==\ ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this slip # vector would be skipped. scale /= slip_mag offset = auto_slip_velocity_scale * slip * scale slip_data.addArrow(start=origin, end=origin + offset, tubeRadius=0.000125, headRadius=0.00025, color=[0, 1, 1]) else: slip_data.addSphere(center=origin, radius=0.000125, color=[0, 1, 1]) add_contact_data(traction_data, "Traction") add_contact_data(slip_data, "Slip velocity") if self.show_pressure or self.show_contact_edges: pos, uvs, tri_mesh, seg_mesh = \ self.process_triangles(surface) if self.show_pressure and len(tri_mesh) > 0: # Copy data to VTK objects. vtk_uvs = vnp.getVtkFromNumpy(uvs) vtk_tris = vtk.vtkCellArray() vtk_tris.Allocate(len(tri_mesh)) for tri in tri_mesh: vtk_tris.InsertNextCell(3, tri) vtk_polydata_tris = vtk.vtkPolyData() vtk_polydata_tris.SetPoints(vnp.getVtkPointsFromNumpy(pos)) vtk_polydata_tris.SetPolys(vtk_tris) vtk_polydata_tris.GetPointData().SetTCoords(vtk_uvs) vtk_mapper = vtk.vtkPolyDataMapper() vtk_mapper.SetInputData(vtk_polydata_tris) # Feed VTK objects into director. item_name = 'Contact surface' polydata_item = vis.PolyDataItem(item_name, vtk_polydata_tris, view) polydata_item.actor.SetMapper(vtk_mapper) polydata_item.actor.SetTexture(self.texture) om.addToObjectModel(polydata_item, contact_data_folder) if self.show_contact_edges and len(seg_mesh) > 0: # Copy data to VTK objects. vtk_segs = vtk.vtkCellArray() vtk_segs.Allocate(len(seg_mesh)) for seg in seg_mesh: vtk_segs.InsertNextCell(2, seg) vtk_polydata_segs = vtk.vtkPolyData() vtk_polydata_segs.SetPoints(vnp.getVtkPointsFromNumpy(pos)) vtk_polydata_segs.SetLines(vtk_segs) vtk_mapper = vtk.vtkPolyDataMapper() vtk_mapper.SetInputData(vtk_polydata_segs) vtk_mapper.Update() # Feed VTK objects into director. item_name = 'Mesh edges' polydata_item = vis.PolyDataItem(item_name, vtk_polydata_segs, view) polydata_item.actor.SetMapper(vtk_mapper) [r, g, b] = color_map.get_contrasting_color() contrasting_color = [r * 255, g * 255, b * 255] polydata_item.actor.GetProperty().SetColor(contrasting_color) om.addToObjectModel(polydata_item, contact_data_folder)
d.addPolyLine(getHelixPoints(), radius=0.01) show(d, (4, 0, 0)) d = DebugData() d.addSphere([0, 0, 0], radius=0.3) show(d, (6, 0, 0)) d = DebugData() d.addFrame(vtk.vtkTransform(), scale=0.5, tubeRadius=0.03) show(d, (0, 2, 0)) d = DebugData() d.addArrow((0, 0, 0), (0, 1, 0)) show(d, (2, 2, 0)) d = DebugData() d.addEllipsoid((0, 0, 0), radii=(0.5, 0.35, 0.2)) show(d, (4, 2, 0)) d = DebugData() d.addTorus(radius=0.5, thickness=0.2) show(d, (6, 2, 0)) d = DebugData() d.addCone(origin=(0,0,0), normal=(0,1,0), radius=0.3, height=0.8, color=[1, 1, 0])
def doFTDraw(self, unusedrobotstate): frames = [] fts = [] vis_names = [] if (hasattr(self.robotStateJointController, 'lastRobotStateMessage') and self.robotStateJointController.lastRobotStateMessage): if self.draw_right: rft = np.array( self.robotStateJointController.lastRobotStateMessage. force_torque.r_hand_force + self.robotStateJointController. lastRobotStateMessage.force_torque.r_hand_torque) rft[0] = -1. * rft[ 0] # right FT looks to be rotated 180deg around the z axis rft[1] = -1. * rft[1] rft[3] = -1. * rft[3] rft[4] = -1. * rft[4] rft -= self.ft_right_bias fts.append(rft) frames.append( self.robotStateModel.getLinkFrame('r_hand_force_torque')) vis_names.append('ft_right') if self.draw_left: lft = np.array( self.robotStateJointController.lastRobotStateMessage. force_torque.l_hand_force + self.robotStateJointController. lastRobotStateMessage.force_torque.l_hand_torque) lft -= self.ft_left_bias fts.append(lft) frames.append( self.robotStateModel.getLinkFrame('l_hand_force_torque')) vis_names.append('ft_left') for i in range(0, len(frames)): frame = frames[i] ft = fts[i] offset = [0., 0., 0.] p1 = frame.TransformPoint(offset) scale = 1.0 / 25.0 # todo add slider for this? scalet = 1.0 / 2.5 p2f = frame.TransformPoint(offset + ft[0:3] * scale) p2t = frame.TransformPoint(offset + ft[3:6]) normalt = (np.array(p2t) - np.array(p1)) normalt = normalt / np.linalg.norm(normalt) d = DebugData() # force if np.linalg.norm(np.array(p2f) - np.array(p1)) < 0.1: d.addLine(p1, p2f, color=[1.0, 0.0, 0.0]) else: d.addArrow(p1, p2f, color=[1., 0., 0.]) # torque if self.draw_torque: d.addCircle(p1, normalt, scalet * np.linalg.norm(ft[3:6])) # frame (largely for debug) vis.updateFrame(frame, vis_names[i] + 'frame', view=self.view, parent='wristft', visible=False, scale=0.2) vis.updatePolyData(d.getPolyData(), vis_names[i], view=self.view, parent='wristft')
def drawShape(self, currShape, next_loc, msg, rotation_matrix=None): ''' Function for drawing shapes. Currently this supports lines, points, and 3D axes currShape: the current shape to be drawn next_loc: the location where to draw the shape msg: the message from the LCM hendler that called this function rotation_matrix: the rotation matrix to be used for the axis shape. Mainly used by the abstract handler ''' # draw a continuous line if (currShape.type == "line"): # check if the duration has been initialized if (currShape.duration == None or len(currShape.points) == 0): currShape.duration = msg.utime / 1000000 # visualize and trace line for 'history' seconds, adding points at a distance at least 10e-5 if (((msg.utime / 1000000) - currShape.duration <= currShape.history) or currShape.history <= 0): # make sure to add at least 2 points before starting to check for the distance between points if (len(currShape.points) < 2): currShape.points.append(next_loc) d = DebugData() d.addPolyLine(currShape.points, radius=currShape.thickness, color=currShape.color) if (currShape.object == None): currShape.object = vis.showPolyData( d.getPolyData(), currShape.name) currShape.object.setProperty('Color', currShape.color) else: currShape.object.setPolyData(d.getPolyData()) else: if (np.linalg.norm( np.array(next_loc) - np.array(currShape.points[-1])) >= 10e-5): currShape.points.append(next_loc) d = DebugData() d.addPolyLine(currShape.points, radius=currShape.thickness, color=currShape.color) if (currShape.object == None): currShape.object = vis.showPolyData( d.getPolyData(), currShape.name) else: currShape.object.setPolyData(d.getPolyData()) elif (currShape.history > 0): if (len(currShape.points) == 0): currShape.duration = msg.utime / 1000000 else: # visualize and trace line for 'history' seconds, adding points at a distance at least 10e-5 if (np.linalg.norm( np.array(next_loc) - np.array(currShape.points[-1])) >= 10e-5): currShape.points.popleft() currShape.points.append(next_loc) d = DebugData() d.addPolyLine(currShape.points, radius=currShape.thickness, color=currShape.color) if (currShape.object == None): currShape.object = vis.showPolyData( d.getPolyData(), currShape.name) else: currShape.object.setPolyData(d.getPolyData()) # draw a point elif (currShape.type == "point"): d = DebugData() d.addSphere(next_loc, radius=currShape.radius) # create a new point if (currShape.created == True): currShape.object = vis.showPolyData(d.getPolyData(), currShape.name) # set color and transparency of point currShape.object.setProperty('Color', currShape.color) currShape.object.setProperty('Alpha', currShape.alpha) currShape.created = False else: # update the location of the last point currShape.object.setPolyData(d.getPolyData()) # draw a set of axes elif (currShape.type == "axes"): # get the rotation matrix rot_matrix = None if (currShape.category != "lcm"): rigTrans = self.plant.EvalBodyPoseInWorld( self.context, self.plant.GetBodyByName(currShape.frame)) rot_matrix = rigTrans.rotation().matrix().transpose() else: rot_matrix = rotation_matrix colors = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] d = DebugData() for i in range(3): d.addArrow(next_loc, next_loc + (rot_matrix[i] * currShape.length), headRadius=0.03, color=colors[i]) # create the 3 axes if (currShape.created == True): currShape.object = vis.showPolyData(d.getPolyData(), currShape.name, colorByName='RGB255') currShape.object.setProperty('Alpha', currShape.alpha) currShape.created = False else: # update the location of the last point currShape.object.setPolyData(d.getPolyData())
def handle_message(self, msg): # Limits the rate of message handling, since redrawing is done in the # message handler. self._sub.setSpeedLimit(30) # Removes the folder completely. om.removeFromObjectModel(om.findObjectByName(self._folder_name)) # Recreates folder. folder = om.getOrCreateContainer(self._folder_name) # Though strangely named, DebugData() is the object through which # drawing is done in DrakeVisualizer. d = DebugData() # Set the color map. color_map = self.create_color_map() # The scale value attributable to auto-scale. auto_force_scale = 1.0 auto_moment_scale = 1.0 auto_traction_scale = 1.0 auto_slip_velocity_scale = 1.0 max_force = -1 max_moment = -1 max_traction = -1 max_slip = -1 # TODO(sean-curtis-TRI) Remove the following comment when this # code can be exercised. # The following code is not exercised presently because the # magnitude mode is always set to kFixedLength. # Determine scaling magnitudes if autoscaling is activated. if self.magnitude_mode == ContactVisModes.kAutoScale: if self.show_spatial_force: for surface in msg.hydroelastic_contacts: force = np.array([surface.force_C_W[0], surface.force_C_W[1], surface.force_C_W[2]]) moment = np.array([surface.moment_C_W[0], surface.moment_C_W[1], surface.moment_C_W[2]]) force_mag = np.linalg.norm(force) moment_mag = np.linalg.norm(moment) if force_mag > max_force: max_force = force_mag if moment_mag > max_moment: max_moment = moment_mag # Prepare scaling information for the traction vectors. if self.show_traction_vectors: for quad_point_data in surface.quadrature_point_data: traction = np.array([quad_point_data.traction_Aq_W[0], quad_point_data.traction_Aq_W[1], quad_point_data.traction_Aq_W[2]]) max_traction = max(max_traction, np.linalg.norm(traction)) # Prepare scaling information for the slip velocity vectors. if self.show_slip_velocity_vectors: for quad_point_data in surface.quadrature_point_data: slip_speed = np.array([quad_point_data.vt_BqAq_W[0], quad_point_data.vt_BqAq_W[1], quad_point_data.vt_BqAq_W[2]]) max_slip_speed = max(max_slip_speed, np.linalg.norm(slip_speed)) # Compute scaling factors. auto_force_scale = 1.0 / max_force auto_moment_scale = 1.0 / max_moment auto_traction_scale = 1.0 / max_traction auto_slip_velocity_scale = 1.0 / max_slip_speed # TODO(drum) Consider exiting early if no visualization options are # enabled. for surface in msg.hydroelastic_contacts: view = applogic.getCurrentRenderView() # Keep track if any DebugData is written to. # Necessary to keep DrakeVisualizer from spewing messages to the # console when no DebugData is sent to director. has_debug_data = False # Draw the spatial force. if self.show_spatial_force: point = np.array([surface.centroid_W[0], surface.centroid_W[1], surface.centroid_W[2]]) force = np.array([surface.force_C_W[0], surface.force_C_W[1], surface.force_C_W[2]]) moment = np.array([surface.moment_C_W[0], surface.moment_C_W[1], surface.moment_C_W[2]]) force_mag = np.linalg.norm(force) moment_mag = np.linalg.norm(moment) # Draw the force arrow if it's of sufficient magnitude. if force_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode == ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this force would be # skipped. scale /= force_mag d.addArrow(start=point, end=point + auto_force_scale * force * scale, tubeRadius=0.005, headRadius=0.01, color=[1, 0, 0]) has_debug_data = True # Draw the moment arrow if it's of sufficient magnitude. if moment_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode == ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this moment would be # skipped. scale /= moment_mag d.addArrow(start=point, end=point + auto_moment_scale * moment * scale, tubeRadius=0.005, headRadius=0.01, color=[0, 0, 1]) has_debug_data = True # Iterate over all quadrature points, drawing traction and slip # velocity vectors. if self.show_traction_vectors or self.show_slip_velocity_vectors: # Arrows and/or spheres are drawn through debug data if there # exists a quadrature point. if surface.num_quadrature_points > 0: has_debug_data = True for quad_point_data in surface.quadrature_point_data: origin = np.array([quad_point_data.p_WQ[0], quad_point_data.p_WQ[1], quad_point_data.p_WQ[2]]) if self.show_traction_vectors: traction = np.array([quad_point_data.traction_Aq_W[0], quad_point_data.traction_Aq_W[1], quad_point_data.traction_Aq_W[2]]) traction_mag = np.linalg.norm(traction) # Draw the arrow only if it's of sufficient magnitude. if traction_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode ==\ ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this traction # would be skipped. scale /= traction_mag offset = auto_traction_scale * traction * scale d.addArrow(start=origin, end=origin + offset, tubeRadius=0.000125, headRadius=0.00025, color=[1, 0, 1]) else: d.addSphere(center=origin, radius=0.000125, color=[1, 0, 1]) if self.show_slip_velocity_vectors: slip = np.array([quad_point_data.vt_BqAq_W[0], quad_point_data.vt_BqAq_W[1], quad_point_data.vt_BqAq_W[2]]) slip_mag = np.linalg.norm(slip) # Draw the arrow only if it's of sufficient magnitude. if slip_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode ==\ ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this slip # vector would be skipped. scale /= slip_mag offset = auto_slip_velocity_scale * slip * scale d.addArrow(start=origin, end=origin + offset, tubeRadius=0.000125, headRadius=0.00025, color=[0, 1, 1]) else: d.addSphere(center=origin, radius=0.000125, color=[0, 1, 1]) # Send everything except pressure and contact edges to director. if has_debug_data: item_name = '{}, {}'.format( surface.body1_name, surface.body2_name) cls = vis.PolyDataItem item = cls(item_name, d.getPolyData(), view) om.addToObjectModel(item, folder) item.setProperty('Visible', True) item.setProperty('Alpha', 1.0) # Coloring for force and moment vectors. item.colorBy('RGB255') if self.show_pressure or self.show_contact_edges: pos, pos_above, pos_below, uvs, tri_mesh, seg_mesh = \ self.process_triangles(surface) if self.show_pressure and len(tri_mesh) > 0: # Copy data to VTK objects. vtk_uvs = vnp.getVtkFromNumpy(uvs) vtk_tris_above = vtk.vtkCellArray() vtk_tris_below = vtk.vtkCellArray() vtk_tris_above.Allocate(len(tri_mesh)) vtk_tris_below.Allocate(len(tri_mesh)) for tri in tri_mesh: vtk_tris_above.InsertNextCell(3, tri) vtk_tris_below.InsertNextCell(3, tri) vtk_polydata_tris_above = vtk.vtkPolyData() vtk_polydata_tris_above.SetPoints( vnp.getVtkPointsFromNumpy(pos_above)) vtk_polydata_tris_above.SetPolys(vtk_tris_above) vtk_polydata_tris_above.GetPointData().SetTCoords(vtk_uvs) vtk_polydata_tris_below = vtk.vtkPolyData() vtk_polydata_tris_below.SetPoints( vnp.getVtkPointsFromNumpy(pos_below)) vtk_polydata_tris_below.SetPolys(vtk_tris_below) vtk_polydata_tris_below.GetPointData().SetTCoords(vtk_uvs) vtk_mapper_above = vtk.vtkPolyDataMapper() vtk_mapper_above.SetInputData(vtk_polydata_tris_above) vtk_mapper_below = vtk.vtkPolyDataMapper() vtk_mapper_below.SetInputData(vtk_polydata_tris_below) # Feed VTK objects into director. item_name = 'Pressure between {}, {}'.format( surface.body1_name, surface.body2_name) polydata_item_above = vis.PolyDataItem( item_name, vtk_polydata_tris_above, view) polydata_item_above.actor.SetMapper(vtk_mapper_above) polydata_item_above.actor.SetTexture(self.texture) om.addToObjectModel(polydata_item_above, folder) item_name = 'Pressure between {}, {}'.format( surface.body1_name, surface.body2_name) polydata_item_below = vis.PolyDataItem( item_name, vtk_polydata_tris_below, view) polydata_item_below.actor.SetMapper(vtk_mapper_below) polydata_item_below.actor.SetTexture(self.texture) om.addToObjectModel(polydata_item_below, folder) if self.show_contact_edges and len(seg_mesh) > 0: # Copy data to VTK objects. vtk_segs = vtk.vtkCellArray() vtk_segs.Allocate(len(seg_mesh)) for seg in seg_mesh: vtk_segs.InsertNextCell(2, seg) vtk_polydata_segs = vtk.vtkPolyData() vtk_polydata_segs.SetPoints( vnp.getVtkPointsFromNumpy(pos)) vtk_polydata_segs.SetLines(vtk_segs) vtk_mapper = vtk.vtkPolyDataMapper() vtk_mapper.SetInputData(vtk_polydata_segs) vtk_mapper.Update() # Feed VTK objects into director. item_name = 'Contact edges between {}, {}'.format( surface.body1_name, surface.body2_name) polydata_item = vis.PolyDataItem( item_name, vtk_polydata_segs, view) polydata_item.actor.SetMapper(vtk_mapper) [r, g, b] = color_map.get_contrasting_color() contrasting_color = [r*255, g*255, b*255] polydata_item.actor.GetProperty().SetColor(contrasting_color) om.addToObjectModel(polydata_item, folder)
def drawFootstepPlan(self, msg, folder, left_color=None, right_color=None, alpha=1.0): for step in folder.children(): om.removeFromObjectModel(step) allTransforms = [] volFolder = getWalkingVolumesFolder() map(om.removeFromObjectModel, volFolder.children()) slicesFolder = getTerrainSlicesFolder() map(om.removeFromObjectModel, slicesFolder.children()) for i, footstep in enumerate(msg.footsteps): trans = footstep.pos.translation trans = [trans.x, trans.y, trans.z] quat = footstep.pos.rotation quat = [quat.w, quat.x, quat.y, quat.z] footstepTransform = transformUtils.transformFromPose(trans, quat) allTransforms.append(footstepTransform) if i < 2: continue if footstep.is_right_foot: mesh = getRightFootMesh() if (right_color is None): color = getRightFootColor() else: color = right_color else: mesh = getLeftFootMesh() if (left_color is None): color = getLeftFootColor() else: color = left_color # add gradual shading to steps to indicate destination frac = float(i)/ float(msg.num_steps-1) this_color = [0,0,0] this_color[0] = 0.25*color[0] + 0.75*frac*color[0] this_color[1] = 0.25*color[1] + 0.75*frac*color[1] this_color[2] = 0.25*color[2] + 0.75*frac*color[2] if self.show_contact_slices: self.drawContactVolumes(footstepTransform, color) contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts() if footstep.is_right_foot: sole_offset = np.mean(contact_pts_right, axis=0) else: sole_offset = np.mean(contact_pts_left, axis=0) t_sole_prev = frameFromPositionMessage(msg.footsteps[i-2].pos) t_sole_prev.PreMultiply() t_sole_prev.Translate(sole_offset) t_sole = transformUtils.copyFrame(footstepTransform) t_sole.Translate(sole_offset) yaw = np.arctan2(t_sole.GetPosition()[1] - t_sole_prev.GetPosition()[1], t_sole.GetPosition()[0] - t_sole_prev.GetPosition()[0]) T_terrain_to_world = transformUtils.frameFromPositionAndRPY([t_sole_prev.GetPosition()[0], t_sole_prev.GetPosition()[1], 0], [0, 0, math.degrees(yaw)]) path_dist = np.array(footstep.terrain_path_dist) height = np.array(footstep.terrain_height) # if np.any(height >= trans[2]): terrain_pts_in_local = np.vstack((path_dist, np.zeros(len(footstep.terrain_path_dist)), height)) d = DebugData() for j in range(terrain_pts_in_local.shape[1]-1): d.addLine(terrain_pts_in_local[:,j], terrain_pts_in_local[:,j+1], radius=0.01) obj = vis.showPolyData(d.getPolyData(), 'terrain slice', parent=slicesFolder, visible=slicesFolder.getProperty('Visible'), color=[.8,.8,.3]) obj.actor.SetUserTransform(T_terrain_to_world) renderInfeasibility = False if renderInfeasibility and footstep.infeasibility > 1e-6: d = DebugData() start = allTransforms[i-1].GetPosition() end = footstepTransform.GetPosition() d.addArrow(start, end, 0.02, 0.005, startHead=True, endHead=True) vis.showPolyData(d.getPolyData(), 'infeasibility %d -> %d' % (i-2, i-1), parent=folder, color=[1, 0.2, 0.2]) stepName = 'step %d' % (i-1) obj = vis.showPolyData(mesh, stepName, color=this_color, alpha=alpha, parent=folder) obj.setIcon(om.Icons.Feet) frameObj = vis.showFrame(footstepTransform, stepName + ' frame', parent=obj, scale=0.3, visible=False) obj.actor.SetUserTransform(footstepTransform) obj.addProperty('Support Contact Groups', footstep.params.support_contact_groups, attributes=om.PropertyAttributes(enumNames=['Whole Foot', 'Front 2/3', 'Back 2/3'])) obj.properties.setPropertyIndex('Support Contact Groups', 0) obj.footstep_index = i obj.footstep_property_callback = obj.properties.connectPropertyChanged(functools.partial(self.onFootstepPropertyChanged, obj)) self.drawContactPts(obj, footstep, color=this_color)
def handle_message(self, msg): # Limits the rate of message handling, since redrawing is done in the # message handler. self._sub.setSpeedLimit(30) # Removes the folder completely. om.removeFromObjectModel(om.findObjectByName(self._folder_name)) # Recreates folder. folder = om.getOrCreateContainer(self._folder_name) # Though strangely named, DebugData() is the object through which # drawing is done in DrakeVisualizer. d = DebugData() # Set the color map. color_map = self.create_color_map() # The scale value attributable to auto-scale. auto_force_scale = 1.0 auto_moment_scale = 1.0 auto_traction_scale = 1.0 auto_slip_velocity_scale = 1.0 max_force = -1 max_moment = -1 max_traction = -1 max_slip = -1 # TODO(sean-curtis-TRI) Remove the following comment when this # code can be exercised. # The following code is not exercised presently because the # magnitude mode is always set to kFixedLength. # Determine scaling magnitudes if autoscaling is activated. if self.magnitude_mode == ContactVisModes.kAutoScale: if self.show_spatial_force: for surface in msg.hydroelastic_contacts: force = np.array([ surface.force_C_W[0], surface.force_C_W[1], surface.force_C_W[2] ]) moment = np.array([ surface.moment_C_W[0], surface.moment_C_W[1], surface.moment_C_W[2] ]) force_mag = np.linalg.norm(force) moment_mag = np.linalg.norm(moment) if force_mag > max_force: max_force = force_mag if moment_mag > max_moment: max_moment = moment_mag # Prepare scaling information for the traction vectors. if self.show_traction_vectors: for quad_point_data in surface.quadrature_point_data: traction = np.array([ quad_point_data.traction_Aq_W[0], quad_point_data.traction_Aq_W[1], quad_point_data.traction_Aq_W[2] ]) max_traction = max(max_traction, np.linalg.norm(traction)) # Prepare scaling information for the slip velocity vectors. if self.show_slip_velocity_vectors: for quad_point_data in surface.quadrature_point_data: slip_speed = np.array([ quad_point_data.vt_BqAq_W[0], quad_point_data.vt_BqAq_W[1], quad_point_data.vt_BqAq_W[2] ]) max_slip_speed = max(max_slip_speed, np.linalg.norm(slip_speed)) # Compute scaling factors. auto_force_scale = 1.0 / max_force auto_moment_scale = 1.0 / max_moment auto_traction_scale = 1.0 / max_traction auto_slip_velocity_scale = 1.0 / max_slip_speed # TODO(drum) Consider exiting early if no visualization options are # enabled. for surface in msg.hydroelastic_contacts: # Draw the spatial force. if self.show_spatial_force: point = np.array([ surface.centroid_W[0], surface.centroid_W[1], surface.centroid_W[2] ]) force = np.array([ surface.force_C_W[0], surface.force_C_W[1], surface.force_C_W[2] ]) moment = np.array([ surface.moment_C_W[0], surface.moment_C_W[1], surface.moment_C_W[2] ]) force_mag = np.linalg.norm(force) moment_mag = np.linalg.norm(moment) # Draw the force arrow if it's of sufficient magnitude. if force_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode == ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this force would be # skipped. scale /= force_mag d.addArrow(start=point, end=point + auto_force_scale * force * scale, tubeRadius=0.005, headRadius=0.01, color=[1, 0, 0]) # Draw the moment arrow if it's of sufficient magnitude. if moment_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode == ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this moment would be # skipped. scale /= moment_mag d.addArrow(start=point, end=point + auto_moment_scale * moment * scale, tubeRadius=0.005, headRadius=0.01, color=[0, 0, 1]) # Iterate over all quadrature points, drawing traction and slip # velocity vectors. if self.show_traction_vectors or self.show_slip_velocity_vectors: for quad_point_data in surface.quadrature_point_data: origin = np.array([ quad_point_data.p_WQ[0], quad_point_data.p_WQ[1], quad_point_data.p_WQ[2] ]) if self.show_traction_vectors: traction = np.array([ quad_point_data.traction_Aq_W[0], quad_point_data.traction_Aq_W[1], quad_point_data.traction_Aq_W[2] ]) traction_mag = np.linalg.norm(traction) # Draw the arrow only if it's of sufficient magnitude. if traction_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode ==\ ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this traction # would be skipped. scale /= traction_mag offset = auto_traction_scale * traction * scale d.addArrow(start=origin, end=origin + offset, tubeRadius=0.000125, headRadius=0.00025, color=[1, 0, 1]) else: d.addSphere(center=origin, radius=0.000125, color=[1, 0, 1]) if self.show_slip_velocity_vectors: slip = np.array([ quad_point_data.vt_BqAq_W[0], quad_point_data.vt_BqAq_W[1], quad_point_data.vt_BqAq_W[2] ]) slip_mag = np.linalg.norm(slip) # Draw the arrow only if it's of sufficient magnitude. if slip_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode ==\ ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this slip # vector would be skipped. scale /= slip_mag offset = auto_slip_velocity_scale * slip * scale d.addArrow(start=origin, end=origin + offset, tubeRadius=0.000125, headRadius=0.00025, color=[0, 1, 1]) else: d.addSphere(center=origin, radius=0.000125, color=[0, 1, 1]) # Iterate over all triangles. for tri in surface.triangles: va = np.array([tri.p_WA[0], tri.p_WA[1], tri.p_WA[2]]) vb = np.array([tri.p_WB[0], tri.p_WB[1], tri.p_WB[2]]) vc = np.array([tri.p_WC[0], tri.p_WC[1], tri.p_WC[2]]) # Save the maximum pressure. self.max_pressure_observed = max(self.max_pressure_observed, tri.pressure_A) self.max_pressure_observed = max(self.max_pressure_observed, tri.pressure_B) self.max_pressure_observed = max(self.max_pressure_observed, tri.pressure_C) # TODO(drum) Vertex color interpolation may be insufficiently # granular if a single triangle spans too large a range of # pressures. Suggested solution is to use a texture map. # Get the colors at the vertices. color_a = color_map.get_color(tri.pressure_A) color_b = color_map.get_color(tri.pressure_B) color_c = color_map.get_color(tri.pressure_C) if self.show_pressure: # TODO(drum) Use a better method for this; the current # approach is susceptible to z-fighting under certain # zoom levels. # Compute a normal to the triangle. We need this normal # because the visualized pressure surface can be coplanar # with parts of the visualized geometry, in which case a # dithering type effect would appear. So we use the normal # to draw two triangles slightly offset to both sides of # the contact surface. # Note that if the area of this triangle is very small, we # won't waste time visualizing it, which also means that # won't have to worry about degenerate triangles). # TODO(edrumwri) Consider allowing the user to set these # next two values programmatically. min_area = 1e-8 offset_scalar = 1e-4 normal = np.cross(vb - va, vc - vb) norm_normal = np.linalg.norm(normal) if norm_normal >= min_area: unit_normal = normal / np.linalg.norm(normal) offset = unit_normal * offset_scalar d.addPolygon([va + offset, vb + offset, vc + offset], color=[color_a, color_b, color_c]) d.addPolygon([va - offset, vb - offset, vc - offset], color=[color_a, color_b, color_c]) # TODO(drum) Consider drawing shared edges just once. if self.show_contact_edges: contrasting_color = color_map.get_contrasting_color() d.addPolyLine(points=(va, vb, vc), isClosed=True, color=contrasting_color) item_name = '{}, {}'.format(surface.body1_name, surface.body2_name) cls = vis.PolyDataItem view = applogic.getCurrentRenderView() item = cls(item_name, d.getPolyData(), view) om.addToObjectModel(item, folder) item.setProperty('Visible', True) item.setProperty('Alpha', 1.0) # Conditional necessary to keep DrakeVisualizer from spewing # messages to the console when the contact surface is empty. if len(msg.hydroelastic_contacts) > 0: item.colorBy('RGB255')
def handle_message(self, msg): # Limits the rate of message handling, since redrawing is done in the # message handler. self._sub.setSpeedLimit(30) # Removes the folder completely. om.removeFromObjectModel(om.findObjectByName(self._folder_name)) # Recreates folder. folder = om.getOrCreateContainer(self._folder_name) # The scale value attributable to auto-scale. auto_scale = 1.0 max_force = -1 # A map from pair of body names to a list of contact forces collision_pair_to_forces = {} for contact in msg.point_pair_contact_info: point = np.array([ contact.contact_point[0], contact.contact_point[1], contact.contact_point[2] ]) force = np.array([ contact.contact_force[0], contact.contact_force[1], contact.contact_force[2] ]) mag = np.linalg.norm(force) if mag < self.min_magnitude: continue if mag > max_force: max_force = mag scale = self.global_scale if self.magnitude_mode == ContactVisModes.kFixedLength: # mag must be > 0 otherwise this force would be skipped. scale /= mag vis_force = force * scale # In point_pair_contact_info, the force is defined as the force # *on body B*. That is what's placed in # lcmt_point_pair_contact_info_for_viz. # However, for two bodies (1, 2), MBP provides no guarantees which # body is "body1" and which is "body2". Some contacts could be # reported as (1, 2) and some as (2, 1). In order to aggregate all # such interactions into a single debug artifact, we need to # reconcile the ordering. force_format = "Force on {} from {}" key = force_format.format(contact.body2_name, contact.body1_name) alt_key = force_format.format(contact.body1_name, contact.body2_name) if key in collision_pair_to_forces: collision_pair_to_forces[key].append((point, vis_force)) elif alt_key in collision_pair_to_forces: collision_pair_to_forces[alt_key].append((point, -vis_force)) else: collision_pair_to_forces[key] = [(point, vis_force)] if self.magnitude_mode == ContactVisModes.kAutoScale: auto_scale = 1.0 / max_force for key, list_of_forces in collision_pair_to_forces.items(): d = DebugData() for p, v in list_of_forces: d.addArrow(start=p, end=p + auto_scale * v, tubeRadius=0.005, headRadius=0.01) vis.showPolyData(d.getPolyData(), str(key), parent=folder, color=[0.2, 0.8, 0.2]) self.update_screen_text()