def get_arrow_text_markers(p, f, frame, m_id, duration, c=(1.,0.,0.,1.0)): t_now = rospy.Time.now() q = hv.arrow_direction_to_quat(f) arrow_len = np.linalg.norm(f) * 0.04 #arrow_len = np.linalg.norm(f) * 0.007 f_unit = f/np.linalg.norm(f) #if 'electric' not in roslib.__path__[0]: # # diamondback # scale = (arrow_len, 0.1, 0.1) #else: # electric scale = (0.20, 0.20, arrow_len) m1 = hv.single_marker(p, q, 'arrow', frame, scale, c, m_id = m_id, duration = duration) m1.header.stamp = t_now m2 = hv.single_marker(p + f_unit * arrow_len * 1.6, q, 'text_view_facing', frame, #m2 = hv.single_marker(p, q, 'text_view_facing', frame, (0.07, 0.07, 0.07), m_id = m_id+1, duration = duration, color=c) m2.text = '%.1fN'%(np.linalg.norm(f)) m2.header.stamp = t_now return m1, m2
def vel_vec_rviz_marker(p, v): quat = hv.arrow_direction_to_quat(v) am = hv.single_marker(p, quat, 'arrow', 'torso_lift_link', m_id=1, duration=0.1) return am
def get_arrow_text_markers(p, f, frame, m_id, duration): t_now = rospy.Time.now() q = hv.arrow_direction_to_quat(f) arrow_len = np.linalg.norm(f) * 0.04 scale = (arrow_len, 0.2, 0.2) m1 = hv.single_marker(p, q, 'arrow', frame, scale, m_id = m_id, duration = duration) m1.header.stamp = t_now m2 = hv.single_marker(p, q, 'text_view_facing', frame, (0.1, 0.1, 0.1), m_id = m_id+1, duration = duration, color=(1.,0.,0.,1.)) m2.text = '%.1f N'%(np.linalg.norm(f)) m2.header.stamp = t_now return m1, m2
def get_arrow_text_markers(p, f, frame, m_id, duration, c=(1., 0., 0., 1.0)): t_now = rospy.Time.now() q = hv.arrow_direction_to_quat(f) arrow_len = np.linalg.norm(f) * 0.04 #arrow_len = np.linalg.norm(f) * 0.007 f_unit = f / np.linalg.norm(f) #if 'electric' not in roslib.__path__[0]: # # diamondback # scale = (arrow_len, 0.1, 0.1) #else: # electric scale = (0.20, 0.20, arrow_len) m1 = hv.single_marker(p, q, 'arrow', frame, scale, c, m_id=m_id, duration=duration) m1.header.stamp = t_now m2 = hv.single_marker( p + f_unit * arrow_len * 1.6, q, 'text_view_facing', frame, #m2 = hv.single_marker(p, q, 'text_view_facing', frame, (0.07, 0.07, 0.07), m_id=m_id + 1, duration=duration, color=c) m2.text = '%.1fN' % (np.linalg.norm(f)) m2.header.stamp = t_now return m1, m2
def visualize_taxel_array(ta, marker_pub): markers = MarkerArray() stamp = ta.header.stamp frame = ta.header.frame_id pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z)) colors = np.zeros((4, pts.shape[0])) colors[0,:] = 243/255.0 colors[1,:] = 132/255.0 colors[2,:] = 0. colors[3,:] = 1.0 scale = (0.005, 0.005, 0.005) duration = 0. m = hv.list_marker(pts.T, colors, scale, 'points', frame, duration=duration, m_id=0) m.header.stamp = stamp markers.markers.append(m) # now draw non-zero forces as arrows. nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z)) fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z)) fmags = ut.norm(fs.T).flatten() if hasattr(ta, 'link_name'): idxs = np.where(fmags > 0.0)[0] #was .01 else: # HACK. need to calibrate the skin patch so that something # reasonable gets outputted. idxs = np.where(fmags > 0.)[0] #was 0.2 #force_marker_scale = 0.04 force_marker_scale = 1.0 duration = 0.4 for i in idxs: p = np.matrix(pts[i]).T n1 = np.matrix(nrmls[i]).T n2 = np.matrix(fs[i]).T if False: q1 = hv.arrow_direction_to_quat(n1) l1 = (n2.T * n1)[0,0] * force_marker_scale if 'electric' not in roslib.__path__[0]: scale = (l1, 0.2, 0.2) else: scale = (0.2, 0.2, 0.2) #something weird here, was (0.2, 0.2, l1) scale = (0.4, 0.4, l1) m = hv.single_marker(p, q1, 'arrow', frame, duration=duration, scale=scale, m_id=3*i+1) m.header.stamp = stamp #markers.markers.append(m) if True: if np.linalg.norm(n2) < 0.30: q2 = hv.arrow_direction_to_quat(n2) l2 = np.linalg.norm(n2) * force_marker_scale m = Marker() m.points.append(Point(p[0,0], p[1,0], p[2,0])) m.points.append(Point(p[0,0]+n2[0,0], p[1,0]+n2[1,0], p[2,0]+n2[2,0])) m.scale = Point(0.02, 0.04, 0.0) m.header.frame_id = frame m.id = 3*i+2 m.type = Marker.ARROW m.action = Marker.ADD m.color.r = 0. m.color.g = 0.8 m.color.b = 0. m.color.a = 1. m.lifetime = rospy.Duration(duration) m.header.stamp = stamp markers.markers.append(m) m = hv.single_marker(p + n2/np.linalg.norm(n2) * l2 * 1.2, q2, 'text_view_facing', frame, (0.07, 0.07, 0.07), m_id = 3*i+3, duration = duration, color=(0.5,0.5,0.5,1.)) m.text = '%.2fm'%(np.linalg.norm(n2)) m.header.stamp = stamp markers.markers.append(m) marker_pub.publish(markers)
def visualize_taxel_array(ta, marker_pub): markers = MarkerArray() stamp = ta.header.stamp frame = ta.header.frame_id pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z)) colors = np.zeros((4, pts.shape[0])) colors[0, :] = 243 / 255.0 colors[1, :] = 132 / 255.0 colors[2, :] = 0. colors[3, :] = 1.0 scale = (0.005, 0.005, 0.005) duration = 0. m = hv.list_marker(pts.T, colors, scale, 'points', frame, duration=duration, m_id=0) m.header.stamp = stamp markers.markers.append(m) # now draw non-zero forces as arrows. nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z)) fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z)) fmags = ut.norm(fs.T).flatten() if hasattr(ta, 'link_name'): idxs = np.where(fmags > 0.01)[0] else: # HACK. need to calibrate the skin patch so that something # reasonable gets outputted. idxs = np.where(fmags > 0.2)[0] force_marker_scale = 0.04 duration = 0.02 for i in idxs: p = np.matrix(pts[i]).T n1 = np.matrix(nrmls[i]).T n2 = np.matrix(fs[i]).T q1 = hv.arrow_direction_to_quat(n1) l1 = (n2.T * n1)[0, 0] * force_marker_scale #if 'electric' not in roslib.__path__[0]: # scale = (l1, 0.2, 0.2) #else: scale = (0.2, 0.2, l1) m = hv.single_marker(p, q1, 'arrow', frame, duration=duration, scale=scale, m_id=3 * i + 1) m.header.stamp = stamp markers.markers.append(m) q2 = hv.arrow_direction_to_quat(n2) l2 = np.linalg.norm(n2) * force_marker_scale #if 'electric' not in roslib.__path__[0]: # scale = (l2, 0.2, 0.2) #else: scale = (0.2, 0.2, l2) m = hv.single_marker(p, q2, 'arrow', frame, duration=duration, scale=scale, color=(0., 0.5, 0., 1.0), m_id=3 * i + 2) m.header.stamp = stamp markers.markers.append(m) m = hv.single_marker(p + n2 / np.linalg.norm(n2) * l2 * 1.6, q2, 'text_view_facing', frame, (0.07, 0.07, 0.07), m_id=3 * i + 3, duration=duration, color=(0., 0.5, 0., 1.)) m.text = '%.1fN' % (np.linalg.norm(n2)) m.header.stamp = stamp markers.markers.append(m) marker_pub.publish(markers)
def visualize_taxel_array(ta, marker_pub): markers = MarkerArray() stamp = ta.header.stamp frame = ta.header.frame_id pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z)) colors = np.zeros((4, pts.shape[0])) colors[0,:] = 243/255.0 colors[1,:] = 132/255.0 colors[2,:] = 0. colors[3,:] = 1.0 scale = (0.005, 0.005, 0.005) duration = 0. m = hv.list_marker(pts.T, colors, scale, 'points', frame, duration=duration, m_id=0) m.header.stamp = stamp markers.markers.append(m) # now draw non-zero forces as arrows. nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z)) fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z)) fmags = ut.norm(fs.T).flatten() if hasattr(ta, 'link_name'): idxs = np.where(fmags > 0.01)[0] else: # HACK. need to calibrate the skin patch so that something # reasonable gets outputted. idxs = np.where(fmags > 0.2)[0] force_marker_scale = 0.04 duration = 0.02 for i in idxs: p = np.matrix(pts[i]).T n1 = np.matrix(nrmls[i]).T n2 = np.matrix(fs[i]).T q1 = hv.arrow_direction_to_quat(n1) l1 = (n2.T * n1)[0,0] * force_marker_scale #if 'electric' not in roslib.__path__[0]: # scale = (l1, 0.2, 0.2) #else: scale = (0.2, 0.2, l1) m = hv.single_marker(p, q1, 'arrow', frame, duration=duration, scale=scale, m_id=3*i+1) m.header.stamp = stamp markers.markers.append(m) q2 = hv.arrow_direction_to_quat(n2) l2 = np.linalg.norm(n2) * force_marker_scale #if 'electric' not in roslib.__path__[0]: # scale = (l2, 0.2, 0.2) #else: scale = (0.2, 0.2, l2) m = hv.single_marker(p, q2, 'arrow', frame, duration=duration, scale=scale, color=(0.,0.5,0.,1.0), m_id=3*i+2) m.header.stamp = stamp markers.markers.append(m) m = hv.single_marker(p + n2/np.linalg.norm(n2) * l2 * 1.6, q2, 'text_view_facing', frame, (0.07, 0.07, 0.07), m_id = 3*i+3, duration = duration, color=(0.,0.5,0.,1.)) m.text = '%.1fN'%(np.linalg.norm(n2)) m.header.stamp = stamp markers.markers.append(m) marker_pub.publish(markers)
def visualize_taxel_array(ta, marker_pub): markers = MarkerArray() stamp = ta.header.stamp frame = ta.header.frame_id pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z)) colors = np.zeros((4, pts.shape[0])) colors[0, :] = 243 / 255.0 colors[1, :] = 132 / 255.0 colors[2, :] = 0. colors[3, :] = 1.0 scale = (0.005, 0.005, 0.005) duration = 0. m = hv.list_marker(pts.T, colors, scale, 'points', frame, duration=duration, m_id=0) m.header.stamp = stamp markers.markers.append(m) # now draw non-zero forces as arrows. nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z)) fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z)) fmags = ut.norm(fs.T).flatten() if hasattr(ta, 'link_name'): idxs = np.where(fmags > 0.0)[0] #was .01 else: # HACK. need to calibrate the skin patch so that something # reasonable gets outputted. idxs = np.where(fmags > 0.)[0] #was 0.2 #force_marker_scale = 0.04 force_marker_scale = 1.0 duration = 0.4 for i in idxs: p = np.matrix(pts[i]).T n1 = np.matrix(nrmls[i]).T n2 = np.matrix(fs[i]).T if False: q1 = hv.arrow_direction_to_quat(n1) l1 = (n2.T * n1)[0, 0] * force_marker_scale if 'electric' not in roslib.__path__[0]: scale = (l1, 0.2, 0.2) else: scale = (0.2, 0.2, 0.2 ) #something weird here, was (0.2, 0.2, l1) scale = (0.4, 0.4, l1) m = hv.single_marker(p, q1, 'arrow', frame, duration=duration, scale=scale, m_id=3 * i + 1) m.header.stamp = stamp #markers.markers.append(m) if True: if np.linalg.norm(n2) < 0.30: q2 = hv.arrow_direction_to_quat(n2) l2 = np.linalg.norm(n2) * force_marker_scale m = Marker() m.points.append(Point(p[0, 0], p[1, 0], p[2, 0])) m.points.append( Point(p[0, 0] + n2[0, 0], p[1, 0] + n2[1, 0], p[2, 0] + n2[2, 0])) m.scale = Point(0.02, 0.04, 0.0) m.header.frame_id = frame m.id = 3 * i + 2 m.type = Marker.ARROW m.action = Marker.ADD m.color.r = 0. m.color.g = 0.8 m.color.b = 0. m.color.a = 1. m.lifetime = rospy.Duration(duration) m.header.stamp = stamp markers.markers.append(m) m = hv.single_marker(p + n2 / np.linalg.norm(n2) * l2 * 1.2, q2, 'text_view_facing', frame, (0.07, 0.07, 0.07), m_id=3 * i + 3, duration=duration, color=(0.5, 0.5, 0.5, 1.)) m.text = '%.2fm' % (np.linalg.norm(n2)) m.header.stamp = stamp markers.markers.append(m) marker_pub.publish(markers)