00001 import math
00002 import numpy as np
00003 from turtlesim.msg import Velocity
00004 from turtlesim.msg import Pose
00005
00006 def unit(v):
00007 ''' Return unitary vector of direction v '''
00008 n = np.linalg.norm(v)
00009 if n != 0:
00010 return v / n
00011 else:
00012 return v
00013
00014 def perp(v):
00015 ''' Return the perpendiculary vector to v '''
00016 return np.array((-v[1], v[0]))
00017
00018 def dist(current_pose, dest_pos):
00019 ''' Return distance between current position and a target '''
00020 if hasattr(dest_pos, 'x') and hasattr(dest_pos, 'y'):
00021 dx = current_pose.x - dest_pos.x
00022 dy = current_pose.y - dest_pos.y
00023 else:
00024 dx = current_pose.x - dest_pos[0]
00025 dy = current_pose.y - dest_pos[1]
00026 return math.hypot(dx, dy)
00027
00028 def control_command(pose, dest_pos, speed = 1.0):
00029 ''' Compute velocity given current pose and target '''
00030 p = np.array((pose.x, pose.y))
00031 o = np.array((math.cos(pose.theta), math.sin(pose.theta)))
00032 t_g = np.array(dest_pos)
00033 d_t = unit(t_g - p)
00034 v_forward = np.dot(d_t, o)
00035 v_rot = np.dot(d_t, perp(o))
00036 return Velocity(v_forward * speed, v_rot * speed * 2)
00037
00038 def orbit_command(pose, center, speed = 1.0, d = 2.5):
00039 ''' Give a command such as orbiting around center '''
00040 c_d = dist(pose, center)
00041 d_d = c_d - d
00042 turn = max(0, d_d)
00043 return Velocity(speed, -turn*turn*0.1)
00044
00045 def angle_diff(a, b):
00046 pi = math.pi
00047 diff = a - b
00048 while diff > pi:
00049 diff -= 2*pi
00050 while diff < -pi:
00051 diff += 2*pi
00052 return diff
