From dedb8ceee4bead1ff05c4ef335ba3f53fbe6afc1 Mon Sep 17 00:00:00 2001 From: scorpio1 Date: Fri, 22 Jul 2022 09:35:44 -0400 Subject: [PATCH] added comments to straight_line.py --- src/straight_line.py | 36 +++++++++++++++++++++--------------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/src/straight_line.py b/src/straight_line.py index d04bd67..069338e 100755 --- a/src/straight_line.py +++ b/src/straight_line.py @@ -26,6 +26,7 @@ def state_callback(data): G=9.8 def convert_to_cylindrical(point): + #this code converts a point to cylindrical coordinates recentered = point-start_point y=np.dot(recentered, unit_vector) circle_portion = recentered-y*unit_vector @@ -37,28 +38,32 @@ def convert_to_cylindrical(point): return y, r, theta, circle_portion def get_error_acceleration(circle_position, r, velocity): + #here we take the velocity and displacement in the direction of the second + #and third unit vector, and use them to calculate accelerations + #based off modelling them as a damped spring velocity_unit_2 = np.dot(velocity, unit_vector_2) velocity_unit_3 = np.dot(velocity, unit_vector_3) circle_unit_2 = np.dot(circle_position, unit_vector_2) circle_unit_3 = np.dot(circle_position, unit_vector_3) accel = -1*(b3*velocity_unit_2+k*circle_unit_2)*unit_vector_2 accel = accel - (b*velocity_unit_3+k*circle_unit_3)*unit_vector_3 - #velocity_in_r = np.dot(circle_position, velocity)/r - #accel = -1*(b*velocity_in_r*circle_position/r + k*circle_position)/m return accel def get_forward_acceleration(y, velocity): + #here we calculate the forward acceleration, either based off how far off + #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-forward_velocity) forward_acceleration_2 = -1*(b2*forward_velocity+k2*(y-300))/m return min(forward_acceleration, forward_acceleration_2)*unit_vector - return forward_acceleration + def accel_publisher(): - + #first bit of code is to get the drone off the ground pub = rospy.Publisher('mavros/setpoint_accel/accel',Vector3Stamped, queue_size=10) setpointpub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) @@ -105,26 +110,25 @@ def accel_publisher(): setpointpub.publish(initial_pose) global start_point start_point=pose - rospy.logwarn('x: %f y: %f z: %f' %(pose[0], pose[1], pose[2])) + #Here is where we start doing navigation control while not rospy.is_shutdown(): + #here we calculate the acceleration we need in the forward direction, + #and the acceleration we need that's perpendicular to that direction y,r,theta,circle_position=convert_to_cylindrical(pose) forward_accel = get_forward_acceleration(y,velocity) error_accel = get_error_acceleration(circle_position,r,velocity) - #accel = get_error_acceleration(circle_position, r, velocity) + get_forward_acceleration(y,velocity) - #accel = get_forward_acceleration(y,velocity) - #if(np.linalg.norm(velocity)>8) and (np.linalg.norm(forward_accel)>0): - # forward_accel = -0*unit_vector + #we then add these together to get our acceleration vector accel = forward_accel+error_accel + #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 - #if np.linalg.norm(velocity)>8: - # accel = [0,0,0] - #accel+=get_error_acceleration(circle_position,r,velocity) to_publish=Vector3Stamped() to_publish.vector.x = accel[0] to_publish.vector.y=accel[1] + #we add 2 to the vertical acceleration because otherwise it sometimes + #just falls to_publish.vector.z=accel[2]+2 pub.publish(to_publish) rate.sleep() @@ -132,20 +136,22 @@ def accel_publisher(): if __name__ == '__main__': pose = None velocity = None + #describe constants for acceleration k = 2 - k2 = 0.5 - m=1.5 b3 = 1*np.sqrt(m*k) b = 0.4*np.sqrt(m*k) b2 = 2*np.sqrt(m*k2) + #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,0,0]) unit_vector_2 = np.array([0,1,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) - + #here we start creating the subscribers, and launching the drone state = State(connected=False,armed=False,guided=False,mode="MANUAL",system_status=0) rospy.init_node('circle_accelerations',anonymous=True) rospy.Subscriber('mavros/local_position/velocity_body', TwistStamped, velocity_callback)