From 53895d5bf31f9991999122e41d03f64c594aac2d Mon Sep 17 00:00:00 2001 From: cesar Date: Mon, 6 Jun 2022 17:15:33 -0300 Subject: [PATCH] updating to work from home --- .gitignore | 2 +- config/Ctrl_param.yaml | 2 +- config/px4_pluginlists.yaml | 4 +- launch/cortex_bridge.launch | 14 +++- launch/headless_spiri.launch | 54 +++++++++++++++ launch/headless_spiri_with_tether.launch | 54 +++++++++++++++ launch/offboard.launch | 7 -- launch/oscillation_damp.launch | 6 +- launch/path_follow.launch | 26 ------- launch/px4.launch | 8 ++- launch/spiri.launch | 55 +++++++++++++++ launch/spiri_with_tether.launch | 54 +++++++++++++++ src/klausen_control.py | 10 +-- src/ref_signalGen.py | 6 +- src/square.py | 88 ------------------------ src/step.py | 62 ----------------- src/waypoint_determine.py | 2 +- 17 files changed, 252 insertions(+), 202 deletions(-) create mode 100644 launch/headless_spiri.launch create mode 100644 launch/headless_spiri_with_tether.launch delete mode 100644 launch/offboard.launch delete mode 100644 launch/path_follow.launch create mode 100644 launch/spiri.launch create mode 100644 launch/spiri_with_tether.launch delete mode 100755 src/square.py delete mode 100755 src/step.py diff --git a/.gitignore b/.gitignore index 8e75294..25d96a3 100644 --- a/.gitignore +++ b/.gitignore @@ -3,10 +3,10 @@ launch/mocap_* launch/cortex_bridge.launch src/MoCap_Localization_*.py src/Mocap_*.py +src/segmented_tether.py src/killswitch_client.py src/land_client.py msg/Marker.msg msg/Markers.msg -launc *.rviz diff --git a/config/Ctrl_param.yaml b/config/Ctrl_param.yaml index c2ac39d..30358c6 100644 --- a/config/Ctrl_param.yaml +++ b/config/Ctrl_param.yaml @@ -1,5 +1,5 @@ # Ros param when using Klausen Ctrl -wait: 52 +wait: 40 waypoints: {x: 0.0, y: 0.0, z: 5.0} diff --git a/config/px4_pluginlists.yaml b/config/px4_pluginlists.yaml index 4402a42..27c4041 100644 --- a/config/px4_pluginlists.yaml +++ b/config/px4_pluginlists.yaml @@ -1,5 +1,6 @@ plugin_blacklist: # common +#- distance_sensor - safety_area # extras - image_pub @@ -12,5 +13,6 @@ plugin_blacklist: - odometry - px4flow -plugin_whitelist: [] +plugin_whitelist: [] #- 'sys_*' + diff --git a/launch/cortex_bridge.launch b/launch/cortex_bridge.launch index c3896a6..09a685a 100644 --- a/launch/cortex_bridge.launch +++ b/launch/cortex_bridge.launch @@ -11,6 +11,14 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo + + + + + + + + - + + + - diff --git a/launch/headless_spiri.launch b/launch/headless_spiri.launch new file mode 100644 index 0000000..f2dba89 --- /dev/null +++ b/launch/headless_spiri.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/headless_spiri_with_tether.launch b/launch/headless_spiri_with_tether.launch new file mode 100644 index 0000000..1c0bb7e --- /dev/null +++ b/launch/headless_spiri_with_tether.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/offboard.launch b/launch/offboard.launch deleted file mode 100644 index 2512d05..0000000 --- a/launch/offboard.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/launch/oscillation_damp.launch b/launch/oscillation_damp.launch index 07c0af2..0e5662d 100644 --- a/launch/oscillation_damp.launch +++ b/launch/oscillation_damp.launch @@ -4,7 +4,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo /--> - + @@ -14,6 +14,8 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo + + - + diff --git a/launch/path_follow.launch b/launch/path_follow.launch deleted file mode 100644 index 50f1d1d..0000000 --- a/launch/path_follow.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - diff --git a/launch/px4.launch b/launch/px4.launch index 777c00c..c182acd 100644 --- a/launch/px4.launch +++ b/launch/px4.launch @@ -8,10 +8,14 @@ + + + + - - + + diff --git a/launch/spiri.launch b/launch/spiri.launch new file mode 100644 index 0000000..fa046dd --- /dev/null +++ b/launch/spiri.launch @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/spiri_with_tether.launch b/launch/spiri_with_tether.launch new file mode 100644 index 0000000..4da8c16 --- /dev/null +++ b/launch/spiri_with_tether.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/klausen_control.py b/src/klausen_control.py index 13a88a3..3b905d4 100755 --- a/src/klausen_control.py +++ b/src/klausen_control.py @@ -103,13 +103,13 @@ class Main: # gains for thrust PD Controller #self.Kp = 2.7 #self.Kd = 3 - self.Kp = 2.7 + self.Kp = 3.0 self.Kd = 3 self.max_a = 14.2 - self.max_t = self.drone_m*self.max_a + self.max_t = self.tot_m*self.max_a self.R = np.empty([3,3]) # rotation matrix self.e3 = np.array([[0],[0],[1]]) - self.thrust_offset = 0.7 # There was found to be a constant offset + self.thrust_offset = 1.0 # There was found to be a constant offset 0.7 # --------------------------------------------------------------------------------# # SUBSCRIBERS # --------------------------------------------------------------------------------# @@ -266,7 +266,7 @@ class Main: #thrust = np.dot(np.dot(9.81*self.drone_m,self.e3) - self.Kp*self.error - self.Kd*self.dr_vel,self.R.T[2])/self.max_t #thrust_vector = np.dot(9.81*self.drone_m*self.e3 - self.Kp*self.error - self.Kd*self.dr_vel,self.R_e3)/self.max_t #thrust_vector = (9.81*self.drone_m*self.e3 - self.Kp*self.error - self.Kd*self.dr_vel -9.81*self.drone_m*self.path_acc)/self.max_t - thrust_vector = (9.81*self.drone_m - self.Kp*self.error[2] - self.Kd*self.dr_vel[2])/self.max_t + thrust_vector = (9.81*self.tot_m - self.Kp*self.error[2] - self.Kd*self.dr_vel[2])/self.max_t #thrust = thrust_vector[0]*self.R_e3[0] + thrust_vector[1]*self.R_e3[1] + thrust_vector[2]*self.R_e3[2] thrust = thrust_vector/(math.cos(self.EulerAng[0]*self.EulerAng[1])) #thrust = thrust_vector[2] @@ -370,7 +370,7 @@ class Main: # Desired body-oriented forces # shouldnt it be tot_m*path_acc? - Fd = B + G[:3] + self.tot_m*self.dr_acc - np.dot(Kd,z1_dot) - np.dot(Kp,self.z1) - np.dot(Ki,self.dt*(self.z1 - self.z1_p)) + Fd = B + G[:3] + self.tot_m*self.dr_acc - np.dot(Kd,z1_dot) - np.dot(Kp,self.z1) - np.dot(Ki,0.5*self.dt*(self.z1 - self.z1_p)) # Update self.z1_p for "integration" self.z1_p = self.z1 diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py index 8a8ae7d..bfdfc6a 100755 --- a/src/ref_signalGen.py +++ b/src/ref_signalGen.py @@ -83,7 +83,7 @@ class Main: self.EPS_I = np.zeros(9) # Epsilon shapefilter # Constants for smooth path generation - self.w_tune = 3.13 # 3.13 works well? ######################################################################### + self.w_tune = 3 # 3.13 works well? ######################################################################### self.epsilon = 0.7 # Damping ratio # need exception if we do not have tether: @@ -117,7 +117,7 @@ class Main: self.z0 = [0, 0, 0, 0] # Constants for feedback - self.Gd = 0.325 + self.Gd = 0.325 #0.325 self.td = 2*self.Gd*math.pi/self.wd # Constants for shape filter/ Input shaping @@ -258,7 +258,7 @@ class Main: 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.load_angles.phi + 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] diff --git a/src/square.py b/src/square.py deleted file mode 100755 index 88af1eb..0000000 --- a/src/square.py +++ /dev/null @@ -1,88 +0,0 @@ -#!/usr/bin/env python2.7 - - -### Cesar Rodriguez June 2021 -### Script to generate set points which form a square with 2m side lengths -import rospy, tf -import time -from geometry_msgs.msg import Point -from std_msgs.msg import Bool - -class Main: - # class method used to initialize variables once when the program starts - def __init__(self): - - # variable(s) - self.Point = Point() - # Init x, y, & z coordinates - self.Point.x = 0 - self.Point.y = 0 - self.Point.z = 3.5 - - self.xarray = [1,2,2,2,1,0,0] - self.yarray = [0,0,1,2,2,2,1] - self.i = 0 - self.j = 0 - self.buffer = 10 - self.bool = False - - self.param_exists = False # check in case param does not exist - while self.param_exists == False: - if rospy.has_param('status/test_type'): - self.test = rospy.get_param('status/test_type') - self.param_exists = True - - elif rospy.get_time() - self.tstart >= 3.0 || rospy.get_param('status/test_type') == 'none' - self.test = 'none' - break - - - # subscriber(s), with specified topic, message type, and class callback method - rospy.Subscriber('/status/path_follow',Bool, self.wait_cb) - - # publisher(s), with specified topic, message type, and queue_size - self.pub_square = rospy.Publisher('/reference/waypoints', Point, queue_size = 5) - - # rate(s) - pub_rate = 1 # rate for the publisher method, specified in Hz - - # timer(s), used to control method loop frequencies as defined by the rate(s) - self.pub_timer = rospy.Timer(rospy.Duration(1.0/pub_rate), self.pub) - - def wait_cb(self,data): - self.bool = data - - # Publish messages - def pub(self,pub_timer): - if self.bool == False: - rospy.loginfo('Waiting...') - else: - # Check if i is too large. loop back to first point - if self.i >= len(self.xarray): - self.Point.x = 0 - self.Point.y = 0 - - else: - self.Point.x = self.xarray[self.i] - self.Point.y = self.yarray[self.i] - - rospy.loginfo("Sending [Point x] %d [Point y] %d", - self.Point.x, self.Point.y) - - # Published desired msgs - # self.Point.header.stamp = rospy.Time.now() - self.pub_square.publish(self.Point) - self.j += 1 - self.i = self.j // self.buffer - -if __name__=="__main__": - - # Initiate ROS node - rospy.init_node('square',anonymous=True) - try: - Main() # create class object - rospy.spin() # loop until shutdown signal - - except rospy.ROSInterruptException: - pass - diff --git a/src/step.py b/src/step.py deleted file mode 100755 index 6a58afc..0000000 --- a/src/step.py +++ /dev/null @@ -1,62 +0,0 @@ -#!/usr/bin/env python2.7 - -### Cesar Rodriguez Dec 2021 -### Generate step input in x or y direction -import rospy, tf -import time -from geometry_msgs.msg import Point -from std_msgs.msg import Bool - -class Main: - # class method used to initialize variables once when the program starts - def __init__(self): - - # variable(s) - self.Point = Point() - # Init x, y, & z coordinates - self.Point.x = 5 - self.Point.y = 0 - self.Point.z = 3.5 # need to check what the xd was previously and save the height - - self.bool = False - - # subscriber(s) - rospy.Subscriber('/status/path_follow',Bool, self.wait_cb) - - # publisher(s), with specified topic, message type, and queue_size - self.pub_step = rospy.Publisher('/reference/waypoints', Point, queue_size = 5) - - # rate(s) - pub_rate = 1 # rate for the publisher method, specified in Hz - - # timer(s), used to control method loop frequencies as defined by the rate(s) - self.pub_timer = rospy.Timer(rospy.Duration(1.0/pub_rate), self.pub) - - # Callbacks - def wait_cb(self,data): - self.bool = data - - # Publish messages - def pub(self,pub_timer): - if self.bool == False: - rospy.loginfo('Waiting...') - else: - #self.Point.header.stamp = rospy.Time.now() - - # Published desired msgs - self.pub_step.publish(self.Point) - - rospy.loginfo("Sending [Point x] %d [Point y] %d", - self.Point.x, self.Point.y) - -if __name__=="__main__": - - # Initiate ROS node - rospy.init_node('step',anonymous=True) - try: - Main() # create class object - rospy.spin() # loop until shutdown signal - - except rospy.ROSInterruptException: - pass - diff --git a/src/waypoint_determine.py b/src/waypoint_determine.py index 70029d9..b6470a4 100755 --- a/src/waypoint_determine.py +++ b/src/waypoint_determine.py @@ -44,7 +44,7 @@ class Main: def wp_input(self): # testing: - self.lat = input('Enter lat: ') + self.lat = input('Enter lat: ') self.lon = input('Enter lon: ') self.lat = 44.6480876