def get_bounding_box(self): sx, sy, sz = self.size pts = np.array([[0, 0, sx, sx], [-sy / 2, sy / 2, sy / 2, -sy / 2], [0, 0, 0, 0], [1, 1, 1, 1]]) pts = geometry.aboutZ(self.theta).dot(pts) pts = geometry.translate(self.x, self.y).dot(pts) mins = pts.min(1) maxs = pts.max(1) xmin = mins[0] ymin = mins[1] xmax = maxs[0] ymax = maxs[1] return ((xmin, ymin), (xmax, ymax))
def update_walls(self): for key, value in self.robot.world.particle_filter.sensor_model.landmarks.items( ): if key.startswith('Wall-'): if key in self.objects: wall = self.objects[key] if not wall.is_fixed and not wall.is_foreign: wall.update(self, x=value[0][0][0], y=value[0][1][0], theta=value[1]) else: print('Creating new wall in worldmap:', key) wall_spec = wall_marker_dict[key] wall = WallObj(id=key, x=value[0][0][0], y=value[0][1][0], theta=value[1], length=wall_spec.length, height=wall_spec.height, door_width=wall_spec.door_width, door_height=wall_spec.door_height, marker_specs=wall_spec.marker_specs, doorways=wall_spec.doorways, door_ids=wall_spec.door_ids, is_foreign=False, spec_id=key) self.objects[key] = wall wall.pose_confidence = +1 # Make the doorways wall.make_doorways(self.robot.world.world_map) # Relocate the aruco markers to their predefined positions spec = wall_marker_dict.get(wall.id, None) if spec is None: return for key, value in spec.marker_specs.items(): if key in self.robot.world.world_map.objects: aruco_marker = self.robot.world.world_map.objects[key] dir = value[0] # +1 for front side or -1 for back side s = 0 if dir == +1 else pi aruco_marker.theta = wrap_angle(wall.theta + s) wall_xyz = geometry.point( -dir * (wall.length / 2 - value[1][0]), 0, value[1][1]) rel_xyz = geometry.aboutZ(aruco_marker.theta + pi / 2).dot(wall_xyz) aruco_marker.x = wall.x + rel_xyz[0][0] aruco_marker.y = wall.y + rel_xyz[1][0] aruco_marker.z = rel_xyz[2][0] aruco_marker.is_fixed = wall.is_fixed
def make_arucos(self, world_map): "Called by add_fixed_landmark to make fixed aruco markers." for key, value in self.marker_specs.items(): # Project marker onto the wall; move marker if it already exists marker = world_map.objects.get(key, None) if marker is None: marker_number = int(key[1 + key.rfind('-'):]) marker = ArucoMarkerObj(world_map.robot.world.aruco, marker_number=marker_number) world_map.objects[marker.id] = marker wall_xyz = geometry.point(self.length / 2 - value[1][0], 0, value[1][1]) s = 0 if value[0] == +1 else pi rel_xyz = geometry.aboutZ(self.theta + s).dot(wall_xyz) marker.x = self.x + rel_xyz[1][0] marker.y = self.y + rel_xyz[0][0] marker.z = rel_xyz[2][0] marker.theta = wrap_angle(self.theta + s) marker.is_fixed = self.is_fixed
def update_charger(self): charger = self.robot.world.charger if charger is None: return charger_id = 'Charger' wmobject = self.objects.get(charger_id, None) if wmobject is None: wmobject = ChargerObj(charger) self.objects[charger_id] = wmobject wmobject.sdk_obj = charger # In case we created charger before seeing it if self.robot.is_on_charger: wmobject.update_from_sdk = False theta = wrap_angle(self.robot.world.particle_filter.pose[2] + pi) charger_offset = np.array([[-30], [0], [0], [1]]) offset = geometry.aboutZ(theta).dot(charger_offset) wmobject.x = self.robot.world.particle_filter.pose[0] + offset[0, 0] wmobject.y = self.robot.world.particle_filter.pose[1] + offset[1, 0] wmobject.theta = theta wmobject.pose_confidence = +1 elif charger.is_visible: wmobject.update_from_sdk = True wmobject.pose_confidence = +1 elif ((charger.pose is None) or not charger.pose.is_comparable(self.robot.pose)): wmobject.update_from_sdk = False wmobject.pose_confidence = -1 else: # Robot re-localized so charger pose came back pass # skip for now due to SDK bug # wmobject.update_from_sdk = True # wmobject.pose_confidence = max(0, wmobject.pose_confidence) if wmobject.update_from_sdk: # True unless pose isn't comparable self.update_coords_from_sdk(wmobject, charger) wmobject.orientation, _, _, wmobject.theta = get_orientation_state( charger.pose.rotation.q0_q1_q2_q3) return wmobject
def world_joint(self): return geometry.translate(self.q[0], self.q[1]).dot(geometry.aboutZ(self.q[2]))
def revolute(self): return geometry.aboutZ(-self.q)