class SpheroidViz: def __init__(self): rot = np.mat([[1, 0, 0], [0, 1. / np.sqrt(2), -1. / np.sqrt(2)], [0, 1. / np.sqrt(2), 1. / np.sqrt(2)]]) self.sspace = SpheroidSpace(0.2) #, np.mat([1.0, 0.5, 0.5]).T, rot) self.colors = [ColorRGBA(1., 0., 0., 1.), ColorRGBA(0., 1., 0., 1.)] self.vis_pub = rospy.Publisher("force_torque_markers_array", MarkerArray) self.pose_pub = rospy.Publisher("spheroid_poses", PoseStamped) m = Marker() m.header.frame_id = "torso_lift_link" m.header.stamp = rospy.Time() m.ns = "force_torque" m.id = 0 m.type = Marker.ARROW m.action = Marker.ADD #m.points.append(Point(0, 0, 0)) m.scale = Vector3(0.01, 0.01, 0.01) m.color = self.colors[0] #self.vis_pub.publish(m) self.m = m self.ma = MarkerArray() def publish_vector(self, m_id): new_m = copy.deepcopy(self.m) new_m.id = m_id u, v, p = 1, np.random.uniform(0, np.pi), np.random.uniform(0, 2 * np.pi) pos = self.sspace.spheroidal_to_cart((u, v, p)) new_m.points.append(Point(*pos)) df_du = self.sspace.partial_u((u, v, p)) df_du *= 0.1 / np.linalg.norm(df_du) new_m.points.append(Point(*(pos + df_du))) self.ma.markers.append(new_m) self.vis_pub.publish(self.ma) rot_gripper = np.pi / 4. nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du) j = np.sqrt(1. / (1. + ny * ny / (nz * nz))) k = -ny * j / nz norm_rot = np.mat([[-nx, ny * k - nz * j, 0], [-ny, -nx * k, j], [-nz, nx * j, k]]) _, norm_quat = PoseConverter.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot) rot_angle = np.arctan(-norm_rot[2, 1] / norm_rot[2, 2]) quat_ortho_rot = tf_trans.quaternion_from_euler( rot_angle + np.pi + rot_gripper, 0.0, 0.0) norm_quat_ortho = tf_trans.quaternion_multiply(norm_quat, quat_ortho_rot) ps_msg = PoseConverter.to_pose_stamped_msg("torso_lift_link", pos, norm_quat_ortho) self.pose_pub.publish(ps_msg)
class SpheroidViz: def __init__(self): rot = np.mat([[1, 0, 0], [0, 1./np.sqrt(2), -1./np.sqrt(2)], [0, 1./np.sqrt(2), 1./np.sqrt(2)]]) self.sspace = SpheroidSpace(0.2)#, np.mat([1.0, 0.5, 0.5]).T, rot) self.colors = [ColorRGBA(1., 0., 0., 1.), ColorRGBA(0., 1., 0., 1.)] self.vis_pub = rospy.Publisher("force_torque_markers_array", MarkerArray) self.pose_pub = rospy.Publisher("spheroid_poses", PoseStamped) m = Marker() m.header.frame_id = "torso_lift_link" m.header.stamp = rospy.Time() m.ns = "force_torque" m.id = 0 m.type = Marker.ARROW m.action = Marker.ADD #m.points.append(Point(0, 0, 0)) m.scale = Vector3(0.01, 0.01, 0.01) m.color = self.colors[0] #self.vis_pub.publish(m) self.m = m self.ma = MarkerArray() def publish_vector(self, m_id): new_m = copy.deepcopy(self.m) new_m.id = m_id u, v, p = 1, np.random.uniform(0, np.pi), np.random.uniform(0, 2 * np.pi) pos = self.sspace.spheroidal_to_cart((u, v, p)) new_m.points.append(Point(*pos)) df_du = self.sspace.partial_u((u, v, p)) df_du *= 0.1 / np.linalg.norm(df_du) new_m.points.append(Point(*(pos+df_du))) self.ma.markers.append(new_m) self.vis_pub.publish(self.ma) rot_gripper = np.pi/4. nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du) j = np.sqrt(1./(1.+ny*ny/(nz*nz))) k = -ny*j/nz norm_rot = np.mat([[-nx, ny*k - nz*j, 0], [-ny, -nx*k, j], [-nz, nx*j, k]]) _, norm_quat = PoseConverter.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot) rot_angle = np.arctan(-norm_rot[2,1] / norm_rot[2,2]) quat_ortho_rot = tf_trans.quaternion_from_euler(rot_angle + np.pi + rot_gripper, 0.0, 0.0) norm_quat_ortho = tf_trans.quaternion_multiply(norm_quat, quat_ortho_rot) ps_msg = PoseConverter.to_pose_stamped_msg("torso_lift_link", pos, norm_quat_ortho) self.pose_pub.publish(ps_msg)
class SpheroidViz: def __init__(self): ellipse_rot = np.mat([[-1., 0., 0.], [0., -1., 0.], [0., 0., 1.]]) self.sspace = SpheroidSpace(0.15, np.mat([0.78, -0.18, 0.1]).T, ellipse_rot) self.colors = [ColorRGBA(1., 0., 0., 1.), ColorRGBA(0., 1., 0., 1.)] self.vis_pub = rospy.Publisher("/force_torque_markers_array", MarkerArray) self.pose_pub = rospy.Publisher("/spheroid_poses", PoseStamped) m = Marker() m.header.frame_id = "torso_lift_link" m.header.stamp = rospy.Time() m.ns = "force_torque" m.id = 0 m.type = Marker.ARROW m.action = Marker.ADD #m.points.append(Point(0, 0, 0)) m.scale = Vector3(0.01, 0.01, 0.01) m.color = self.colors[0] #self.vis_pub.publish(m) self.m = m self.ma = MarkerArray() def publish_vector(self, m_id): new_m = copy.deepcopy(self.m) new_m.id = m_id u, v, p = 1.0, np.random.uniform(0, np.pi), np.random.uniform( 0, 2 * np.pi) pos = self.sspace.spheroidal_to_cart((u, v, p)) new_m.points.append(Point(*pos)) df_du = self.sspace.partial_u((u, v, p)) df_du *= 0.1 / np.linalg.norm(df_du) new_m.points.append(Point(*(pos + df_du))) self.ma.markers.append(new_m) self.vis_pub.publish(self.ma) rot_gripper = np.pi / 4. pos, rot = self.sspace.spheroidal_to_pose((u, v, p), rot_gripper) ps_msg = PoseConverter.to_pose_stamped_msg("torso_lift_link", pos, rot) self.pose_pub.publish(ps_msg)
class SpheroidViz: def __init__(self): ellipse_rot = np.mat([[-1., 0., 0.], [0., -1., 0.], [0., 0., 1.]]) self.sspace = SpheroidSpace(0.15, np.mat([0.78, -0.18, 0.1]).T, ellipse_rot) self.colors = [ColorRGBA(1., 0., 0., 1.), ColorRGBA(0., 1., 0., 1.)] self.vis_pub = rospy.Publisher("/force_torque_markers_array", MarkerArray) self.pose_pub = rospy.Publisher("/spheroid_poses", PoseStamped) m = Marker() m.header.frame_id = "torso_lift_link" m.header.stamp = rospy.Time() m.ns = "force_torque" m.id = 0 m.type = Marker.ARROW m.action = Marker.ADD #m.points.append(Point(0, 0, 0)) m.scale = Vector3(0.01, 0.01, 0.01) m.color = self.colors[0] #self.vis_pub.publish(m) self.m = m self.ma = MarkerArray() def publish_vector(self, m_id): new_m = copy.deepcopy(self.m) new_m.id = m_id u, v, p = 1.0, np.random.uniform(0, np.pi), np.random.uniform(0, 2 * np.pi) pos = self.sspace.spheroidal_to_cart((u, v, p)) new_m.points.append(Point(*pos)) df_du = self.sspace.partial_u((u, v, p)) df_du *= 0.1 / np.linalg.norm(df_du) new_m.points.append(Point(*(pos+df_du))) self.ma.markers.append(new_m) self.vis_pub.publish(self.ma) rot_gripper = np.pi/4. pos, rot = self.sspace.spheroidal_to_pose((u,v,p), rot_gripper) ps_msg = PoseConverter.to_pose_stamped_msg("torso_lift_link", pos, rot) self.pose_pub.publish(ps_msg)