I’ve tried to do this several ways, but none have worked. The code below is the closest I can get to it working. It seems like the angular velocity is being improperly calculated; however, using the given equations (3 through 8) and doing the math out by hand, it is calculating the “correct” value. The robot undershoots the goal and then tries to correct itself but eventually loops all over the place forever. Is there something I’m missing, or am I going about this the wrong way?
def go_to(xg, yg, thetag_degrees):
rho = float("inf")
thetag = math.radians(thetag_degrees)
while rho > 0.01:
x = odometry.odom_pose['x']
y = odometry.odom_pose['y']
theta = odometry.odom_pose['theta']
dx = xg - x
dy = yg - y
rho = math.sqrt(pow(dx, 2) + pow(dy, 2))
alpha = normalize(-thetag + math.atan2(dy, dx))
beta = normalize(-thetag - alpha)
v = k_rho*rho
w = (k_alpha*alpha + k_beta*beta)
print('theta: %.4f' % theta) # low effort attempt at troubleshooting
print('x : %.4f' % x)
print('y : %.4f' % y)
print('dx : %.4f' % dx)
print('dy : %.4f' % dy)
print('rho : %.4f' % rho)
print('alpha: %.4f' % alpha)
print('beta : %.4f' % beta)
print('v : %.4f' % v)
print('w : %.4f' % w)
print('----------------------------')
velocity.move(v, w)
rospy.sleep(0.01)
k_rho = 0.3
k_alpha = 0.8
k_beta = -0.15
velocity = VelocityController(‘/cmd_vel’)
odometry = OdometryReader(‘/odom’)
go_to(3, 1, 0)
velocity.move(0, 0)
odometry.unregister()
error = math.hypot(odometry.odom_pose[‘x’], odometry.odom_pose[‘y’])
print(‘Final positioning error is %.2fm’ % error)