From a1dcc7717589652f6ac9cfc04f4de107ec842f2c Mon Sep 17 00:00:00 2001 From: "cesar.alejnadro" Date: Tue, 5 Apr 2022 16:08:28 -0300 Subject: [PATCH] Removed unused dependencies --- .gitignore | 2 + CMakeLists.txt | 1 + launch/klausen_dampen.launch | 2 + src/LinkState.py | 81 +++++++++++++++++------------------- src/klausen_control.py | 74 ++++++++++++++++---------------- src/ref_signalGen.py | 70 ++++++++++++++----------------- 6 files changed, 110 insertions(+), 120 deletions(-) diff --git a/.gitignore b/.gitignore index 3dbf71b..8e75294 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ launch/debug.launch +launch/mocap_* launch/cortex_bridge.launch src/MoCap_Localization_*.py src/Mocap_*.py @@ -6,5 +7,6 @@ src/killswitch_client.py src/land_client.py msg/Marker.msg msg/Markers.msg +launc *.rviz diff --git a/CMakeLists.txt b/CMakeLists.txt index 17af69a..cbdd0e2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,6 +54,7 @@ add_message_files( tethered_status.msg RefSignal.msg EulerAngles.msg + LoadAngles.msg ) ## Generate services in the 'srv' folder diff --git a/launch/klausen_dampen.launch b/launch/klausen_dampen.launch index df8a76f..b2ce370 100644 --- a/launch/klausen_dampen.launch +++ b/launch/klausen_dampen.launch @@ -31,11 +31,13 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo pkg="oscillation_ctrl" type="wpoint_tracker.py" name="waypoints_server" + launch-prefix="xterm -e" /> = 3.0: + self.wait = 0.0 break # Will be set to true when test should start self.bool = False - # initialize variables - self.phi = 0.0 # Payload angle of deflection from x-axis - self.theta = 0.0 # Payload angle of deflection from y-axis - self.tetherL = 0.0 # Tether length - self.has_run = 0 # Boolean to keep track of first run instance - self.phidot = 0.0 # - self.thetadot = 0.0 # - self.phibuf = 0.0 # Need buffers to determine their rates - self.thetabuf = 0.0 # - self.pload = True # Check if payload exists - # Max dot values to prevent 'blowup' - self.phidot_max = 3.0 - self.thetadot_max = 3.0 - # Vehicle is spawned with yaw offset for convenience, need to deal with that self.yaw_offset = 0.0 # variables for message gen - self.status = tethered_status() - self.status.drone_id = 'spiri_with_tether::spiri::base' - self.status.pload_id = 'spiri_with_tether::mass::payload' + self.status = tethered_status() + self.drone_id = 'spiri_with_tether::spiri::base' + self.pload_id = 'spiri_with_tether::mass::payload' + self.loadAngles = LoadAngles() + + # initialize variables + self.tetherL = 0.0 # Tether length + self.has_run = 0 # Boolean to keep track of first run instance + self.phibuf = 0.0 # Need buffers to determine their rates + self.thetabuf = 0.0 # + self.pload = True # Check if payload exists + + # Max dot values to prevent 'blowup' + self.phidot_max = 3.0 + self.thetadot_max = 3.0 # service(s) self.service1 = '/gazebo/get_link_state' @@ -69,7 +68,8 @@ class Main: rospy.wait_for_service(self.service1,timeout=10) # publisher(s) - self.publisher = rospy.Publisher('/status/twoBody_status', tethered_status, queue_size=1) + self.twobody_pub = rospy.Publisher('/status/twoBody_status', tethered_status, queue_size=1) + self.loadAng_pub = rospy.Publisher('/status/load_angles', LoadAngles, queue_size=1) self.pub_wd = rospy.Publisher('/status/path_follow', Bool, queue_size=1) self.pub_timer = rospy.Timer(rospy.Duration(1.0/rate), self.link_state) @@ -120,18 +120,18 @@ class Main: # Establish links needed --> Spiri base and payload # P = Position vector - drone_P = get_P(self.status.drone_id,reference) + drone_P = get_P(self.drone_id,reference) # Get orientation of drone in euler angles to determine yaw offset drone_Eul = self.euler_array(drone_P.link_state.pose.orientation) # Check if payload is part of simulation if not drone_P.success: - self.status.drone_id = 'spiri::base' - drone_P = get_P(self.status.drone_id,reference) # i.e. no payload + self.drone_id = 'spiri::base' + drone_P = get_P(self.drone_id,reference) # i.e. no payload self.pload = False - pload_P = get_P(self.status.pload_id,reference) + pload_P = get_P(self.pload_id,reference) if not self.has_run == 1: if self.pload == True: @@ -184,30 +184,30 @@ class Main: x_sep = pload_Px - drone_Px if math.fabs(x_sep) >= self.tetherL or x_sep == 0: - self.theta = 0 + self.loadAngles.theta = 0 else: - self.theta = math.asin(x_sep/self.tetherL) + self.loadAngles.theta = math.asin(x_sep/self.tetherL) # Determine thetadot - self.thetadot = (self.theta - self.thetabuf)/self.dt - self.thetadot = self.cutoff(self.thetadot,self.thetadot_max) - self.thetabuf = self.theta + self.loadAngles.thetadot = (self.loadAngles.theta - self.thetabuf)/self.dt + self.loadAngles.thetadot = self.cutoff(self.loadAngles.thetadot,self.thetadot_max) + self.thetabuf = self.loadAngles.theta # Determine phi (roll) y_sep = pload_Py - drone_Py if math.fabs(y_sep) >= self.tetherL or y_sep == 0: - self.phi = 0 + self.loadAngles.phi = 0 else: - self.phi = math.asin(y_sep/self.tetherL) + self.loadAngles.phi = math.asin(y_sep/self.tetherL) # Determine phidot - self.phidot = (self.phi - self.phibuf)/self.dt - self.phidot = self.cutoff(self.phidot,self.phidot_max) - self.phibuf = self.phi # Update buffer + self.loadAngles.phidot = (self.loadAngles.phi - self.phibuf)/self.dt + self.loadAngles.phidot = self.cutoff(self.loadAngles.phidot,self.phidot_max) + self.phibuf = self.loadAngles.phi # Update buffer else: # Otherwise, vars = 0 - x_sep = self.phi = self.phidot = self.theta = self.thetadot = 0 + x_sep = self.loadAngles.phi = self.loadAngles.phidot = self.loadAngles.theta = self.thetadot = 0 # Print and save results print "\n" @@ -216,21 +216,18 @@ class Main: print "drone pos.x: " + str(round(drone_Px,2)) print "drone pos.y: " + str(round(drone_Py,2)) print "drone pos.z: " + str(round(drone_Pz,2)) - print "phi: " + str(round(self.phi*180/3.14,3)) - print "theta: " + str(round(self.theta*180/3.14,3)) + print "phi: " + str(round(self.loadAngles.phi*180/3.14,3)) + print "theta: " + str(round(self.loadAngles.theta*180/3.14,3)) # Populate message self.status.header.stamp = rospy.Time.now() self.status.drone_pos = drone_P.link_state.pose self.status.pload_pos = pload_P.link_state.pose - self.status.length = self.tetherL - self.status.phi = self.phi - self.status.phidot = self.phidot - self.status.theta = self.theta - self.status.thetadot = self.thetadot + self.loadAngles.header.stamp = rospy.Time.now() # Publish message - self.publisher.publish(self.status) + self.twobody_pub.publish(self.status) + self.loadAng_pub.publish(self.loadAngles) except rospy.ServiceException as e: rospy.loginfo("Get Link State call failed: {0}".format(e)) diff --git a/src/klausen_control.py b/src/klausen_control.py index 65bbcb8..563d8f9 100755 --- a/src/klausen_control.py +++ b/src/klausen_control.py @@ -12,15 +12,15 @@ import numpy as np import time import math -from tf.transformations import * -from scipy.integrate import odeint -from offboard_ex.msg import tethered_status, RefSignal -from controller_msgs.msg import FlatTarget -from geometry_msgs.msg import Point, Pose -from geometry_msgs.msg import TwistStamped, Vector3, Vector3Stamped, PoseStamped, Quaternion -from gazebo_msgs.srv import GetLinkState -from sensor_msgs.msg import Imu -from pymavlink import mavutil +from tf.transformations import * +from scipy.integrate import odeint +from oscillation_ctrl.msg import tethered_status, RefSignal, LoadAngles +from controller_msgs.msg import FlatTarget +from geometry_msgs.msg import Point, Pose +from geometry_msgs.msg import TwistStamped, Vector3, Vector3Stamped, PoseStamped, Quaternion +from gazebo_msgs.srv import GetLinkState +from sensor_msgs.msg import Imu +from pymavlink import mavutil class Main: @@ -46,19 +46,14 @@ class Main: self.path_pos = np.zeros([3,1]) self.path_vel = np.zeros([3,1]) self.path_acc = np.zeros([3,1]) - self.pl_pos = Pose() self.dr_pos = Pose() self.quaternion = PoseStamped() + self.load_angles = LoadAngles() self.EulerAng = [0,0,0] # Will find the euler angles, and then convert to q # Drone var - self.phi = 0.0 # Payload angle of deflection from x-axis - self.phidot = 0.0 - self.theta = 0.0 # Payload angle of deflection from y-axis - self.thetadot = 0.0 self.has_run = 0 # Bool to keep track of first run instance - self.drone_id = '' - self.pload_id = '' + # Col1 = theta, theta dot; Col2 = phi, phidot for self.PHI self.PHI = np.array([[0,0],[0,0]]) self.dr_vel = np.zeros([3,1]) @@ -105,7 +100,7 @@ class Main: # SUBSCRIBERS # --------------------------------------------------------------------------------# # Topic, msg type, and class callback method - rospy.Subscriber('/status/twoBody_status', tethered_status, self.linkState_cb) + rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb) rospy.Subscriber('/reference/path', FlatTarget, self.refsig_cb) rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) @@ -125,21 +120,22 @@ class Main: # CALLBACK FUNCTIONS # --------------------------------------------------------------------------------# - # Callback to get link names, states, and pload deflection angles - def linkState_cb(self,msg): + # Callback pload deflection angles and vel + def loadAngles_cb(self,msg): try: - self.drone_id = msg.drone_id - self.pload_id = msg.pload_id - self.dr_pos = msg.drone_pos - self.pl_pos = msg.pload_pos - #self.tetherL = msg.length - self.phi = msg.phi - self.phidot = msg.phidot - self.theta = msg.theta - self.thetadot = msg.thetadot + self.load_angles = msg # Populate self.PHI - self.PHI = np.array([[self.theta,self.thetadot],[self.phi,self.phidot]]) + self.PHI = np.array([[self.load_angles.theta,self.load_angles.thetadot],[self.load_angles.phi,self.load_angles.phidot]]) + + except ValueError: + pass + + + # Callback drone pose + def dronePos_cb(self,msg): ### NEED to add mavros/local_pos.. sub + try: + self.dr_pos = msg.pose except ValueError: pass @@ -195,14 +191,14 @@ class Main: def control(self): # Populate position vector and gamma (g). g is state space vector: [px,py,pz,theta,phi] p = np.array([[self.dr_pos.position.x],[self.dr_pos.position.y],[self.dr_pos.position.z]]) - g = np.array([[self.dr_pos.position.x],[self.dr_pos.position.y],[self.dr_pos.position.z],[self.theta],[self.phi]]) + g = np.array([[self.dr_pos.position.x],[self.dr_pos.position.y],[self.dr_pos.position.z],[self.load_angles.theta],[self.load_angles.phi]]) # State some variables for shorthand L = self.tetherL - c_theta = math.cos(self.theta) - c_phi = math.cos(self.phi) - s_theta = math.sin(self.theta) - s_phi = math.sin(self.phi) + c_theta = math.cos(self.load_angles.theta) + c_phi = math.cos(self.load_angles.phi) + s_theta = math.sin(self.load_angles.theta) + s_phi = math.sin(self.load_angles.phi) # Check for tether if L <= 0.01: @@ -224,15 +220,15 @@ class Main: [L*self.pl_m*c_theta, L*self.pl_m*s_phi*s_theta, -L*self.pl_m*c_phi*s_theta, 0, L**2*self.pl_m]] - C = [[0,0,0,0,-L*self.thetadot*self.pl_m*s_theta], + C = [[0,0,0,0,-L*self.load_angles.thetadot*self.pl_m*s_theta], - [0,0,0,L*self.pl_m*(self.phidot*c_theta*s_phi + self.thetadot*c_phi*s_theta), L*self.pl_m*(self.phidot*c_phi*s_theta + self.thetadot*c_theta*s_phi)], + [0,0,0,L*self.pl_m*(self.load_angles.phidot*c_theta*s_phi + self.load_angles.thetadot*c_phi*s_theta), L*self.pl_m*(self.load_angles.phidot*c_phi*s_theta + self.load_angles.thetadot*c_theta*s_phi)], - [0,0,0,-L*self.pl_m*(self.phidot*c_phi*c_theta - self.thetadot*s_phi*s_theta),-L*self.pl_m*(self.thetadot*c_phi*c_theta - self.phidot*s_phi*s_theta)], + [0,0,0,-L*self.pl_m*(self.load_angles.phidot*c_phi*c_theta - self.load_angles.thetadot*s_phi*s_theta),-L*self.pl_m*(self.load_angles.thetadot*c_phi*c_theta - self.load_angles.phidot*s_phi*s_theta)], - [0,0,0,-0.5*(L**2)*self.pl_m*self.thetadot*math.sin(2*self.theta) + 0.5*0.01*self.thetadot*math.sin(2*self.theta), -0.5*(L**2)*self.pl_m*self.phidot*math.sin(2*self.theta)], + [0,0,0,-0.5*(L**2)*self.pl_m*self.load_angles.thetadot*math.sin(2*self.load_angles.theta) + 0.5*0.01*self.load_angles.thetadot*math.sin(2*self.load_angles.theta), -0.5*(L**2)*self.pl_m*self.load_angles.phidot*math.sin(2*self.load_angles.theta)], - [0,0,0,0.5*(L**2)*self.pl_m*self.phidot*math.sin(2*self.theta),0]] + [0,0,0,0.5*(L**2)*self.pl_m*self.load_angles.phidot*math.sin(2*self.load_angles.theta),0]] G = [[0],[0],[-9.81*self.tot_m],[L*9.81*self.pl_m*c_theta*s_phi],[L*9.81*self.pl_m*c_phi*s_theta]] diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py index d7236fa..d04467b 100755 --- a/src/ref_signalGen.py +++ b/src/ref_signalGen.py @@ -12,7 +12,7 @@ from pymavlink import mavutil from scipy import signal from scipy.integrate import odeint -from oscillation_ctrl.msg import tethered_status, RefSignal +from oscillation_ctrl.msg import tethered_status, RefSignal, LoadAngles from oscillation_ctrl.srv import WaypointTrack, WaypointTrackRequest from controller_msgs.msg import FlatTarget from geometry_msgs.msg import Pose, Vector3, PoseStamped, Point, TwistStamped @@ -39,22 +39,14 @@ class Main: self.t = np.linspace(0, self.tmax, self.n) # Time array # Message generation/ collection - self.state = State() - self.mode = '' - self.vel_data = TwistStamped() # This is needed to get drone vel from gps - self.imu_data = Imu() # Needed for to get drone acc from IMU - self.ref_sig = FlatTarget() # Smooth Signal - self.ref_sig.position.z = 5.0 # This does not need to be determined + self.state = State() + self.mode = '' + self.vel_data = TwistStamped() # This is needed to get drone vel from gps + self.imu_data = Imu() # Needed for to get drone acc from IMU + self.ref_sig = FlatTarget() # Smooth Signal + self.load_angles = LoadAngles() - self.phi = 0.0 # Payload angle of deflection from x-axis - self.phidot = 0.0 - self.theta = 0.0 # Payload angle of deflection from y-axis - self.thetadot = 0.0 self.has_run = 0 # Bool to keep track of first run instance - self.drone_id = '' - self.pload_id = '' - - self.pl_pos = Pose() self.dr_pos = Pose() self.dr_vel = self.vel_data.twist.linear self.dr_acc = self.imu_data.linear_acceleration @@ -74,7 +66,8 @@ class Main: # SUBSCRIBERS # # --------------------------------------------------------------------------------# # Topic, msg type, and class callback method - rospy.Subscriber('/status/twoBody_status', tethered_status, self.linkState_cb) + rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb) + rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb) rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) rospy.Subscriber('/mavros/state', State, self.state_cb) @@ -198,18 +191,17 @@ class Main: # mavros publishes a disconnected state message on init # Callback to get link names, states, and pload deflection angles - def linkState_cb(self,msg): + def loadAngles_cb(self,msg): try: - self.drone_id = msg.drone_id - self.pload_id = msg.pload_id - self.dr_pos = msg.drone_pos - self.pl_pos = msg.pload_pos - self.phi = -msg.phi - self.phidot = -msg.phidot - self.theta = msg.theta - self.thetadot = msg.thetadot -# self.tetherL = msg.length - + self.load_angles = msg + except ValueError: + pass + + # Callback drone pose + def dronePos_cb(self,msg): ### NEED to add mavros/local_pos.. sub + try: + self.dr_pos = msg.pose + except ValueError: pass @@ -298,13 +290,13 @@ class Main: elif cmd == 2: # Feedback if self.FB_idx < len(self.theta_fb): # First, fill up the delay array - self.theta_fb[self.FB_idx] = self.theta - self.theta_vel_fb[self.FB_idx] = self.thetadot - self.theta_acc_fb[self.FB_idx] = self.thetadot - self.theta_vel_fb[self.FB_idx-1] + self.theta_fb[self.FB_idx] = self.load_angles.theta + self.theta_vel_fb[self.FB_idx] = self.load_angles.thetadot + self.theta_acc_fb[self.FB_idx] = self.load_angles.thetadot - self.theta_vel_fb[self.FB_idx-1] - self.phi_fb[self.FB_idx] = self.phi - self.phi_vel_fb[self.FB_idx] = self.phidot - self.phi_acc_fb[self.FB_idx] = self.phidot - self.phi_vel_fb[self.FB_idx-1] + self.phi_fb[self.FB_idx] = self.load_angles.phi + self.phi_vel_fb[self.FB_idx] = self.load_angles.phidot + self.phi_acc_fb[self.FB_idx] = self.load_angles.phidot - self.phi_vel_fb[self.FB_idx-1] else: # once array is filled, need to shift values w/ latest value at end @@ -316,13 +308,13 @@ class Main: self.phi_vel_fb[:] = np.roll(self.phi_vel_fb[:],-1) self.phi_acc_fb[:] = np.roll(self.phi_acc_fb[:],-1) - self.theta_fb[len(self.theta_fb)-1] = self.theta # change final value - self.theta_vel_fb[len(self.theta_fb)-1] = self.thetadot - self.theta_acc_fb[len(self.theta_fb)-1] = self.thetadot - self.theta_vel_fb[len(self.theta_fb)-1] + self.theta_fb[len(self.theta_fb)-1] = self.load_angles.theta # change final value + self.theta_vel_fb[len(self.theta_fb)-1] = self.load_angles.thetadot + self.theta_acc_fb[len(self.theta_fb)-1] = self.load_angles.thetadot - self.theta_vel_fb[len(self.theta_fb)-1] - self.phi_fb[len(self.phi_fb)-1] = self.phi - self.phi_vel_fb[len(self.theta_fb)-1] = self.phidot - self.phi_acc_fb[len(self.theta_fb)-1] = self.phidot - self.phi_vel_fb[len(self.theta_fb)-1] + self.phi_fb[len(self.phi_fb)-1] = self.load_angles.phi + self.phi_vel_fb[len(self.theta_fb)-1] = self.load_angles.phidot + self.phi_acc_fb[len(self.theta_fb)-1] = self.load_angles.phidot - self.phi_vel_fb[len(self.theta_fb)-1] else: print('No delay')