From 5a6c18d9273aed11ece0271fb746e772bd588d4b Mon Sep 17 00:00:00 2001 From: cesar Date: Tue, 29 Nov 2022 11:11:14 -0400 Subject: [PATCH] Updated alititude controller --- .gitignore | 19 ++- CMakeLists.txt | 1 + config/mocapGazebo_params.yaml | 11 +- config/mocapLab_params.yaml | 17 +- config/spiri_params.yaml | 1 + launch/oscillation_damp.launch | 1 + src/klausen_control.py | 280 +++++++++++++++++++++++---------- src/path_follow.cpp | 16 -- src/ref_signalGen.py | 89 ++++++----- 9 files changed, 283 insertions(+), 152 deletions(-) diff --git a/.gitignore b/.gitignore index 6c0440d..ee8d331 100644 --- a/.gitignore +++ b/.gitignore @@ -1,8 +1,18 @@ -config/mocap* +*.rviz +setup.txt +px4_setup/protobuf_install.txt +config/mocapGazebo_params.yaml +config/mocapLab_params.yaml launch/cortex_bridge.launch +launch/development_* +launch/esp_* launch/headless_spiri_mocap.launch launch/headless_spiri_with_tether_mocap.launch launch/mocap_* +launch/replay.launch +msg/Marker.msg +msg/Markers.msg +srv/BodyToWorld.srv src/development_* src/killswitch_client.py src/land_client.py @@ -11,9 +21,6 @@ src/Mocap_*.py src/mocap_* src/segmented_tether.py src/segmented_tether_fast.py -msg/Marker.msg -msg/Markers.msg -*.rviz -setup.txt -px4_setup/protobuf_install.txt + + diff --git a/CMakeLists.txt b/CMakeLists.txt index 077d719..1c29bf5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,6 +32,7 @@ add_message_files( add_service_files( FILES WaypointTrack.srv + BodyToWorld.srv ) ## Generate actions in the 'action' folder diff --git a/config/mocapGazebo_params.yaml b/config/mocapGazebo_params.yaml index 79c4ac5..c7a46d4 100644 --- a/config/mocapGazebo_params.yaml +++ b/config/mocapGazebo_params.yaml @@ -1,14 +1,15 @@ # Ros param when using Klausen Ctrl -wait_time: 30 # parameter which can be set to run desired tests at a desired time +wait_time: 15 # parameter which can be set to run desired tests at a desired time #drone_mass: 0.614 # weight with new battery drone_mass: 0.602 -pload_mass: 0.1 # mass of payload. Needs to be changed in spiri_with_tether file as well + +pload: true +pload_mass: 0.2 # mass of payload. #pload_mass: 0.15 # Pload mass with 100g weight -pload_mass: 0.10 # Pload mass with 50g weight #pload_mass: 0.05 # Pload mass with just basket #pload_mass: 0.25 -use_ctrl: false # starts PX4 without attitude controller -waypoints: {x: 0.0, y: 0.0, z: 5.0} # takeoff waypoints +change_mode: true # choose whether to switch to oscillation damping controller +waypoints: {x: 0.0, y: 0.0, z: 2.0} # takeoff waypoints # sets waypoints to run a square test square_x: [0.5,1,1,1,0.5,0,0] square_y: [0,0,0.5,1,1,1,0.5] diff --git a/config/mocapLab_params.yaml b/config/mocapLab_params.yaml index eae0f55..5335a35 100644 --- a/config/mocapLab_params.yaml +++ b/config/mocapLab_params.yaml @@ -6,15 +6,18 @@ wait_time: 30 # parameter which can be set to run desired tests at a desired tim drone_mass: 0.602 #PLOAD MASSES -#pload_mass: 0.15 # Pload mass with 100g weight -pload_mass: 0.10 # Pload mass with 50g weight -#pload_mass: 0.05 # Pload mass with just basket #pload_mass: 0.25 +#pload_mass: 0.15 # Pload mass with 100g weight +#pload_mass: 0.10 # Pload mass with 50g weight +#pload_mass: 0.05 # Pload mass with just basket +pload_mass: 0.01 # No Pload +pload: false # CTRL PARAMETER - should be false to start always -use_ctrl: false # starts PX4 without attitude controller +# use_ctrl: false # starts PX4 without attitude controller +change_mode: false # choose whether to switch to oscillation damping controller -waypoints: {x: 0.0, y: 0.0, z: 1.75} # takeoff waypoints +waypoints: {x: -1.0, y: -1.0, z: 2.0} # takeoff waypoints # sets waypoints to run a square test square_x: [0.5,1,1,1,0.5,0,0] @@ -22,6 +25,6 @@ square_y: [0,0,0.5,1,1,1,0.5] # HOVER THROTTLE - Changes depending on mass of pload and drone # hover_throttle: 0.32 # Hover throttle with pload 0.15 kg -hover_throttle: 0.28 # Hover throttle with pload 0.10 kg -# hover_throttle: 0.22 # Hover throttle with no pload +#hover_throttle: 0.28 # Hover throttle with pload 0.10 kg +hover_throttle: 0.23 # Hover throttle with no pload - about 0.23 with full battery #hover_throttle: 0.26 # Hover throttle with pload 0.05 kg diff --git a/config/spiri_params.yaml b/config/spiri_params.yaml index 7255a5c..ea97aee 100644 --- a/config/spiri_params.yaml +++ b/config/spiri_params.yaml @@ -2,6 +2,7 @@ wait_time: 30 # parameter which can be set to run desired tests at a desired time drone_mass: 1.437 # mass of drone pload_mass: 0.25 # mass of payload +pload: false use_ctrl: false # starts PX4 without attitude controller - needs to be set to false to takeoff waypoints: {x: 0.0, y: 0.0, z: 5.0} # takeoff waypoints # sets waypoints to run a square test diff --git a/launch/oscillation_damp.launch b/launch/oscillation_damp.launch index 7a8ccd6..c54a5c3 100644 --- a/launch/oscillation_damp.launch +++ b/launch/oscillation_damp.launch @@ -35,6 +35,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo pkg="oscillation_ctrl" type="klausen_control.py" name="klausenCtrl_node" + clear_params="true" /> 0.01 else False - # Check if tether was correctly detected + if rospy.get_param('status/pload', False): + self.tetherL = self.get_tether() + self.tether = True + else: + self.tetherL = 0.0 + self.tether = False + + # Check if tether was correctly detected or not self.tether_check() # Retrieve drone and payload masses from config file @@ -88,18 +151,19 @@ class Main: self.a45_0 = np.zeros(2) self.alpha = np.zeros([5,1]) self.alphadot = np.zeros([5,1]) - self.a45 = self.alpha[3:5] + self.a45 = np.zeros(2) self.a45dot = np.array([[0],[0]]) # Error terms self.z1 = np.zeros([3,1]) # dr_pos - ref_sig_pos self.z2 = np.zeros([5,1]) # [vx;vy;vz;thetadot;phidot] - alpha + self.z_sum = np.zeros([3,1]) # Tuning gains - self.K1 = np.identity(3) - self.K2 = np.identity(5) - self.tune = 0.1 # Tuning parameter - self.dist = np.array([0,0,0,0.1,0.1]) # Wind disturbance + self.K1 = np.identity(3)*0.75 # 0.75 + self.K2 = np.identity(5)*0.35 # 0.35 + self.tune = 1 # Tuning parameter 1 + self.dist = np.array([0,0,0,0.1,0.1]) # Wind disturbance # Gain terms self.Kp = np.identity(3) + np.dot(self.K2[:3,:3],self.K1) + self.tune*np.identity(3) self.Kd = self.tot_m*self.K1 + self.tune*self.K2[:3,:3] @@ -107,9 +171,12 @@ class Main: # PD Thrust Controller terms # gains for thrust PD Controller - self.Kp_thrust = 1.5 - self.Kd_thrust = 1.0 + ### Failed when both gains were set to 1.0 + ##### Works better when Kp_thrust is higher? Try 10 + self.Kp_thrust = 3.5 # 1.5 for lab # 2.5 for gazebo + self.Kd_thrust = 2.5 # 1.5 for lab # 2.5 for gazebo 2.0 if no tether? self.R = np.empty([3,3]) # rotation matrix + self.g = np.array([0,0,9.81]).reshape(3,1) self.e3 = np.array([[0],[0],[1]]) # Get scaling thrust factor, kf self.kf = self.get_kf() @@ -124,15 +191,16 @@ class Main: 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) - # --------------------------------------------------------------------------------# # PUBLISHERS # --------------------------------------------------------------------------------# - self.pub_att_targ = rospy.Publisher('/command/att_target',AttitudeTarget,queue_size = 10) # timer(s), used to control method loop freq(s) as defined by the rate(s) - self.pub_time = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) #f this was 5.0 rate before + self.pub_time = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) # this was 5.0 rate before + + self.throttle_timer = rospy.Timer(rospy.Duration(2.0/rate), self.determine_throttle) # this was 5.0 rate before + # --------------------------------------------------------------------------------# # SETUP PARAM METHODS @@ -142,6 +210,7 @@ class Main: """ Get tether length based on set parameters""" param_exists = False while param_exists == False: + rospy.loginfo('Waiting for tether length...') if rospy.has_param('status/tether_length'): tether_length = rospy.get_param('status/tether_length') # Tether length rospy.loginfo('TETHER LENGTH IN CONFG FILE') @@ -167,6 +236,7 @@ class Main: if self.tether: param_exists = False while param_exists == False: + rospy.loginfo('Waiting for pload mass...') if rospy.has_param('status/pload_mass'): pl_m = rospy.get_param('status/pload_mass') # wait time rospy.loginfo('PLOAD MASS FOUND') @@ -183,11 +253,23 @@ class Main: def get_kf(self): if rospy.has_param('status/hover_throttle'): + + rospy.loginfo('total mass: %.3f',self.tot_m) + hover_throttle = rospy.get_param('status/hover_throttle') + rospy.loginfo('Using hover throttle from config %.3f',hover_throttle) + self.max_throttle = 0.5 else: - hover_throttle = (self.tot_m*9.81 + 11.2)/34.84 # linear scaling depending on dependent on mass + + rospy.loginfo('total mass: %.3f',self.tot_m) + + hover_throttle = (self.tot_m*9.81 + 9.81)/34.84 # linear scaling dependent on mass + rospy.set_param('status/hover_throttle',hover_throttle) + rospy.loginfo('Determined hover throttle to be %.3f',hover_throttle) + self.max_throttle = 0.8 kf = hover_throttle/(self.tot_m*9.81) + rospy.set_param('status/motor_constant',kf) return kf # Check if vehicle has tether @@ -207,44 +289,52 @@ class Main: self.load_angles = msg # Populate self.PHI self.PHI = np.array([[self.load_angles.theta,self.load_angles.phi],[self.load_angles.thetadot,self.load_angles.phidot]]) - except ValueError: - pass + + # self.PHI = np.array([[0.0,0.0],[0.0,0.0]]) + + # print('PHI:\n{0}'.format(self.PHI)) + # print('Angles:\n{0}'.format(self.PHI[0,:])) + # print('Ang_dot:\n{0}'.format(self.PHI[1,:])) + + except ValueError as e: + rospy.loginfo('Load angles callback failed due to: {0}'.format(e)) # Callback drone pose def dronePos_cb(self,msg): + """ Subscribed to /mavros/local_position/pose, gets PoseStamped msgs """ try: self.dr_pos = msg.pose self.EulerPose = self.convert2eul(self.dr_pos.orientation) # self.dr_pos = msg.drone_pos - except ValueError: - pass + except ValueError as e: + rospy.loginfo('Drone pos callback failed due to: {0}'.format(e)) - # Callback for drone velocity ####### IS THIS ON? ########## + # Callback for drone velocity def droneVel_cb(self,msg): try: - self.dr_vel = np.array([[msg.twist.linear.x],[msg.twist.linear.y],[msg.twist.linear.z]]) + self.dr_vel = np.array([msg.twist.linear.x,msg.twist.linear.y,msg.twist.linear.z]).reshape(3,1) self.dr_angVel = np.array([[msg.twist.angular.x],[msg.twist.angular.y],[msg.twist.angular.z]]) - except ValueError or TypeError: - pass + except (ValueError or TypeError) as e: + rospy.loginfo('Drone vel callback failed due to: {0}'.format(e)) # Callback for drone accel from IMU data def droneAcc_cb(self,msg): try: self.dr_acc = np.array([[msg.linear_acceleration.x],[msg.linear_acceleration.y],[msg.linear_acceleration.z]]) - except ValueError: - pass + except ValueError as e: + rospy.loginfo('Drone accel. callback failed due to: {0}'.format(e)) # Callback reference signal def refsig_cb(self,msg): try: self.path_pos = np.array([[msg.position.x],[msg.position.y],[msg.position.z]]) self.path_vel = np.array([[msg.velocity.x],[msg.velocity.y],[msg.velocity.z]]) - self.path_acc = np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z]]) #TODO + self.path_acc = np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z]]) - except ValueError: - pass + except ValueError as e: + rospy.loginfo('Reference signal callback failed due to: {0}'.format(e)) def waypoints_srv_cb(self): if '/status/waypoint_tracker' in self.service_list: @@ -260,12 +350,15 @@ class Main: # ---------------------------------ODE SOLVER-------------------------------------# def statespace(self,y,t,Ka,Kb,Kc): # Need the statespace array: - a1,a2 = y - K = np.dot(Ka,[[a1],[a2]]) + Kb + _y = np.array(y).reshape(2,1) + # print('Ka:\n{0}\nKb:\n{1}'.format(Ka, Kb)) + + K = np.dot(Ka,_y) + Kb # Kb should be 2x1 # Derivative of statespace array: - dydt = np.dot(Kc,K) - dydt = [dydt[0][0], dydt[1][0]] # og dydt is list of arrays, need list of floats + dydt = np.dot(Kc,K) + # print('dydt:\n{0}'.format(dydt)) + dydt = [dydt[0][0], dydt[1][0]] return dydt # --------------------------------------------------------------------------------# @@ -314,7 +407,7 @@ class Main: Convers quaternion in pose message into euler angles Input - :param Q: orientatiom pose message + :param Q: orientation pose message Output :return: Array of euler angles @@ -340,26 +433,29 @@ class Main: euler = [roll,pitch,yaw] return euler - def determine_throttle(self): - # thrust as per Geometric Tracking Control of a Quadrotor UAV on SE(3) - # Taeyoung Lee, Melvin Leok, and N. Harris McClamroch - self.waypoints_srv_cb() - self.R = self.quaternion_rotation_matrix() + def determine_throttle(self,throttle_timer): + """ Thrust as per Geometric Tracking Control of a Quadrotor UAV on SE(3) Taeyoung Lee, Melvin Leok, and N. Harris McClamroch""" + + self.waypoints_srv_cb() # check for new waypoints + self.R = self.quaternion_rotation_matrix() # get rotation matrix + b3 = self.R.dot(np.array([0,0,1]).reshape(3,1)) + self.error = np.array([ [self.path_pos[0] - self.dr_pos.position.x], [self.path_pos[1] - self.dr_pos.position.y], [self.path_pos[2] - self.dr_pos.position.z]]).reshape(3,1) - self.error_vel = self.path_vel - self.R.dot(self.dr_vel) + + self.error_vel = self.path_vel.reshape(3,1) - self.dr_vel # best performance - # determine Rotation Matrix - self.R_e3 = np.array([[self.R.T[2][0]],[self.R.T[2][1]],[self.R.T[2][2]]]) + thrust_vector = self.tot_m*self.g + self.Kp_thrust*self.error + self.Kd_thrust*self.error_vel + self.tot_m*self.path_acc + # thrust_vector = (self.g*self.tot_m + self.Kp_thrust*self.error + self.Kd_thrust*self.error_vel) * self.kf - thrust_vector = (9.81*self.tot_m + self.Kp_thrust*self.error[2] + self.Kd_thrust*self.error_vel[2] - self.tot_m*self.path_acc[2])*self.kf - thrust = thrust_vector/(math.cos(self.EulerPose[0])*math.cos(self.EulerPose[1])) + # thrust = thrust_vector/(math.cos(self.EulerPose[0])*math.cos(self.EulerPose[1])) + thrust = (thrust_vector[0] * b3[0] + thrust_vector[1] * b3[1] + thrust_vector[2] * b3[2]) * self.kf # best performance # Value needs to be between 0 - 1.0 - self.att_targ.thrust = max(0.0,min(thrust,1.0)) - + self.att_targ.thrust = max(0.0,min(thrust,self.max_throttle)) # will never want it to be more than 0.5 with LabDrone + # print('thrust_vector:\n{0}\nthrust:{1}'.format(thrust_vector, self.att_targ.thrust)) def determine_q(self): """ Determine attitude commands based on feedback and feedforward methods""" @@ -378,13 +474,13 @@ class Main: M = [[self.tot_m, 0, 0, 0, L*self.pl_m*c_theta], [0, self.tot_m, 0, -L*self.pl_m*c_phi*c_theta, L*self.pl_m*s_phi*s_theta], [0, 0, self.tot_m, -L*self.pl_m*c_theta*s_phi, -L*self.pl_m*c_phi*s_theta], - [0, -L*self.pl_m*c_phi*c_theta,-L*self.pl_m*c_theta*s_phi, (L**2)*self.pl_m*c_theta**2 + 0.01*s_theta**2, 0], + [0, -L*self.pl_m*c_phi*c_theta,-L*self.pl_m*c_theta*s_phi, (L**2)*self.pl_m*c_theta**2 + 0.001*s_theta**2, 0], [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.load_angles.thetadot*self.pl_m*s_theta], [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.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.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.load_angles.thetadot*math.sin(2*self.load_angles.theta) + 0.5*0.001*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.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]] @@ -407,60 +503,81 @@ class Main: M_b = M[3:5,:3] # M_4:5,1:3 - rows 4 to 5 and columns 1 to 3 M_c = M[:3,3:5] # M_1:3,4:5 - rows 1 to 3 and columns 4 to 5 C_c = C[:3,3:5] - - # Constants from Eq. 49 - Ka = -(D_a + C_a + self.K2[3:5,3:5]) - Kb = -G_a + np.dot(self.K2[3:5,3:5],self.PHI[:,1]) - np.dot(M_b,self.path_acc - np.dot(self.K1,self.dr_vel - self.path_vel)) - + # Determine alpha - if self.tether: + if self.tether: + # Constants from Eq. 49 + Ka = -(D_a + C_a + self.K2[3:5,3:5]) + Kb = -G_a + np.dot(self.K2[3:5,3:5],self.PHI[1,:].reshape(2,1)) - np.dot(M_b,self.path_acc - np.dot(self.K1,self.dr_vel - self.path_vel)) + + #TODO divide by M_a after finding a45_buff + M_aI = np.linalg.inv(M_a) # Inverse of M_a + # print('M_aI was found to be:\n{0}\n'.format(M_aI)) # SOLVE ODE (get alpha) - # Populate buffer - self.a45_buff = odeint(self.statespace,self.a45_0,self.t,args=(Ka,Kb,M_aI)) + # Populate buffer + self.a45_buff = odeint(self.statespace,self.a45,self.t,args=(Ka,Kb,M_aI)) + # self.a45_buff = odeint(self.statespace,self.a45,self.t,args=(Ka,Kb,np.identity(2))) # spits out a 2,2 matrix + # print('a45_0 was found to be:\n{0}\na45 was found to be:\n{1}\n'.format(self.a45_0,self.a45)) # Update a45_0 to new a45. Need to transpose to column vector - self.a45_0 = self.a45_buff[-1,:] - self.a45 = np.array([[self.a45_0[0]],[self.a45_0[1]]]) + self.a45_0 = self.a45_buff[0,:] + self.a45 = self.a45_buff[-1,:] + # self.a45 = M_aI.dot(self.a45_buff[-1,:]) + + # print('a45_0 was found to be {0}'.format(self.a45_0)) # Get alphadot_4:5 - self.a45dot = self.statespace(self.a45_0,1,Ka,Kb,M_aI) # Do not need 't' and that's why it is a 1 - self.a45dot = np.array([[self.a45dot[0]],[self.a45dot[1]]]) + self.a45dot = np.array(self.statespace(self.a45,1,Ka,Kb,M_aI)) # Do not need 't' and that's why it is a 1 + # self.a45dot_buff = self.statespace(self.a45,1,Ka,Kb,np.identity(2)) # Do not need 't' and that's why it is a 1 + # self.a45dot = M_aI.dot(np.array([[self.a45dot_buff[0]],[self.a45dot_buff[1]]])) else: # if no tether, alpha_4:5 = 0 self.a45 = np.array([[0],[0]]) self.a45dot = np.array([[0],[0]]) - # Determine a_1:3 - self.alpha[:3] = self.path_vel - np.dot(self.K1,p - self.path_pos) - self.alpha[3:5] = self.a45 - # populate error terms - self.z1 = p - self.path_pos - z1_dot = self.dr_vel - self.path_vel + self.z1 = p.reshape(3,1) - self.path_pos.reshape(3,1) + # z1_dot = self.dr_vel - self.path_vel + z1_dot = self.R.dot(-self.path_vel) + self.dr_vel self.z2 = g - self.alpha - B = np.dot(C_c,self.a45) + np.dot(M_c,self.a45dot) + # Determine alpha + self.alpha[:3] = self.path_vel - np.dot(self.K1,self.z1) + self.alpha[3:5] = self.a45.reshape(2,1) + # determine stability factor + B = np.dot(C_c,self.a45.reshape(2,1)) + np.dot(M_c,self.a45dot.reshape(2,1)) dr_orientation = [self.dr_pos.orientation.x, self.dr_pos.orientation.y, self.dr_pos.orientation.z, self.dr_pos.orientation.w] dr_orientation_inv = quaternion_inverse(dr_orientation) + if rospy.get_param('status/use_ctrl', False): + z_int = self.z_sum + self.z1_p * self.dt + 0.5 * (self.z1 - self.z1_p) * self.dt + else: + z_int = np.zeros(3).reshape(3,1) + + # path_acc = self.R.dot(self.path_acc) + path_acc = np.array([self.path_acc[0],self.path_acc[1],self.path_acc[2] + 9.81]).reshape(3,1) # need to add gravity for controller to work + # Desired body-oriented forces - Fd = B + G[:3] + self.tot_m*self.dr_acc - np.dot(self.Kd,z1_dot) - np.dot(self.Kp,self.z1) - np.dot(self.Ki,0.5*self.dt*(self.z1 - self.z1_p)) + Fd = B + G[:3] + self.tot_m*path_acc - np.dot(self.Kd,z1_dot) - np.dot(self.Kp,self.z1) - np.dot(self.Ki,z_int) + # rospy.loginfo('\nFd:\n{0}'.format(Fd)) # Update self.z1_p for integration self.z1_p = self.z1 + self.z_sum = z_int - # Covert Fd into drone frame - Fd_tf = Fd + # Covert Fd into drone frame, do not need Fz since we have a seperate altitude controller + # Fd_tf = quaternion_multiply(dr_orientation,quaternion_multiply([Fd[0],Fd[1],Fd[2],0.0],dr_orientation_inv)) # Real part of Fd needs = 0 + Fd_tf = quaternion_multiply(dr_orientation,quaternion_multiply([Fd[0],Fd[1],0.0,0.0],dr_orientation_inv)) # Real part of Fd needs = 0 - Fd_tf = quaternion_multiply(dr_orientation,quaternion_multiply([Fd[0],Fd[1],Fd[2],0.0],dr_orientation_inv)) # Real part of Fd needs = 0 + # rospy.loginfo('\nFd_tf:\n{0}'.format(Fd_tf)) # Convert forces to attiude *EulerAng[2] = yaw = 0 - self.EulerAng[1] = math.atan(Fd_tf[0]/(self.drone_m*9.81)) # Pitch - self.EulerAng[0] = math.atan(-Fd_tf[1]*math.cos(self.EulerAng[1])/(self.drone_m*9.81)) # Roll + self.EulerAng[1] = -math.atan(-Fd_tf[0]/(self.tot_m*9.81)) # Pitch + self.EulerAng[0] = -math.atan(Fd_tf[1]*math.cos(self.EulerAng[1])/(self.tot_m*9.81)) # Roll q = quaternion_from_euler(self.EulerAng[0],self.EulerAng[1],self.EulerAng[2]) @@ -470,11 +587,14 @@ class Main: # Attitude control self.att_targ.header.stamp = rospy.Time.now() self.att_targ.header.frame_id = '/map' - self.att_targ.type_mask = 1|2|4 + self.att_targ.type_mask = 1|2|4 # ignore body rate command self.att_targ.orientation.x = q[0] self.att_targ.orientation.y = q[1] self.att_targ.orientation.z = q[2] self.att_targ.orientation.w = q[3] + self.att_targ.body_rate.x = 0.0 + self.att_targ.body_rate.y = 0.0 + self.att_targ.body_rate.z = 0.0 def user_feedback(self,F_noTransform, F_Transform): print('\n') @@ -484,8 +604,8 @@ class Main: rospy.loginfo('Fd after transform: %.2f, %.2f, %.2f', F_Transform[0],F_Transform[1],F_Transform[2]) def publisher(self,pub_time): + # self.determine_throttle() self.determine_q() - self.determine_throttle() self.pub_att_targ.publish(self.att_targ) # --------------------------------------------------------------------------------# diff --git a/src/path_follow.cpp b/src/path_follow.cpp index 7afb312..41e93f9 100644 --- a/src/path_follow.cpp +++ b/src/path_follow.cpp @@ -37,25 +37,11 @@ void att_targ_cb(const mavros_msgs::AttitudeTarget::ConstPtr& msg){ // Cb to recieve pose information geometry_msgs::PoseStamped buff_pose; -geometry_msgs::Quaternion q_init; geometry_msgs::PoseStamped mavPose; -bool gps_read = false; double current_altitude; void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ mavPose = *msg; current_altitude = mavPose.pose.position.z; - // while(gps_read == false){ - // q_init = mavPose.pose.orientation; - // if(q_init.x == 0.0 && q_init.w == 0.0){ - // ROS_INFO("Waiting..."); - // } else { - // buff_pose.pose.orientation.x = q_init.x; - // buff_pose.pose.orientation.y = q_init.y; - // buff_pose.pose.orientation.z = q_init.z; - // buff_pose.pose.orientation.w = q_init.w; - // gps_read = true; - // } - // } } int main(int argc, char **argv) @@ -90,8 +76,6 @@ int main(int argc, char **argv) ("mavros/cmd/arming"); ros::ServiceClient set_mode_client = nh.serviceClient ("mavros/set_mode"); - ros::ServiceClient takeoff_cl = nh.serviceClient - ("mavros/cmd/takeoff"); ros::ServiceClient waypoint_cl = nh.serviceClient ("status/waypoint_tracker"); diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py index a3d71e0..380902d 100755 --- a/src/ref_signalGen.py +++ b/src/ref_signalGen.py @@ -17,9 +17,14 @@ from sensor_msgs.msg import Imu class DesiredPoint(): def __init__(self,x,y,z): - self.x = x - self.y = y - self.z = z + self.point = Point() + self.point.x = x + self.point.y = y + self.point.z = z + self.return_xd() + + def return_xd(self): + return self.point class Main: @@ -44,12 +49,15 @@ class Main: self.ref_sig = RefSignal() # Smooth Signal self.load_angles = LoadAngles() - self.has_run = 0 # Bool to keep track of first run instance self.dr_pos = Pose() self.dr_vel = self.vel_data.twist.linear self.dr_acc = self.imu_data.linear_acceleration - self.tetherL = self.get_tether() + if rospy.get_param('status/pload'): + self.tetherL = self.get_tether() + else: self.tetherL = 0.0 + rospy.loginfo('tether length: {0}'.format(self.tetherL)) + # --------------------------------------------------------------------------------# # SUBSCRIBERS # # --------------------------------------------------------------------------------# @@ -78,7 +86,7 @@ class Main: self.EPS_I = np.zeros(9) # Epsilon shapefilter # Constants for smooth path generation - self.w_tune = 1 # Increase this to increase aggresiveness of trajectory i.e. higher accelerations + self.w_tune = 1.0 # 0.5 for Gaz. Increase this to increase aggresiveness of trajectory i.e. higher accelerations self.epsilon = 0.7 # Damping ratio # need exception if we do not have tether: @@ -86,16 +94,16 @@ class Main: self.wn = self.w_tune else: self.wn = math.sqrt(9.81/self.tetherL) - # self.wn = 7 + # self.wn = 1.51 # 1.01 is the imperically determined nat freq in gazebo self.wd = self.wn*math.sqrt(1 - self.epsilon**2) self.k4 = 4*self.epsilon*self.w_tune self.k3 = ((2 + 4*self.epsilon**2)*self.w_tune**2)/self.k4 self.k2 = (4*self.epsilon*self.w_tune**3)/(self.k4*self.k3) - self.k1 = (self.w_tune**4)/(self.k2*self.k3*self.k4) + self.k1 = (self.w_tune**4)/(self.k4*self.k3*self.k2) # For saturation: - self.max = [0, 3, 1.5, 3] #[0, 5, 1.5, 3] + self.max = [0, 5, 2.5, 3] #[0, 3, 1.5, 3] - lab testing self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack) self.empty_point = Point() # Needed to query waypoint_server @@ -119,8 +127,8 @@ class Main: self.A2 = self.A1*self.K # Need to determine how large of any array needs to be stored to use delayed functions - self.SF_delay_x = np.zeros([4,int(round(self.t2/self.dt))]) # Shapefilter delay; 4 - p,v,a,j - self.SF_delay_y = np.zeros([4,int(round(self.t2/self.dt))]) + self.SF_delay_x = np.zeros([3,int(round(self.t2/self.dt))]) # Shapefilter delay; 4 - p,v,a since we do not need j + self.SF_delay_y = np.zeros([3,int(round(self.t2/self.dt))]) self.phi_fb = np.zeros(int(round(self.td/self.dt))) # Feedback delay self.phi_vel_fb = np.zeros(int(round(self.td/self.dt))) @@ -196,7 +204,7 @@ class Main: if rospy.has_param('status/tether_length'): tether_length = rospy.get_param('status/tether_length') # Tether length self.param_exists = True - elif rospy.get_time() - self.tstart >= 15: + elif rospy.get_time() - self.tstart >= 3: tether_length = 0.0 break return tether_length @@ -214,11 +222,11 @@ class Main: pos,vel,acc,jer = y error = xd - pos - if abs(error) <= 0.01: error = 0.0 + # if abs(error) <= 0.2: error = 0.0 # works well without saturation # Derivative of statesape array: - dydt = [vel, acc, jer, - self.k4*(self.k3*(self.k2*(self.k1*(error) - vel) - acc) - jer)] + dydt = [vel, acc, jer, self.k4*(self.k3*(self.k2*(self.k1*(error) - vel) - acc) - jer)] + return dydt # Sigmoid @@ -261,11 +269,11 @@ class Main: # First, fill up the delay array 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.theta_acc_fb[self.FB_idx] = (self.load_angles.thetadot - self.theta_vel_fb[self.FB_idx-1])/self.dt 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] + self.phi_acc_fb[self.FB_idx] = (self.load_angles.phidot - self.phi_vel_fb[self.FB_idx-1])/self.dt else: # once array is filled, need to shift values w/ latest value at end @@ -277,13 +285,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.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.theta_fb[-1] = self.load_angles.theta # change final value + self.theta_vel_fb[-1] = self.load_angles.thetadot + self.theta_acc_fb[-1] = (self.load_angles.thetadot - self.theta_vel_fb[-1])/self.dt - 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] + self.phi_fb[-1] = self.load_angles.phi + self.phi_vel_fb[-1] = self.load_angles.phidot + self.phi_acc_fb[-1] = (self.load_angles.phidot - self.phi_vel_fb[-1])/self.dt else: print('No delay') @@ -308,6 +316,7 @@ class Main: self.y[:,i] = np.clip(self.y[:,i], a_min = -self.max[i], a_max = self.max[i]) self.z[:,i] = np.clip(self.z[:,i], a_min = -self.max[i], a_max = self.max[i]) + # convolution for j in range(3): # 3 is due to pos, vel, acc. NOT due x, y, z self.delay(j,SHAPEFIL) # Determine the delay (shapefilter) array @@ -319,18 +328,18 @@ class Main: else: self.EPS_I[3*j] = self.A1*self.x[-1,j] + self.A2*self.SF_delay_x[j,0] # Determine convolution (x) - self.EPS_I[3*j+1] = self.A1*self.y[-1,j] + self.A2*self.SF_delay_y[j,0] # Determine convolution (y) + self.EPS_I[3*j+1] = self.A1*self.y[-1,j] + self.A2*self.SF_delay_y[j,0] # Determine convolution (y) + self.delay(1,FEEDBACK) # Detemine feedback array self.sigmoid() # Determine sigmoid gain - EPS_D = self.fback() # Feedback Epsilon + Eps_D = self.fback() # Feedback Epsilon - self.EPS_F = self.EPS_I + self.s_gain*EPS_D + self.EPS_F = self.EPS_I + self.s_gain*Eps_D # Populate msg with epsilon_final self.ref_sig.header.stamp = rospy.Time.now() - #self.ref_sig.type_mask = 2 # Need typemask = 2 to use correct attitude controller - Jaeyoung Lin self.ref_sig.position.x = self.EPS_F[0] self.ref_sig.position.y = self.EPS_F[1] self.ref_sig.position.z = self.EPS_F[2] @@ -341,9 +350,10 @@ class Main: self.ref_sig.acceleration.y = self.EPS_F[7] self.ref_sig.acceleration.z = self.EPS_F[8] + # update initial states self.x0 = [self.x[1,0], self.x[1,1], self.x[1,2], self.x[1,3]] self.y0 = [self.y[1,0], self.y[1,1], self.y[1,2], self.y[1,3]] - self.z0 = [self.z[1,0], self.z[1,1], self.z[1,2], self.z[1,3]] + self.z0 = [self.z[1,0], self.z[1,1], self.z[1,2], self.z[1,3]] self.SF_idx += 1 self.FB_idx += 1 @@ -360,9 +370,9 @@ class Main: ydotr = -self.Gd*self.tetherL*math.cos(self.phi_fb[0])*self.phi_vel_fb[0] yddotr = self.Gd*self.tetherL*math.sin(self.phi_fb[0])*self.phi_vel_fb[0]**2 + math.cos(self.phi_fb[0])*self.phi_acc_fb[0] - EPS_D = np.array([xr,yr,0,xdotr,ydotr,0,xddotr,yddotr,0]) + Eps_D = np.array([xr,yr,0,xdotr,ydotr,0,xddotr,yddotr,0]) - return EPS_D + return Eps_D def user_feeback(self): @@ -376,17 +386,20 @@ class Main: def publisher(self,pub_tim): - # Determine final ref signal - self.convo() + # Determine final ref signal + try: + self.convo() - # Publish reference signal - self.pub_path.publish(self.ref_sig) + # Publish reference signal + self.pub_path.publish(self.ref_sig) - # Publish what the setpoints are - self.waypointTracker_pub.publish(self.xd) + # Publish what the setpoints are + self.waypointTracker_pub.publish(self.xd) - # Give user feedback on published message: - self.user_feeback() + # Give user feedback on published message: + self.user_feeback() + except AttributeError: + pass # catches case at beginning when self.xd is not properly initialized if __name__=="__main__":