Upload files to 'src'

src/ref_signalGen.py

@@ -0,0 +1,425 @@
+#!/usr/bin/env python2.7
+
+### Cesar Rodriguez July 2021
+### Based off of Klausen 2017 - Smooth trajectory generation based on desired waypoints
+
+import rospy,  tf
+import numpy as np
+import time
+import math
+
+from scipy import signal
+from scipy.integrate import odeint 
+
+from offboard_ex.msg import tethered_status, RefSignal
+
+from controller_msgs.msg import FlatTarget
+
+from geometry_msgs.msg import Pose, Vector3, PoseStamped 
+
+from geometry_msgs.msg import TwistStamped
+from sensor_msgs.msg import Imu
+
+from mavros_msgs.msg import State
+
+from pymavlink import mavutil
+
+class Main:
+
+	def __init__(self):
+
+		# rate(s)
+		rate = 30 # rate for the publisher method, specified in Hz -- 10 Hz
+
+		# initialize variables
+		self.tstart		= rospy.get_time() # Keep track of the start time
+		while self.tstart == 0.0:				 # Need to make sure get_rostime works
+			self.tstart		= rospy.get_time()
+
+		self.dt 			= 1.0/rate
+#		self.dt 			= 0.5
+		# self.tmax			= 100
+		self.tmax			= self.dt
+		self.n				= self.tmax/self.dt + 1
+		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.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
+		self.dr_acc_p = self.imu_data.linear_acceleration
+
+		# Get tether length
+		self.param_exists = False
+		while self.param_exists == False:
+			if rospy.has_param('sim/tether_length'):
+				self.tetherL	= rospy.get_param('sim/tether_length') # Tether length
+				self.param_exists = True
+
+		# Smooth path variables 
+		self.EPS_F 		= np.zeros(9)    # Final Epsilon/ signal
+		self.EPS_I 		= np.zeros(9)		 # Epsilon shapefilter
+		
+		# Constants for smooth path generation
+		self.w_tune		= 3.0 # 3.13 works well? #########################################################################
+		self.epsilon	= 0.7 								# Damping ratio
+		self.wn				= math.sqrt(9.81/self.tetherL)
+		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)
+		
+		# For saturation:
+		self.jmax 	= [3, 3, 1]
+		self.amax		= [1.5, 1.5, 1]
+		self.vmax		= [3, 3, 1]
+		self.max		=	[0, 3, 1.5, 3]  #[0, 3, 1.5, 3]
+		# create the array: [vmax; amax; jmax]
+		self.max_array = np.array([[self.vmax],[self.amax],[self.jmax]]).T
+
+ 		# Desired position array
+		self.xd = np.array([[0],[0],[5.0]])
+		self.des_waypoints = True # True = changing waypoints
+
+		# Initial conditions: [pos, vel, acc, jerk]
+		self.x0 = [0, 0, 0, 0]
+		self.y0 = [0, 0, 0, 0]
+		self.z0 = [0, 0, 0, 0]
+
+# --------------------------------------------------------------------------------#		
+# 														FEEDBACK AND INPUT SHAPING
+# --------------------------------------------------------------------------------#
+
+		# Constants for feedback
+		self.Gd = 0.325 
+		self.td = 2*self.Gd*math.pi/self.wd
+
+		# Constants for shape filter/ Input shaping
+		self.t1				= 0
+		self.t2				= math.pi/self.wd
+		self.K				= math.exp(-self.epsilon*math.pi/math.sqrt(1 - self.epsilon**2))
+		self.A1				= 1/(1 + self.K)
+		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.phi_fb     = np.zeros(int(round(self.td/self.dt))) # Feedback delay
+		self.phi_vel_fb = np.zeros(int(round(self.td/self.dt))) 
+		self.phi_acc_fb = np.zeros(int(round(self.td/self.dt)))
+
+		self.theta_fb     = np.zeros(int(round(self.td/self.dt)))
+		self.theta_vel_fb = np.zeros(int(round(self.td/self.dt)))
+		self.theta_acc_fb = np.zeros(int(round(self.td/self.dt)))
+
+		# Need index to keep track of path array of ^^^^ length
+		self.SF_idx = 0
+		self.FB_idx = 0
+
+		# Constants for sigmoid
+		self.at 			= 3 	# acceleration theshold
+		self.pc 			= 0.7 # From Klausen 2017
+		self.sk				=	len(self.SF_delay_x[0])  # from Klausen 2017
+		self.ak 			= np.zeros(len(self.SF_delay_x[0]))
+		self.s_gain 	= 0.0 # Gain found from sigmoid	
+		
+# --------------------------------------------------------------------------------#		
+# 																	SUBSCRIBERS																		#
+# --------------------------------------------------------------------------------#
+		# Topic, msg type, and class callback method
+		rospy.Subscriber('/status/twoBody_status', tethered_status, self.linkState_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)
+		rospy.Subscriber('/reference/waypoints',PoseStamped, self.waypoints_cb)
+
+# --------------------------------------------------------------------------------#		
+# 																	PUBLISHERS
+# --------------------------------------------------------------------------------#
+		# Publish desired path to compute attitudes
+		self.pub_path = rospy.Publisher('/reference/path',FlatTarget,queue_size = 10)
+		# Needed for geometric controller to compute thrust
+		self.pub_ref = rospy.Publisher('/reference/flatsetpoint',FlatTarget,queue_size = 10)
+
+		# timer(s), used to control method loop freq(s) as defined by the rate(s)
+		self.pub_tim  = rospy.Timer(rospy.Duration(1.0/rate), self.publisher)
+		
+# ------------------------------------ _init_ ends ------------------------------ # 
+
+# --------------------------------------------------------------------------------#		
+# 																CALLBACK FUNCTIONS
+# --------------------------------------------------------------------------------#
+	def state_cb(self, data):
+		if self.state.armed != data.armed:
+			rospy.loginfo("armed state changed from {0} to {1}".format(
+				self.state.armed, data.armed))
+
+		if self.state.connected != data.connected:
+			rospy.loginfo("connected changed from {0} to {1}".format(
+				self.state.connected, data.connected))
+
+		if self.state.mode != data.mode:
+			rospy.loginfo("mode changed from {0} to {1}".format(
+				self.state.mode, data.mode))
+
+		if self.state.system_status != data.system_status:
+			rospy.loginfo("system_status changed from {0} to {1}".format(
+				mavutil.mavlink.enums['MAV_STATE'][
+					self.state.system_status].name, mavutil.mavlink.enums[
+						'MAV_STATE'][data.system_status].name))
+
+		self.state = data
+    # mavros publishes a disconnected state message on init
+
+	# Callback to get link names, states, and pload deflection angles
+	def linkState_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
+							
+		except ValueError:
+			pass
+
+	# Callback for drone velocity
+	def droneVel_cb(self,msg):
+		try:
+			self.dr_vel = msg.twist.linear
+
+		except ValueError or TypeError:
+			pass
+
+	# Callback for drone accel from IMU data
+	def droneAcc_cb(self,msg):
+		try:
+			self.dr_acc = msg.linear_acceleration
+
+		except ValueError:
+			pass
+
+	def waypoints_cb(self,msg):
+		try:
+			if self.des_waypoints == True:
+				self.xd[0] = msg.pose.position.x
+				self.xd[1] = msg.pose.position.y
+				#self.xd[2] = msg.pose.position.z
+
+		except ValueError:
+			pass
+# --------------------------------------------------------------------------------#		
+# 																			ALGORITHM
+# --------------------------------------------------------------------------------#
+# --------------------------------------------------------------------------------#		
+# 									TRAJECTORY GENERATION BASED ON WAYPONTS xd
+# --------------------------------------------------------------------------------#
+	def statespace(self,y,t,xd):
+		# Update initial conditions #		
+
+		# Need the statespace array:
+		pos,vel,acc,jer = y
+
+		# Derivative of statesape array:		
+		dydt = [vel, acc, jer, 
+						self.k4*(self.k3*(self.k2*(self.k1*(xd - pos) - vel) - acc) - jer)]
+		return dydt
+
+	# Sigmoid
+	def sigmoid(self):
+		for i in range(self.sk):
+			if math.sqrt(self.SF_delay_x[2,i]**2 + self.SF_delay_y[2,i]**2) < self.at:
+				self.ak[i] = 1
+			else:
+				self.ak[i] = 0
+
+		pk = sum(self.ak)/self.sk
+		self.s_gain = 1/(1 + math.exp(-self.sk*(pk-self.pc)))
+
+	def delay(self,j,cmd):
+		'''
+		Function to keep track of values in past. Needed for
+		Shape filtering as well as feedback		
+
+		j   -> tells if we are doing pos, vel, or acc
+		cmd -> determines SF or FB
+		'''
+		if cmd == 1: # Shape filter 
+			if self.SF_idx < len(self.SF_delay_x[0]):
+				# First, fill up the delay array
+				self.SF_delay_x[j,self.SF_idx] = self.x[1,j]
+				self.SF_delay_y[j,self.SF_idx] = self.y[1,j]
+
+			else:
+				# once array is filled, we start using the first value every time 
+				# x
+				self.SF_delay_x[j,:] = np.roll(self.SF_delay_x[j,:],-1) 			# makes the first value last
+				self.SF_delay_x[j,len(self.SF_delay_x[0])-1] = self.x[1,j]    # updates last value to current x
+
+				# y
+				self.SF_delay_y[j,:] = np.roll(self.SF_delay_y[j,:],-1)
+				self.SF_delay_y[j,len(self.SF_delay_y[0])-1] = self.y[1,j]
+
+		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.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]
+
+			else:
+				# once array is filled, need to shift values w/ latest value at end
+				self.theta_fb[:] 		 = np.roll(self.theta_fb[:],-1)
+				self.theta_vel_fb[:] = np.roll(self.theta_vel_fb[:],-1)
+				self.theta_acc_fb[:] = np.roll(self.theta_acc_fb[:],-1)
+
+				self.phi_fb[:]		 = np.roll(self.phi_fb[:],-1)
+				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.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]
+
+		else:
+			print('No delay')
+
+	# Convolution and input shape filter, and feedback
+	def convo(self):
+
+		# needed for delay function
+		# 1 = determine shapefilter array
+		# 2 = determine theta/phi_fb
+		shapeFil = 1
+		feedBack = 2
+
+		# SOLVE ODE (get ref pos, vel, accel)
+		self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd[0],))
+		self.y = odeint(self.statespace,self.y0,self.t,args=(self.xd[1],))
+		self.z = odeint(self.statespace,self.z0,self.t,args=(self.xd[2],))
+
+		for i in range(1,len(self.y0)):	
+			self.x[:,i] = np.clip(self.x[:,i], a_min = -self.max[i], a_max = self.max[i])
+			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])
+			
+		for j in range(3): # 3 is due to pos, vel, acc. NOT due x, y, z
+
+			self.delay(j,shapeFil) # Determine the delay array
+
+			if self.SF_idx < len(self.SF_delay_x[0]):
+				self.EPS_I[3*j] 	= self.x[1,j] 
+				self.EPS_I[3*j+1] = self.y[1,j] 
+				self.EPS_I[3*j+2] = self.z[1,j]
+			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+2] = self.z[1,j] 																			 # No need to convolute z-dim
+
+			self.delay(1,feedBack) # Detemine feedback array
+			
+		self.sigmoid()       	# Determine sigmoid gain
+		EPS_D = self.fback()  # Feedback Epsilon
+
+		for i in range(9): 
+			# Populate EPS_F buffer with desired change based on feedback
+			self.EPS_F[i] = self.EPS_I[i] + EPS_D[i] #+ EPS_D[i]
+
+#		self.x0 = [self.dr_pos.position.x, self.dr_vel.x, self.dr_acc.x, self.x[1,3]]
+#		self.y0 = [self.dr_pos.position.y, self.dr_vel.y, self.dr_acc.y, self.y[1,3]] 
+#		self.z0 = [self.dr_pos.position.z, self.dr_vel.z, self.dr_acc.z, self.z[1,3]]
+
+		self.x0 = [self.dr_pos.position.x, self.x[1,1], self.x[1,2], self.x[1,3]]
+		self.y0 = [self.dr_pos.position.y, self.y[1,1], self.y[1,2], self.y[1,3]] 
+		self.z0 = [self.dr_pos.position.z, self.z[1,1], self.z[1,2], self.z[1,3]] 
+
+		self.SF_idx += 1
+		self.FB_idx += 1
+
+
+	def fback(self):
+		
+		xr 		 =  self.Gd*self.tetherL*math.sin(self.theta_fb[0])
+		xdotr  =  self.Gd*self.tetherL*math.cos(self.theta_fb[0])*self.theta_vel_fb[0]
+		xddotr = -self.Gd*self.tetherL*math.sin(self.theta_fb[0])*(self.theta_vel_fb[0]**2) + math.cos(self.theta_fb[0])*self.theta_acc_fb[0]
+
+		yr 		 = -self.Gd*self.tetherL*math.sin(self.phi_fb[0])
+		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])
+
+		return EPS_D 
+
+	def publisher(self,pub_tim):
+	
+		# Determine final ref signal		
+		self.convo()
+		
+		# Populate msg with epsilon_final
+		self.ref_sig.position.x = self.EPS_F[0]
+		self.ref_sig.position.y = self.EPS_F[1]
+
+		self.ref_sig.velocity.x = self.EPS_F[3]
+		self.ref_sig.velocity.y	= self.EPS_F[4]
+		self.ref_sig.velocity.z	= self.EPS_F[5]
+
+		self.ref_sig.acceleration.x	= self.EPS_F[6]
+		self.ref_sig.acceleration.y	= self.EPS_F[7]
+		self.ref_sig.acceleration.z	= self.EPS_F[8]
+
+		# Need typemask = 2 to use correct attitude controller - Jaeyoung Lin
+		self.ref_sig.type_mask = 2
+
+		# Publish command
+		self.ref_sig.header.stamp = rospy.Time.now()
+		self.pub_path.publish(self.ref_sig)
+		self.pub_ref.publish(self.ref_sig)
+		
+		# Feedback to user
+		rospy.loginfo(' Var |   x   |   y   |   z   ')
+		rospy.loginfo('Pos:    %.2f    %.2f    %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2])
+		rospy.loginfo('Vel:    %.2f    %.2f    %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5])
+		rospy.loginfo('Acc:    %.2f    %.2f    %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8])
+		rospy.loginfo('_______________________')
+		
+if __name__=="__main__":
+
+	# Initiate ROS node
+	rospy.init_node('desPath_node',anonymous=True)
+	try:
+		Main() 				# create class object
+		rospy.spin() 	# loop until shutdown signal
+
+	except rospy.ROSInterruptException:
+	        pass
+

src/square.py

@@ -0,0 +1,89 @@
+#!/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 PoseStamped
+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 	= PoseStamped()
+		# Init x, y, & z coordinates
+		self.Point.pose.position.x	= 0
+		self.Point.pose.position.y	= 0
+		self.Point.pose.position.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
+
+		# 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', PoseStamped, 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.pose.position.x = 0
+				self.Point.pose.position.y = 0
+
+			else:
+				self.Point.pose.position.x = self.xarray[self.i]
+				self.Point.pose.position.y = self.yarray[self.i]
+				
+				rospy.loginfo("Sending [Point x] %d [Point y] %d",
+											 self.Point.pose.position.x, self.Point.pose.position.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
+
+#		self.Point.header.stamp = rospy.Time.now()
+#		self.Point.pose.position.x = self.xarray[self.i]
+#		self.Point.pose.position.y = self.yarray[self.i]
+		
+#		rospy.loginfo("Sending [Point x] %d [Point y] %d",
+#									 self.Point.pose.position.x, self.Point.pose.position.y) 	
+			
+		# Published desired msgs
+#		self.pub_square.publish(self.Point)
+#		self.i += 1
+
+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
+

src/step.py

@@ -0,0 +1,62 @@
+#!/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 PoseStamped
+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 	= PoseStamped()
+		# Init x, y, & z coordinates
+		self.Point.pose.position.x	= 1
+		self.Point.pose.position.y	= 0
+		self.Point.pose.position.z	= 4.0
+		
+		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', PoseStamped, 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.pose.position.x, self.Point.pose.position.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
+