Naogolf简易训练
Target List
- 图像定位
- 重定位
- 找球
- 击球
实现代码
step1.
def locateWithImage(self, best_ball_x=400, best_ball_y=320, best_ball_radius=28):
self.motionProxy.angleInterpolationWithSpeed(["HeadPitch", "HeadYaw"], [20 * rad, 0 * rad], 0.1)
self.ballDetect.updateBallData(client="cx", fitting=True)
centerX, centerY, radius = self.ballDetect.getBallInfoInImage()
bias_x = centerY - best_ball_y
bias_y = centerX - best_ball_x
Kp = 0.0008
if abs(radius - best_ball_radius) < 15:
if (abs(bias_x) < 40 and abs(bias_y) < 20):
self.tts.say("I am OK")
return True
else:
self.motionProxy.moveInit()
move_x = Kp * bias_x
move_x = -1.0 / 100 if (move_x < 0 and move_x > -1.0 / 100) else move_x
move_x = 1.0 / 100 if (move_x > 0 and move_x < 1.0 / 100) else move_x
if abs(bias_x) > 15:
self.motionProxy.moveTo(-move_x, 0, 0)
time.sleep(0.5)
move_y = Kp * bias_y
move_y = -1.0 / 100 if (move_y < 0 and move_y > -1.0 / 100) else move_y
move_y = 1.0 / 100 if (move_y > 0 and move_y < 1.0 / 100) else move_y
if abs(bias_y) > 15:
self.motionProxy.moveTo(0, -move_y, 0)
time.sleep(0.5)
step2.
# encoding:utf-8
def moveCircle_stick(self, stickAngle, ball_d, compensateAngle1):
'''
绕球走半径为机器人与球的距离的圆弧，分正反手
参数:
stickAngle:黄杆与机器人的角度
ball_d:机器人与球的距离
compensateAngle1:补偿角(机器人-杆-球的角度)
'''
# 杆-球-机器人的夹角(分锐角和钝角)
stick_ball_robotAngle = math.pi - compensateAngle1 * rad - abs(stickAngle)
# 正手打
if stickAngle >= 0:
self.isbackhandFlag = False
if 0 <= stick_ball_robotAngle < 0.5 * math.pi:
moveAngle = 0.5 * math.pi - stick_ball_robotAngle
move_d = (ball_d ** 2 + ball_d ** 2
- 2 * ball_d * ball_d * math.cos(moveAngle)) ** 0.5
self.motionProxy.moveInit()
self.motionProxy.moveTo(0, -move_d, moveAngle)
else:
moveAngle = stick_ball_robotAngle - 0.5 * math.pi
move_d = (ball_d ** 2 + ball_d ** 2
- 2 * ball_d * ball_d * math.cos(moveAngle)) ** 0.5
self.motionProxy.moveInit()
self.motionProxy.moveTo(0, move_d, -moveAngle)
# 反手打
else:
self.isbackhandFlag = True
stickAngle = abs(stickAngle)
if 0 <= stick_ball_robotAngle < 0.5 * math.pi:
moveAngle = 0.5 * math.pi - stick_ball_robotAngle
move_d = (ball_d ** 2 + ball_d ** 2
- 2 * ball_d * ball_d * math.cos(0.5 * math.pi - stick_ball_robotAngle)) ** 0.5
self.motionProxy.moveInit()
self.motionProxy.moveTo(0, move_d, -moveAngle)
else:
moveAngle = stick_ball_robotAngle - 0.5 * math.pi
move_d = (ball_d ** 2 + ball_d ** 2
- 2 * ball_d * ball_d * math.cos(stick_ball_robotAngle - 0.5 * math.pi)) ** 0.5
self.motionProxy.moveInit()
self.motionProxy.moveTo(0, -move_d, moveAngle)
step3.
# encoding:utf-8
def walkToBall(self, min_ball_d=0.35, max_ball_theta=0.15):
# 行走到球附近，若没有找到球则往前走一点
#
# 参数：
# min_ball_d:最小接近球的距离
# max_ball_theta:最大偏离角
isfindBall = self.findBall()
# 如果没有找到球则摇头找球
if isfindBall is False:
while True:
isfindBall = self.moveheadToFindball()
if isfindBall is False:
self.motionProxy.moveInit()
self.motionProxy.moveTo(0.5, 0, 0)
else:
break
# 否则找到球，调整机器人身体，使其正对着球
self.motionProxy.moveTo(0, 0, self.ball_info[2])
self.motionProxy.angleInterpolationWithSpeed("HeadYaw", 0, 0.1) # 头部回正
moveTask_1 = self.motionProxy.post.moveTo(1.5 * self.ball_info[0], 0, 0)
while True:
isfindBall = self.findBall(self.pitchAngle, 0)
if isfindBall is False:
self.moveheadToFindball(pitchAngles=[-10], yawAngles=[-15, 15])
ball_d = ((self.ball_info[0] ** 2 + self.ball_info[1] ** 2) ** 0.5)
ball_theta = abs(self.ball_info[2])
time.sleep(0.5)
# 头不断往下低,防止看不到球
Names = ["HeadPitch"]
if ball_d > 0.4:
self.pitchAngle = 0
elif 0.3 < ball_d < 0.4:
self.pitchAngle = (-100 * ball_d + 40)
elif 0.02 < ball_d < 0.3:
self.pitchAngle = 20
self.motionProxy.angleInterpolationWithSpeed(Names, self.pitchAngle * rad, 0.1)
# 到达最小距离内，停止
if min_ball_d / 3 < ball_d < min_ball_d:
self.tts.say("I am right.")
self.motionProxy.stop(moveTask_1)
time.sleep(0.5)
self.motionProxy.moveInit()
break
# 偏差在范围内，继续走
elif 0.02 < abs(ball_theta) < max_ball_theta:
# 到达最小接近球的距离
if min_ball_d / 3 < ball_d < min_ball_d:
self.tts.say("I am right.")
self.motionProxy.stop(moveTask_1)
time.sleep(0.5)
self.motionProxy.moveInit()
break
else:
continue
# 超过最大偏离角
elif abs(ball_theta) > max_ball_theta:
self.tts.say("I am wrong.")
self.motionProxy.stop(moveTask_1)
time.sleep(0.5)
self.motionProxy.moveInit()
self.motionProxy.moveTo(0, 0, self.ball_info[2])
moveTask_1 = self.motionProxy.post.moveTo(1.5 * self.ball_info[0], 0, 0)
step4.
# encoding:utf-8
def forehandToHitball(self, hitSpeed):
# 正手击球
#
# 参数：
# hitSpeed:击球的力度
# 手回正
names = ["RShoulderPitch", "RShoulderRoll", "RElbowRoll", "RElbowYaw", "RWristYaw"]
maxSpeedFraction = 0.1
targetAngles = [[90 * rad, -20 * rad, 5 * rad, 90 * rad, 0 * rad],
[80 * rad, -40 * rad, 5 * rad, 90 * rad, 0 * rad],
[50 * rad, -45 * rad, 50 * rad, 90 * rad, -37 * rad],
[50 * rad, 5 * rad, 50 * rad, 90 * rad, 20 * rad]] # 击球
for targetAngle in targetAngles:
if targetAngle == targetAngles[-1]:
maxSpeedFraction = hitSpeed
self.motionProxy.angleInterpolationWithSpeed(names, targetAngle, maxSpeedFraction)
time.sleep(0.5)
def backhandToHitball(self, hitSpeed):
# 反手击球
#
# 参数：
# hitSpeed：击球的力度
names = ["RShoulderPitch", "RShoulderRoll", "RElbowRoll", "RElbowYaw", "RWristYaw"]
maxSpeedFraction = 0.1
targetAngles = [[90 * rad, -20 * rad, 5 * rad, 90 * rad, 0 * rad], # 手回正
[80 * rad, -40 * rad, 5 * rad, 90 * rad, 0 * rad],
[50 * rad, -45 * rad, 50 * rad, 90 * rad, 0 * rad],
[50 * rad, 0 * rad, 70 * rad, 90 * rad, 40 * rad],
[60 * rad, 2 * rad, 60 * rad, 90 * rad, 40 * rad],
[60 * rad, -20 * rad, 60 * rad, 90 * rad, -20 * rad]]
for targetAngle in targetAngles:
if targetAngle == targetAngles[-1]:
maxSpeedFraction = hitSpeed
self.motionProxy.angleInterpolationWithSpeed(names, targetAngle, maxSpeedFraction)
time.sleep(0.5)
