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

worked on obstacle avoidance

This commit is contained in:
Matas Keras 2022-08-03 13:05:52 -04:00
parent 266573f2c1
commit db9c47b846
2 changed files with 25 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/obstacle_detection.py
View File

@ -31,7 +31,7 @@ def obstacle_callback(data):
def get_balance(weights):
distance=min(weights)
balance = (-1*distance+30)/27
balance = (-1*distance+80)/87
if balance<0:
balance=0
return balance
@ -44,7 +44,7 @@ def get_min_radius(weights):
radii_left = np.zeros(length)
radii_right = np.zeros(length)
for x in range(length):
angle = (length-10)*one_degree
angle = (x-30)*one_degree
radii_left[x],radii_right[x] = get_radius(x,angle)
index_left = np.argmin(radii_left)
index_right = np.argmin(radii_right)
@ -55,8 +55,8 @@ def get_min_radius(weights):
def get_radius(weight, angle):
radius_left = (weight*weight-9)/(6+2*weight*np.cos(np.pi/2-angle))
radius_right = (weight*weight-9)/(6+2*weight*np.cos(np.pi/2+angle))
radius_left = (weight*weight-25)/(10+2*weight*np.cos(np.pi/2-angle))
radius_right = (weight*weight-25)/(10+2*weight*np.cos(np.pi/2+angle))
return radius_left,radius_right
@ -69,9 +69,15 @@ def avoidance(weights):
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
acceleration_x = np.cos(angle_acceleration)*magnitude_acceleration
acceleration_y = np.sin(angle_acceleration)*magnitude_acceleration
magnitude_acceleration = 200*(velocity[0]*velocity[0]+velocity[1]*velocity[1])/(min_radius)
magnitude_velocity = np.sqrt(velocity[1]*velocity[1]+velocity[0]*velocity[0])
if magnitude_velocity<5:
magnitude_forward_accel = 5-magnitude_velocity
else:
magnitude_forward_accel = 0
acceleration_x = np.cos(angle_acceleration)*magnitude_acceleration+np.cos(angle_velocity)*magnitude_forward_accel
acceleration_y = np.sin(angle_acceleration)*magnitude_acceleration+np.sin(angle_velocity)*magnitude_forward_accel
return np.array([acceleration_x,acceleration_y,0])
@ -98,11 +104,11 @@ def get_error_acceleration(circle_position, r, velocity):
circle_unit_3 = np.dot(circle_position, unit_vector_3)
accel_2_1 = -1*(b3*velocity_unit_2+k*circle_unit_2)
accel_2_2 = 2*(9.5-abs(velocity_unit_2))*velocity_unit_2/abs(velocity_unit_2)
accel_2_2 = 2*(4.5-abs(velocity_unit_2))*velocity_unit_2/abs(velocity_unit_2)
accel_3_1 = -1*(b*velocity_unit_3+k*circle_unit_3)
accel_3_2 = 2*(9.5-abs(velocity_unit_3))*velocity_unit_3/abs(velocity_unit_3)
accel_3_2 = 2*(4.5-abs(velocity_unit_3))*velocity_unit_3/abs(velocity_unit_3)
error_accel = min(velocity_unit_2*accel_2_1,velocity_unit_2*accel_2_2)*unit_vector_2/velocity_unit_2
error_accel += min(velocity_unit_3*accel_3_1,velocity_unit_3*accel_3_2)*unit_vector_3/velocity_unit_3
return error_accel
@ -112,7 +118,7 @@ def get_forward_acceleration(y, velocity):
#the target speed we are, or based off a damped spring, depending on which
#is less (or if we are close to the finish)
forward_velocity=np.dot(velocity,unit_vector)
forward_acceleration = 2*(9.5-abs(forward_velocity))*forward_velocity/abs(forward_velocity)
forward_acceleration = 2*(4.5-abs(forward_velocity))*forward_velocity/abs(forward_velocity)
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
@ -136,7 +142,7 @@ def guidance_law():
accel = balance*avoidance_accel+(1-balance)*accel_straight
#vertical component is not affected by avoidance, so we undo the changes
accel[2] = accel[2]/(1-balance)
#then we decrease it if the acceleration is over 2g
accelerations=Vector3Stamped()
@ -226,8 +232,8 @@ if __name__ == '__main__':
#describe the vectors that define our direction of travel
#the first vector is the direction we travel, the other two are two more
#vectors that form an orthonormal basis with the first one
unit_vector = np.array([1,5,0])
unit_vector_2 = np.array([-5,1,0])
unit_vector = np.array([0,1,0])
unit_vector_2 = np.array([1,0,0])
unit_vector_3 = np.array([0,0,1])
unit_vector = unit_vector/np.linalg.norm(unit_vector)
unit_vector_2 = unit_vector_2/np.linalg.norm(unit_vector_2)

12
src/obstacle_test.py
View File

@ -38,12 +38,12 @@ def distance_to_line(line1,vector1, center1, line2, bound1, bound2):
return distance
def calculate_distance_array():
distances = np.zeros(21)
distances = np.zeros(61)
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])-30*one_degree
if center[0]<0:
first_angle+=np.pi
for x in range(21):
for x in range(61):
angle = first_angle+x*one_degree
vector = np.array([np.cos(angle),np.sin(angle)])
line = [0,0]
@ -67,8 +67,8 @@ def distance_publisher():
while not rospy.is_shutdown():
distances = calculate_distance_array()
to_publish = LaserScan()
to_publish.angle_min = -10*one_degree
to_publish.angle_max = 10*one_degree
to_publish.angle_min = -30*one_degree
to_publish.angle_max = 30*one_degree
to_publish.angle_increment = one_degree
to_publish.time_increment = 0
to_publish.scan_time = 0.1
@ -81,7 +81,7 @@ def distance_publisher():
if __name__ == '__main__':
one_degree = 5*np.pi/180
pose = None
wall_lines = [([1,100],[-10000,-10000],[10000,10000])]
wall_lines = [ ([5,150],[-10000,-10000],[10000,10000])]
try:
distance_publisher()
except rospy.ROSInterruptException: