def test_from_two_vecs(self): v1 = Vector3(100, 0, 0) v2 = Vector3(1, 1, 0) qt = Quaternion.from_two_vectors(v1, v2) angle = qt.angular_distance(quaternion=Quaternion()) self.assertAlmostEqual(angle * 180.0 / math.pi, 45.0) v3 = v1 * qt v3.normalize() v2.normalize() self.assertTrue(numpy.isclose(v2.data(), v3.data()).all())
def test_euler(self): angles = (0.1, 0.2, 0.3) qt_ex = Quaternion.from_euler_extrinsic(*angles) qt_in = Quaternion.from_euler_intrinsic( angles=reversed(angles), ordering=[Axis.AXIS_Z, Axis.AXIS_Y, Axis.AXIS_X]) self.assertTrue(numpy.isclose(qt_ex.coeffs(), qt_in.coeffs()).all()) euler = qt_ex.to_euler() for i in range(3): self.assertAlmostEqual(angles[i], euler[i])
def from_msg(cls, ros_msg): # type: (genpy.Message) -> Transform if isinstance(ros_msg, RosTransform): return Transform(translation=Vector3.from_msg(ros_msg.translation), rotation=Quaternion.from_msg(ros_msg.rotation)) elif isinstance(ros_msg, RosPose): return Transform(translation=Vector3.from_msg(ros_msg.position), rotation=Quaternion.from_msg(ros_msg.orientation)) else: raise NotImplementedError( 'Unknown message type to convert to math6D.Transform: {}'. format(type(ros_msg)))
def test_transform_matrix(self): t = Transform(Vector3(1, 2, 3), Quaternion.from_euler_extrinsic(0.1, 0.2, 0.3)) m = t.matrix() t2 = Transform.from_matrix(m) m2 = t2.matrix() self.assertTrue(numpy.isclose(m, m2).all())
def test_from_xy(self): random_qt = Quaternion.from_euler_extrinsic(0.2, 0.3, 0.9) x_vec = Vector3(1, 0, 0) * random_qt y_vec = Vector3(0, 1, 0) * random_qt z_vec = Vector3(0, 0, 1) * random_qt m = numpy.identity(3) m[:, 0] = x_vec.data() m[:, 1] = y_vec.data() m[:, 2] = z_vec.data() m = numpy.matrix(m) new_q = Quaternion.from_matrix(m) self.assertTrue( numpy.isclose(new_q.coeffs(), random_qt.coeffs()).all())
def from_matrix(cls, matrix): # type: (numpy.matrix) -> Transform # Check matrix properties if matrix.shape != (4, 4): raise ValueError( 'Invalid matrix shape: Input must be a 4x4 matrix') if not numpy.isclose(numpy.linalg.det(matrix), 1.0): raise ValueError('Matrix determinant must be +1.0') q = Quaternion.from_matrix(matrix[0:3, 0:3]) t = Vector3(matrix[0:3, 3].flatten()) return Transform(t, q)
def __init__(self, *args, **kwargs): self.__rotation = None self.__translation = None if len(args) == 0 and len(kwargs) == 0: self.__rotation = Quaternion() self.__translation = Vector3() elif len(args) == 1: _arg = args[0] if isinstance(_arg, genpy.Message): t = Transform.from_msg(_arg) self.__rotation = t.rotation self.__translation = t.translation else: raise NotImplementedError( 'Expected Message type, got: {}'.format(type(_arg))) elif len(args) + len(kwargs) == 2: if len(args) == 2: _tr = args[0] _rot = args[1] else: _tr = kwargs[ self. TRANSLATION_KW] if self.TRANSLATION_KW in kwargs else Vector3( ) _rot = kwargs[ self. ROTATION_KW] if self.ROTATION_KW in kwargs else Quaternion( ) if isinstance(_tr, RosVector3): self.__translation = Vector3().from_msg(_tr) elif isinstance(_tr, Vector3): self.__translation = _tr else: self.__translation = Vector3(_tr) if isinstance(_rot, RosQuaternion): self.__rotation = Quaternion().from_msg(_rot) elif isinstance(_rot, Quaternion): self.__rotation = _rot else: self.__rotation = Quaternion(_rot) else: raise NotImplementedError( 'Unexpected input args of len: {}'.format( len(args) + len(kwargs)))
class Transform(MsgConversion): """ Isometric Transform """ TRANSLATION_KW = 'translation' ROTATION_KW = 'rotation' def __init__(self, *args, **kwargs): self.__rotation = None self.__translation = None if len(args) == 0 and len(kwargs) == 0: self.__rotation = Quaternion() self.__translation = Vector3() elif len(args) == 1: _arg = args[0] if isinstance(_arg, genpy.Message): t = Transform.from_msg(_arg) self.__rotation = t.rotation self.__translation = t.translation else: raise NotImplementedError( 'Expected Message type, got: {}'.format(type(_arg))) elif len(args) + len(kwargs) == 2: if len(args) == 2: _tr = args[0] _rot = args[1] else: _tr = kwargs[ self. TRANSLATION_KW] if self.TRANSLATION_KW in kwargs else Vector3( ) _rot = kwargs[ self. ROTATION_KW] if self.ROTATION_KW in kwargs else Quaternion( ) if isinstance(_tr, RosVector3): self.__translation = Vector3().from_msg(_tr) elif isinstance(_tr, Vector3): self.__translation = _tr else: self.__translation = Vector3(_tr) if isinstance(_rot, RosQuaternion): self.__rotation = Quaternion().from_msg(_rot) elif isinstance(_rot, Quaternion): self.__rotation = _rot else: self.__rotation = Quaternion(_rot) else: raise NotImplementedError( 'Unexpected input args of len: {}'.format( len(args) + len(kwargs))) # # Properties # @property def translation(self): # type: () -> Vector3 return self.__translation @property def rotation(self): # type: () -> Quaternion return self.__rotation @translation.setter def translation(self, translation): # type: (Vector3) -> None self.__translation = translation @rotation.setter def rotation(self, rotation): # type: (Quaternion) -> None self.__rotation = rotation # # Transform Methods # def invert(self): self.__rotation.invert() self.__translation = -(self.__rotation * self.__translation) def inverse(self): # type: () -> Transform t = copy.deepcopy(self) t.invert() return t def matrix(self): # type: () -> numpy.matrix m = numpy.identity(4, dtype=numpy.float64) m[:3, :3] = self.rotation.matrix() m[:3, 3] = self.translation.data() return numpy.asmatrix(m) def dist_squared(self, other): """Return the square of the metric distance, as the unweighted sum of linear and angular distance, to the 'other' transform. Note that the units and scale among linear and angular representations matters heavily. """ return self.translation.dist_squared( other.translation) + self.rotation.angular_distance( other.rotation)**2 def dist(self, other): """Return the metric distance, as unweighted combined linear and angular distance, to the 'other' transform. Note that the units and scale among linear and angular representations matters heavily. """ return numpy.sqrt(self.dist_squared(other)) # # Static Methods # @classmethod def identity(cls): # type: () -> Transform return Transform() @classmethod def from_matrix(cls, matrix): # type: (numpy.matrix) -> Transform # Check matrix properties if matrix.shape != (4, 4): raise ValueError( 'Invalid matrix shape: Input must be a 4x4 matrix') if not numpy.isclose(numpy.linalg.det(matrix), 1.0): raise ValueError('Matrix determinant must be +1.0') q = Quaternion.from_matrix(matrix[0:3, 0:3]) t = Vector3(matrix[0:3, 3].flatten()) return Transform(t, q) # # Operators # def __mul__(self, other): if isinstance(other, Transform): t = self.translation + (self.rotation * other.translation) q = self.rotation * other.rotation return Transform(t, q) elif isinstance(other, Vector3): v = numpy.ones(4) v[:3] = other.data() return Vector3(numpy.dot(self.matrix(), v)[:3]) else: raise NotImplementedError('Cannot multiply type of: {}'.format( type(other))) # # Printing # def __repr__(self): return 'T[tran={}, rot={}]'.format(self.translation, self.rotation) def __str__(self): return self.__repr__() # # ROS compatibility # @classmethod def from_msg(cls, ros_msg): # type: (genpy.Message) -> Transform if isinstance(ros_msg, RosTransform): return Transform(translation=Vector3.from_msg(ros_msg.translation), rotation=Quaternion.from_msg(ros_msg.rotation)) elif isinstance(ros_msg, RosPose): return Transform(translation=Vector3.from_msg(ros_msg.position), rotation=Quaternion.from_msg(ros_msg.orientation)) else: raise NotImplementedError( 'Unknown message type to convert to math6D.Transform: {}'. format(type(ros_msg))) def to_msg(self, message_type=RosTransform): # type: (type) -> typing.Union[RosTransform, RosPose] if message_type == RosTransform: return RosTransform( translation=self.__translation.to_msg(RosVector3), rotation=self.__rotation.to_msg(RosQuaternion)) elif message_type == RosPose: return RosPose(position=self.__translation.to_msg(RosPoint), orientation=self.__rotation.to_msg(RosQuaternion)) else: raise NotImplementedError( 'Unknown target msg type: {}'.format(message_type))
def test_quaternion_inverse(self): qt = Quaternion(1, 0, 0, 0) qt_inv = qt.inverse() self.assertTrue(numpy.isclose(qt.coeffs(), qt_inv.coeffs()).all())
def generate_cartographer_map(cartographer_configuration_directory, world_sdf_path, resolution=0.02, map_origin=(-30, -10), map_size=(64, 64), submap_locations=((30, 22, 64, 32), (46, 22, 64, 32))): assert os.path.isdir(cartographer_configuration_directory) assert os.path.isfile(world_sdf_path) temp_png_f = tempfile.NamedTemporaryFile() temp_pb_f = tempfile.NamedTemporaryFile() cmd = [ 'rosrun', 'cartographer_ros', 'sdf_to_pbstream', '--configuration_directory', cartographer_configuration_directory, '--pbstream', temp_pb_f.name, '--map_png', temp_png_f.name, '--world_sdf', world_sdf_path, '--map_origin', # bottom left corner position '{},{}'.format(*map_origin), '--map_size', '{},{}'.format(*map_size) ] for s in submap_locations: cmd += ['--submap_location', ','.join([str(_s) for _s in s])] str_cmd = ' '.join(cmd) subprocess.check_call(str_cmd, shell=True, stderr=subprocess.STDOUT) temp_pb_f.seek(0) pb_bytes = temp_pb_f.read() temp_png_f.seek(0) im_bytes = temp_png_f.read() xml_file = open(world_sdf_path, 'r') sdf_xml = xml_file.read() parser = etree.XMLParser(remove_blank_text=True) original_xml_element = etree.XML(sdf_xml, parser=parser) world = original_xml_element.find('world') # # Extract the zones # zones = [] for model in world.findall('model'): links = model.findall('link') for link in links: layer = (link.attrib['layer'] if 'layer' in link.attrib else '').lower() zone_type = get_zone_type(layer) if zone_type != Zone.UNKNOWN: collision = link.find('visual') geometry = collision.find('geometry') polyline = geometry.find('polyline') points = [[float(t) for t in point.text.split(' ')] for point in polyline.findall('point')] zones.append( Zone(name=link.attrib['name'] if 'name' in link.attrib else layer, zone_type=zone_type, polygon=PolygonMsg(points=[ Point32Msg(point[0], point[1], 0) for point in points ]))) # # Extract the path nodes # nodes = [] for model in world.findall('model'): for node in model.findall('node'): position = node.find('position') node_id = node.attrib['id'] points = [float(t) for t in position.text.split(' ')] nodes.append(Node(id=node_id, x=points[0], y=points[1])) # # Extract the paths # paths = [] for model in world.findall('model'): for path in model.findall('path'): node_ids = [ni.text for ni in path.findall('node')] paths.append(Path( name='', nodes=node_ids, )) # # Extract the markers # markers = [] for model in world.findall('model'): links = model.findall('link') for link in links: layer = (link.attrib['layer'] if 'layer' in link.attrib else '').lower() if layer == 'marker': pose = link.find('pose') values = [float(f) for f in pose.text.split(' ')] if len(values) != 6: raise Exception('Invalid Pose: {}'.format(values)) qt = Quaternion.from_euler_extrinsic(*values[3:]) markers.append( Marker(name=link.attrib['name'] if 'name' in link.attrib else layer, marker_type=0, pose=PoseMsg(position=PointMsg( values[0], values[1], values[2]), orientation=QuaternionMsg(x=qt.x, y=qt.y, z=qt.z, w=qt.w)))) # # Save the map # add_map_srv = rospy.ServiceProxy(name='/map_manager/add_map', service_class=AddMap) add_map_srv.wait_for_service(timeout=10) add_response = add_map_srv.call( AddMapRequest(map=Map(info=MapInfo( name='sim_map', meta_data=MapMetaData( resolution=resolution, width=map_size[0] / resolution, height=map_size[1] / resolution, origin=PoseMsg( position=PointMsg(x=map_origin[0], y=map_origin[1])))), markers=markers, zones=zones, paths=paths, nodes=nodes, default_zone=Zone.EXCLUSION_ZONE), occupancy_grid=CompressedImage(format='png', data=im_bytes), map_data=pb_bytes)) # type: AddMapResponse if not add_response.success: raise Exception('Failed to save map: {}'.format(add_response.message))