scorpio1
/
OscillationControlWithCircle
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

fixes for obstacle avoidance nodes

This commit is contained in:
Matas Keras 2022-07-29 14:33:45 -04:00
parent 6a930316ff
commit 266573f2c1
2 changed files with 16 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/obstacle_detection.py
View File

@ -66,9 +66,9 @@ def avoidance(weights):
if velocity[0]<0: if velocity[0]<0:
angle_velocity+=np.pi angle_velocity+=np.pi
if go_left: if go_left:
angle_acceleration = angle_velocity+np.pi/2
else:
angle_acceleration = angle_velocity-np.pi/2 angle_acceleration = angle_velocity-np.pi/2
else:
angle_acceleration = angle_velocity+np.pi/2
magnitude_acceleration = 1.2*(velocity[0]*velocity[0]+velocity[1]*velocity[1])/min_radius magnitude_acceleration = 1.2*(velocity[0]*velocity[0]+velocity[1]*velocity[1])/min_radius
acceleration_x = np.cos(angle_acceleration)*magnitude_acceleration acceleration_x = np.cos(angle_acceleration)*magnitude_acceleration
acceleration_y = np.sin(angle_acceleration)*magnitude_acceleration acceleration_y = np.sin(angle_acceleration)*magnitude_acceleration
@ -113,7 +113,7 @@ def get_forward_acceleration(y, velocity):
#is less (or if we are close to the finish) #is less (or if we are close to the finish)
forward_velocity=np.dot(velocity,unit_vector) forward_velocity=np.dot(velocity,unit_vector)
forward_acceleration = 2*(9.5-abs(forward_velocity))*forward_velocity/abs(forward_velocity) forward_acceleration = 2*(9.5-abs(forward_velocity))*forward_velocity/abs(forward_velocity)
forward_acceleration_2 = -1*(b2*forward_velocity+k2*(y-100))/m forward_acceleration_2 = -1*(b2*forward_velocity+k2*(y-1000))/m
return min(forward_acceleration*forward_velocity, forward_acceleration_2*forward_velocity)*unit_vector/forward_velocity return min(forward_acceleration*forward_velocity, forward_acceleration_2*forward_velocity)*unit_vector/forward_velocity
def guidance_law(): def guidance_law():
@ -124,6 +124,10 @@ def guidance_law():
error_accel = get_error_acceleration(circle_position,r,velocity) error_accel = get_error_acceleration(circle_position,r,velocity)
#we then add these together to get our acceleration vector #we then add these together to get our acceleration vector
accel_straight = forward_accel+error_accel accel_straight = forward_accel+error_accel
magnitude_accel=np.linalg.norm(accel_straight)
if magnitude_accel>2*G:
accel_straight=accel_straight/magnitude_accel*2*G
#next we deal with the avoidance component #next we deal with the avoidance component
balance = get_balance(weights) balance = get_balance(weights)
avoidance_accel = avoidance(weights) avoidance_accel = avoidance(weights)
@ -134,9 +138,7 @@ def guidance_law():
accel[2] = accel[2]/(1-balance) accel[2] = accel[2]/(1-balance)
#then we decrease it if the acceleration is over 2g #then we decrease it if the acceleration is over 2g
magnitude_accel=np.linalg.norm(accel)
if magnitude_accel>2*G:
accel=accel/magnitude_accel*2*G
accelerations=Vector3Stamped() accelerations=Vector3Stamped()
accelerations.vector.x = accel[0] accelerations.vector.x = accel[0]
accelerations.vector.y=accel[1] accelerations.vector.y=accel[1]
@ -211,7 +213,7 @@ def accel_publisher():
if __name__ == '__main__': if __name__ == '__main__':
G=9.8 G=9.8
one_degree=np.pi/180 one_degree=5*np.pi/180
pose = None pose = None
velocity = None velocity = None
#describe constants for acceleration #describe constants for acceleration

12
src/obstacle_test.py
View File

@ -27,13 +27,13 @@ def intersection(line1, line2):
def distance_to_line(line1,vector1, center1, line2, bound1, bound2): def distance_to_line(line1,vector1, center1, line2, bound1, bound2):
intersect = intersection(line1,line2) intersect = intersection(line1,line2)
#if the intersection point is outside where the line actually exists, return inifinity #if the intersection point is outside where the line actually exists, return inifinity
if(np.all(intersect>bound1 and intercet<bound2)): if not (intersect[0]>bound1[0] and intersect[1]>bound1[1] and intersect[0]<bound2[0] and intersect[1]<bound2[1]):
return float('inf') return float('inf')
recentered_intersect = intersect-center1 recentered_intersect = intersect-center1
distance = recentered_intersec[0]/vector1[0] distance = recentered_intersect[0]/vector1[0]
#if the distance is negative, the point is behind, so we return infinity to show nothing ahead #if the distance is negative, the point is behind, so we return infinity to show nothing ahead
if distance<0: if distance<0:
return float('inf') return float('NaN')
return distance return distance
@ -41,6 +41,8 @@ def calculate_distance_array():
distances = np.zeros(21) distances = np.zeros(21)
center = np.array([pose.x,pose.y]) center = np.array([pose.x,pose.y])
first_angle = np.arctan(center_vector[1]/center_vector[0])-10*one_degree first_angle = np.arctan(center_vector[1]/center_vector[0])-10*one_degree
if center[0]<0:
first_angle+=np.pi
for x in range(21): for x in range(21):
angle = first_angle+x*one_degree angle = first_angle+x*one_degree
vector = np.array([np.cos(angle),np.sin(angle)]) vector = np.array([np.cos(angle),np.sin(angle)])
@ -77,9 +79,9 @@ def distance_publisher():
rate.sleep rate.sleep
if __name__ == '__main__': if __name__ == '__main__':
one_degree = np.pi/180 one_degree = 5*np.pi/180
pose = None pose = None
wall_lines = [] wall_lines = [([1,100],[-10000,-10000],[10000,10000])]
try: try:
distance_publisher() distance_publisher()
except rospy.ROSInterruptException: except rospy.ROSInterruptException: