oscillation_ctrl/src/ref_signalGen.py

#!/usr/bin/env python

### 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
import rosservice

from scipy.integrate import odeint 
from oscillation_ctrl.msg import  RefSignal, LoadAngles
from oscillation_ctrl.srv import WaypointTrack
from geometry_msgs.msg import Pose, PoseStamped, Point, TwistStamped 
from sensor_msgs.msg import Imu

class Main:

	def __init__(self):                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      

		# rate(s)
		rate = 10 # 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.tmax			= self.dt # used to get desired pos, vel, and acc for next time step (tmax) 
		self.n				= self.tmax/self.dt + 1
		self.t 				= np.linspace(0, self.tmax, self.n) # Time array

		# Message generation/ collection
		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	 = RefSignal()	  # Smooth Signal
		self.load_angles = LoadAngles()

		self.dr_pos 	= Pose()
		self.dr_vel		= self.vel_data.twist.linear
		self.dr_acc		= self.imu_data.linear_acceleration
	
		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 									  #
# --------------------------------------------------------------------------------#
		# Topic, msg type, and class callback method
		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)

# --------------------------------------------------------------------------------#		
# 									PUBLISHERS
# --------------------------------------------------------------------------------#
		# Publish desired path to compute attitudes
		self.pub_path = rospy.Publisher('/reference/path',RefSignal,queue_size = 10)
		self.waypointTracker_pub = rospy.Publisher('/reference/waypoints',Point,queue_size = 10) # not needed. Used for performance analysis

		# 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)

# --------------------------------------------------------------------------------#		
# 							FEEDBACK AND INPUT SHAPING
# --------------------------------------------------------------------------------#

		# 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		= 2 # 1.5 for lab #  1.5 for Gaz. Increase this to increase aggresiveness of trajectory i.e. higher accelerations
		# self.epsilon	= 0.8  # Damping ratio
		self.epsilon	= 0.707  # Damping ratio
		# self.epsilon	= 0.7  # Damping ratio
		
		# need exception if we do not have tether:
		if self.tetherL == 0.0:
			self.wn = self.w_tune
		else:
			self.wn	= math.sqrt(9.81/self.tetherL) 
			# self.wn	= 3 #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.k4*self.k3*self.k2)
		
		# For saturation:
		self.max  =	[0, 7, 5, 3]  #[0, 5, 3, 3] - lab testing
 
		self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)		
		self.empty_point = Point() # Needed to query waypoint_server

		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]

		# Constants for feedback
		self.Gd = 0.325 #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
		# From Klausen 2015
		self.t2	= math.pi/ self.wn
		self.K	= 1
		self.A1	= 0.4948
		self.A2	= 0.5052
		rospy.loginfo('Regarding input shaping: K = {0}, A1 = {1}, A2 = {2}, and t2 = {3}'.format(self.K, self.A1, self.A2, self.t2))	

		# Need to determine how large of any array needs to be stored to use delayed functions
		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))) 
		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.pc 	= 0.99 # Changed
		self.sk		= self.SF_delay_x.shape[1]  # from Klausen 2017
		self.ak 	= np.zeros(self.sk)
		self.s_gain	= 0.0 # Gain found from sigmoid	

		self.waypoint_service = False
# --------------------------------------------------------------------------------#		
# 								CALLBACK FUNCTIONS
# --------------------------------------------------------------------------------#

	# Callback to get link names, states, and pload deflection angles
	def loadAngles_cb(self,msg):
		try:
			self.load_angles = msg	
			pass					
		except ValueError:
			pass

	# Callback drone pose
	def dronePos_cb(self,msg): 
		try:
			self.dr_pos 	= msg.pose
			# self.dr_pos 	= msg.drone_pos

		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_srv_cb(self):
		if self.waypoint_service:
			try:
				resp = self.get_xd(False,self.empty_point)
				self.xd = resp.xd
			except ValueError as e:
				rospy.loginfo('{0}'.format(e))			
		else:
			if '/status/waypoint_tracker' in rosservice.get_service_list(): # dont want to always call this service
				# rospy.wait_for_service('/status/waypoint_tracker')
				self.waypoint_service = True
				try:
					resp = self.get_xd(False,self.empty_point)
					self.xd = resp.xd
				except ValueError as e:
					rospy.loginfo('{0}'.format(e))
			else:
				rospy.loginfo('Service not available, defaulting to save point:\nx = 0.0 m, y = 0.0 m, x = 2.0 m')
				self.xd = Point(x=0.0,y=0.0,z=2.0)

	def get_tether(self):
		""" Get tether length based on set parameters"""
		self.param_exists = False
		while self.param_exists == False:
			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 >= 3:
				tether_length = 0.0
				break
		return tether_length
# --------------------------------------------------------------------------------#		
# 									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

		error = xd - pos
		# Derivative of statesape array:		
		dydt = [vel, acc, jer, self.k4*(self.k3*(self.k2*(self.k1*(error) - 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.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.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.dt

			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[-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[-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')

	# Convolution of shape filter and trajectory, and feedback
	def convo(self):
		# check if waypoints have changed
		self.waypoints_srv_cb()
		# 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.x,))
		self.y = odeint(self.statespace,self.y0,self.t,args=(self.xd.y,))
		self.z = odeint(self.statespace,self.z0,self.t,args=(self.xd.z,))

		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])
			
		# 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
			self.EPS_I[3*j+2] = self.z[-1,j] # No need to convolute z-dim

			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] 
				
			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.delay(1,FEEDBACK) # Detemine feedback array
			
		# self.sigmoid()        # Determine sigmoid gain
		Eps_D = self.fback()  # Feedback Epsilon

		self.EPS_F = self.EPS_I  + self.s_gain*Eps_D
		# self.EPS_F = self.EPS_I  + Eps_D

		# Populate msg with epsilon_final
		self.ref_sig.header.stamp = rospy.Time.now()
		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]
		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]
		
		# 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.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):
		""" Uses first element in feedback array as this is what get replaced in each iteration """
		
		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 user_feeback(self):

		# 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('_______________________')

	def publisher(self,pub_tim):
	
		# Determine final ref signal
		try:		
			self.convo()

			# Publish reference signal
			self.pub_path.publish(self.ref_sig)

			# Publish what the setpoints are
			self.waypointTracker_pub.publish(self.xd)

			# Give user feedback on published message:
			# self.user_feeback()
		except AttributeError as e:
			rospy.loginfo('Error {0}'.format(e))

			pass # catches case at beginning when self.xd is not properly initialized

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