def __call__(self, sensor, landmark): p = sensor.position nh = sensor.orientation.xh q = landmark.position mh = landmark.orientation.xh vec = q - p if (vec * nh) <= 0.0 or (nh * mh) <= 0.0: return 0.0, gmi.Vector(), gmi.Vector() return self.spline(vec.norm) * (vec * nh) / vec.norm * ( nh * mh), gmi.Vector(), gmi.Vector()
def coverage_controller(): value = 0.0 pos_grad = gmi.Vector() ori_grad = gmi.Vector() for landmark in landmarks: if landmark.coverage < landmark.TARGET_COVERAGE: val, pg, og = FOOTPRINT(pose, landmark.POSE) value += val landmark.coverage += val landmark.contributed_coverage += val pos_grad += pg ori_grad += og return value, pos_grad, ori_grad
def pose_controller(): pos_grad = gmi.Vector() ori_grad = gmi.Vector() if not reference_landmark is None: ref_pose = reference_landmark.POSE pos_grad = GAIN*((ref_pose.position-pose.position)-pose.orientation.xh) ori_grad = 0.3*GAIN*(pose.orientation.xh.cross(ref_pose.orientation.xh)) value = 0.0 for landmark in landmarks: if landmark.coverage < landmark.TARGET_COVERAGE: val, _, _ = FOOTPRINT(pose, landmark.POSE) landmark.coverage += val landmark.contributed_coverage += val value += val return value, pos_grad, ori_grad
def __call__(self, sns, lmk): R = self.__BIG_RADIUS r = self.__SMALL_RADIUS q = lmk.position p = sns.position nh = sns.orientation.xh vec = (q - p - r * nh) xi = vec * nh zeta2_mi2 = vec.norm**2 - xi**2 if xi > 0: val = 1.0 - vec.norm**2 / R**2 pg = 2 * vec / R**2 og = 2 * vec / R**2 if val < 0.0: return 0.0, gmi.Vector(), gmi.Vector() else: return val, pg, gmi.Vector() else: val = 1.0 - xi**2 / r**2 - zeta2_mi2 / R**2 pg = 2 * xi * (1 / r**2 - 1 / R**2) * nh + 2 / R**2 * vec og = (2 * xi * (1 / R**2 - 1 / r**2) + 2 / R**2) * vec if val < 0.0: return 0.0, gmi.Vector(), gmi.Vector() else: return val, pg, gmi.Vector()
def outward_orthogonal(self, point): if -self._RADIUS < point.x < self._RADIUS: vec = gmi.Vector(point.x, point.y, 0.0) return vec / vec.norm else: return Vector(0, 1, 0)
def outward_orthogonal(self, point): vec = gmi.Vector(point.x - self._X0, point.y - self._Y0, 0.0) return vec / vec.norm
def __call__(self, sns, lmk): return max( (0.0, sns.orientation.xh * lmk.orientation.xh)), gmi.Vector(), sns.orientation.xh.cross( lmk.orientation.xh)