def _update_interpose(self): base_pose = WorldModel.get_pose(self._base) target_pose = WorldModel.get_pose(self._target) if base_pose is None or target_pose is None: return False angle_to_target = tool.getAngle(base_pose, target_pose) interposed_pose = Pose(0, 0, 0) if not self._to_dist is None: trans = tool.Trans(base_pose, angle_to_target) tr_interposed_pose = Pose(self._to_dist, 0.0, 0) interposed_pose = trans.invertedTransform(tr_interposed_pose) elif not self._from_dist is None: angle_to_base = tool.getAngle(target_pose, base_pose) trans = tool.Trans(target_pose, angle_to_base) tr_interposed_pose = Pose(self._from_dist, 0.0, 0) interposed_pose = trans.invertedTransform(tr_interposed_pose) interposed_pose.theta = angle_to_target self.pose = interposed_pose return True
def _update_receive_ball(self): ball_pose = WorldModel.get_pose('Ball') ball_vel = WorldModel.get_velocity('Ball') result = False if WorldModel.ball_is_moving(): angle_velocity = tool.getAngleFromCenter(ball_vel) trans = tool.Trans(ball_pose, angle_velocity) role_pose = WorldModel.get_pose(self._my_role) if role_pose is None: return False tr_pose = trans.transform(role_pose) fabs_y = math.fabs(tr_pose.y) if self._receiving == False and \ fabs_y < self._can_receive_dist - self._can_receive_hysteresis: self._receiving = True elif self._receiving == True and \ fabs_y > self._can_receive_dist + self._can_receive_hysteresis: self._receiving = False if self._receiving and tr_pose.x > 0.0: tr_pose.y = 0.0 inv_pose = trans.invertedTransform(tr_pose) angle_to_ball = tool.getAngle(inv_pose, ball_pose) self.pose = Pose(inv_pose.x, inv_pose.y, angle_to_ball) result = True return result
def _update_intersection(self): target_pose = WorldModel.get_pose(self._target) base_pose = WorldModel.get_pose(self._base) if target_pose is None or base_pose is None: return False intersection = tool.get_intersection(base_pose, target_pose, self._pose1, self._pose2) angle = tool.getAngle(intersection, target_pose) intersection.x = tool.limit(intersection.x, self._range_x[0], self._range_x[1]) intersection.y = tool.limit(intersection.y, self._range_y[0], self._range_y[1]) self.pose = Pose(intersection.x, intersection.y, angle) return True
def _update_keep_y(self): target_pose = WorldModel.get_pose(self._target) if target_pose is None: return False keep_pose = Pose(target_pose.x, self._keep_y, 0.0) keep_pose.x = tool.limit(keep_pose.x, self._range_x[0], self._range_x[1]) angle = tool.getAngle(keep_pose, target_pose) self.pose = Pose(keep_pose.x, keep_pose.y, angle) return True
def is_arrived(self, role): # robotが目標位置に到着したかを判断する # 厳し目につける role_pose = WorldModel.get_pose(role) if role_pose is None: return False arrived = False distance = tool.getSize(self.pose, role_pose) # 目標位置との距離、目標角度との差がtolerance以下であれば到着判定 if distance < self._arrived_position_tolerance: diff_angle = tool.normalize(self.pose.theta - role_pose.theta) if tool.normalize(diff_angle) < self._arrived_angle_tolerance: arrived = True return arrived
def _update_look_intersection(self): target_pose = WorldModel.get_pose(self._target) if target_pose is None: return False target_theta = target_pose.theta dist = 300 # フィールドサイズが300 mを超えたら書き直す必要あり look_pose = Pose(dist * math.cos(target_theta), dist * math.sin(target_theta), 0) intersection = tool.get_intersection(look_pose, target_pose, self._pose1, self._pose2) angle = tool.getAngle(intersection, target_pose) intersection.x = tool.limit(intersection.x, self._range_x[0], self._range_x[1]) intersection.y = tool.limit(intersection.y, self._range_y[0], self._range_y[1]) self.pose = Pose(intersection.x, intersection.y, angle) return True
def _update_approach_to_shoot(self): # Reference to this idea # http://wiki.robocup.org/images/f/f9/Small_Size_League_-_RoboCup_2014_-_ETDP_RoboDragons.pdf ball_pose = WorldModel.get_pose('Ball') target_pose = WorldModel.get_pose(self._target) role_pose = WorldModel.get_pose(self._my_role) if target_pose is None or role_pose is None: return False # ボールからターゲットを見た座標系で計算する angle_ball_to_target = tool.getAngle(ball_pose, target_pose) trans = tool.Trans(ball_pose, angle_ball_to_target) tr_role_pose = trans.transform(role_pose) # tr_role_poseのloser_side判定にヒステリシスをもたせる if self._role_is_lower_side == True and \ tr_role_pose.y > self._role_pose_hystersis: self._role_is_lower_side = False elif self._role_is_lower_side == False and \ tr_role_pose.y < - self._role_pose_hystersis: self._role_is_lower_side = True if self._role_is_lower_side: tr_role_pose.y *= -1.0 tr_approach_pose = Pose(0, 0, 0) if tr_role_pose.x > 0: # 1.ボールの斜め後ろへ近づく # copysign(x,y)でyの符号に合わせたxを取得できる tr_approach_pose = Pose( -self._pose_max.x, math.copysign(self._pose_max.y, tr_role_pose.y), 0) else: # ボール裏へ回るためのピボットを生成 pivot_pose = Pose(0, self._tuning_param_pivot_y, 0) angle_pivot_to_role = tool.getAngle(pivot_pose, tr_role_pose) limit_angle = self._tuning_angle + math.pi * 0.5 if tr_role_pose.y > self._tuning_param_pivot_y and \ angle_pivot_to_role < limit_angle: # 2.ボール後ろへ回りこむ diff_angle = tool.normalize(limit_angle - angle_pivot_to_role) decrease_coef = diff_angle / self._tuning_angle tr_approach_pose = Pose(-self._pose_max.x, self._pose_max.y * decrease_coef, 0) else: # 3.ボールに向かう diff_angle = tool.normalize(angle_pivot_to_role - limit_angle) approach_coef = diff_angle / (math.pi * 0.5 - self._tuning_angle) if approach_coef > 1.0: approach_coef = 1.0 pos_x = approach_coef * ( 2.0 * constants.BallRadius - self._tuning_param_x) + self._tuning_param_x tr_approach_pose = Pose(-pos_x, 0, 0) # 上下反転していたapproach_poseを元に戻す if self._role_is_lower_side: tr_approach_pose.y *= -1.0 self.pose = trans.invertedTransform(tr_approach_pose) self.pose.theta = angle_ball_to_target return True