Program for calculating the linear drag coefficient in all 6 directions

Drag.py

@@ -0,0 +1,189 @@
+#!/usr/bin/env python
+import rospy
+from geometry_msgs.msg import WrenchStamped
+from nav_msgs.msg import Odometry
+# Initialized ros parameters and global values (feel free to tweak, values were empirically gained through trial/error)
+rospy.set_param('LINEAR_FORCE', 10)
+rospy.set_param("TORQUE", 5)
+rospy.set_param('ForceDown', -20)
+rospy.set_param('TimeOfForceDown', 10)
+Linear_Drag_X = 0
+Linear_Drag_Y = 0
+Linear_Drag_Z = 0
+Roll_Drag = 0
+Pitch_Drag = 0
+Yaw_Drag = 0
+Velocity = 0
+'''
+Velocities between two time intervals will never be truly 'equal' in real world
+Must compare the abs of the difference of the two with a small delta value
+if the difference is less than this value we can conclude that the numbers are satisfyingly close
+also used as a value that is satisfyingly close to zero. Hard-coded (can be changed).
+'''
+delta = .00001
+# Max velocity cannot be initialized to 0 or there will be an initial divide-by-zero error
+Max_Velocity = .1
+Bouyancy = 0
+# Magnitude of applied force for determining drag coeffcients in linear axes
+appliedLinearForce = rospy.get_param("LINEAR_FORCE", default=10)
+# Magnitude of applied torque for determining drag coeffcients in rotational axes
+appliedTorque = rospy.get_param("TORQUE", default=5)
+appliedForceDown = rospy.get_param('ForceDown', default=-20)
+TimeOfAplliedForceDown = rospy.get_param('TimeOfForceDown')
+
+
+def Find_Bouyancy(choice):  # Initial function used to upward force of buoyancy (used in determining drag in Z axis)
+    global Bouyancy
+    Down_Force = -.5  # Applies initial downward force in z axis
+    pub = rospy.Publisher('/wrench', WrenchStamped, queue_size=30)
+    rospy.init_node('move_sub', anonymous=False)
+    force_msg = WrenchStamped()
+    force_msg.wrench.force.z = appliedForceDown
+    pub.publish(force_msg)  # publish wrench with force
+    rospy.sleep(TimeOfAplliedForceDown)  # node sleeps for some amount of time before continuing
+    force_msg.wrench.force.z = 0  # Applies 0 force which stops downward force
+    pub.publish(force_msg)
+    while not (rospy.is_shutdown()):
+        rospy.Subscriber("/odom", Odometry, get_velocity, choice)
+        rospy.sleep(1)
+        if Velocity > 0.0:
+            while not (rospy.is_shutdown()):
+                force_msg.wrench.force.z = Down_Force
+                pub.publish(force_msg)
+                Down_Force = Down_Force - .001
+                rospy.sleep(.01)
+                if Velocity < 0.0:
+                    break
+            break
+    Bouyancy = abs(Down_Force)
+
+
+def get_velocity(data, choice):  # Function sets velocity in a certain axis depending on char input
+    global Velocity
+    # linear
+    if choice == 'x':
+        Velocity = data.twist.twist.linear.x
+    elif choice == 'y':
+        Velocity = data.twist.twist.linear.y
+    elif choice == 'z':
+        Velocity = data.twist.twist.linear.z
+    # rotational
+    elif choice == 'rl':
+        Velocity = data.twist.twist.angular.x
+    elif choice == 'p':
+        Velocity = data.twist.twist.angular.y
+    elif choice == 'yw':
+        Velocity = data.twist.twist.angular.z
+
+
+def Calculate_Drag(choice):
+    '''
+    Calculates drag based on the initial applied force and the approximate max velocity
+    the sub achieves in that axis. See for formula:
+    Axis determined by char argument.
+    '''
+    global Linear_Drag_X, Linear_Drag_Y, Linear_Drag_Z, Pitch_Drag, Roll_Drag, Yaw_Drag, Max_Velocity, Bouyancy
+    if (choice == 'x'):
+        Linear_Drag_X = (appliedLinearForce / abs(Max_Velocity))
+    elif (choice == 'y'):
+        Linear_Drag_Y = (appliedLinearForce / abs(Max_Velocity))
+    elif (choice == 'z'):
+        # Buoyancy affects z axis and must be subtracted from applied for before division.
+        Linear_Drag_Z = ((appliedLinearForce - Bouyancy) / (abs(Max_Velocity)))
+    elif (choice == 'rl'):
+        Roll_Drag = (appliedTorque / abs(Max_Velocity))
+    elif (choice == 'p'):
+        Pitch_Drag = (appliedTorque / abs(Max_Velocity))
+    elif (choice == 'yw'):
+        Yaw_Drag = (appliedTorque / abs(Max_Velocity))
+
+
+def Apply_Force(choice):
+    '''
+    Function applies force in a given axis and allows sub to achieve
+    terminal (linear/rotationl) velocity in that direction. Once that
+    max velocity is found, it is used in the Calculate_Drag() function.
+    '''
+    global Max_Velocity, Bouyancy, Linear_Drag_Z
+    pub = rospy.Publisher('/wrench', WrenchStamped, queue_size=20)  # Publisher for applying wrench (force/torque)
+    rospy.init_node('move_sub', anonymous=False)
+    force_msg = WrenchStamped()
+    # Char argument determines axis of applied force/torque
+    if(choice == 'x'):
+        force_msg.wrench.force.x = appliedLinearForce
+    elif(choice == 'y'):
+        force_msg.wrench.force.y = appliedLinearForce
+    elif(choice == 'z'):
+        force_msg.wrench.force.z = -appliedLinearForce
+    elif(choice == 'yw'):
+        force_msg.wrench.torque.z = appliedTorque
+    elif(choice == 'rl'):
+        force_msg.wrench.torque.x = appliedTorque
+    elif(choice == 'p'):
+        force_msg.wrench.torque.y = appliedTorque
+
+    pub.publish(force_msg)
+
+    while not (rospy.is_shutdown()):
+        '''
+        On each iteration of while loop: velocity is gained at two points in time (deltat = 2 seconds)
+        the velocity is compared by taking the absolute value of the difference of the two velocities
+        if the abs of the difference of the two velocities is smaller than small delta value,
+        the two velocities are assumed to be approximately equal. If two velocities taken at different
+        time intervals are equal, the velocity is assumed to be maximized, reaching terminal velocity.
+        '''
+        rospy.Subscriber("/odom", Odometry, get_velocity, choice)
+        velocity1 = Velocity
+        rospy.sleep(2)
+        velocity2 = Velocity
+        Compare_Velocities = abs(velocity1 - velocity2)
+        if Compare_Velocities < delta:
+            Max_Velocity = velocity2
+            Calculate_Drag(choice)  # Once velocity is max, calculate drag using max velocity
+            break  # When the velocities are 'equal', the loop breaks.
+
+    # Once drag is calculated, apply wrench with force/torque of zero to slow sub in that axis
+    if(choice == 'x'):
+        force_msg.wrench.force.x = 0
+    elif(choice == 'y'):
+        force_msg.wrench.force.y = 0
+    elif(choice == 'z'):
+        force_msg.wrench.force.z = 0
+    elif(choice == 'yw'):
+        force_msg.wrench.torque.z = 0
+    elif(choice == 'rl'):
+        force_msg.wrench.torque.x = 0
+    elif(choice == 'p'):
+        force_msg.wrench.torque.y = 0
+
+    pub.publish(force_msg)
+
+    while(Velocity > delta and not rospy.is_shutdown()):
+        # While loop stops program from proceeding until the sub has basically stopped movement in that direction
+        rospy.sleep(2)
+        continue
+
+
+if __name__ == '__main__':
+    try:
+        # Buoyancy is calculated
+        Find_Bouyancy('z')
+        # Drag Coeffcients are calculated in each axis
+        Apply_Force('z')
+        Apply_Force('y')
+        Apply_Force('x')
+        Apply_Force('yw')
+        Apply_Force('rl')
+        Apply_Force('p')
+        # Drag coefficients are written to file, do with them what you wish.
+        file_object = open("DragCoefficients", 'w')
+        file_object.write("Drag Coefficients:")
+        file_object.write("\nLinear X: " + str(Linear_Drag_X))
+        file_object.write("\nLinear Y: " + str(Linear_Drag_Y))
+        file_object.write("\nLinear Z: " + str(Linear_Drag_Z))
+        file_object.write("\nRoll: " + str(Roll_Drag))
+        file_object.write("\nPitch: " + str(Pitch_Drag))
+        file_object.write("\nYaw: " + str(Yaw_Drag))
+        file_object.close()
+    except rospy.ROSInterruptException:
+        pass