More improvements to controller

.gitignore

@@ -1,3 +1,4 @@
+config/mocap_*
 launch/cortex_bridge.launch
 launch/debug.launch
 launch/klausen_dampen.launch
@@ -7,10 +8,12 @@ src/killswitch_client.py
 src/land_client.py
 src/MoCap_*.py
 src/Mocap_*.py
+src/mocap_*
 src/segmented_tether.py
 src/segmented_tether_fast.py
 msg/Marker.msg
 msg/Markers.msg
 *.rviz
+CMakeLists.txt
 setup.txt
 

CMakeLists.txt

@@ -15,39 +15,10 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
 )
 
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
 ################################################
 ## Declare ROS messages, services and actions ##
 ################################################
 
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
 ## Generate messages in the 'msg' folder
 add_message_files(
   FILES
@@ -78,26 +49,6 @@ generate_messages(
   sensor_msgs
 )
 
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
 ###################################
 ## catkin specific configuration ##
 ###################################
@@ -112,7 +63,7 @@ catkin_package(
 #  LIBRARIES oscillation_ctrl
   CATKIN_DEPENDS CATKIN_DEPENDS roscpp mavros geometry_msgs message_runtime
 
-#  DEPENDS system_lib
+
 )
 
 ###########
@@ -138,6 +89,18 @@ target_link_libraries(pathFollow_node ${catkin_LIBRARIES})
 
 add_dependencies(pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
 
+#add_executable(mocap_offb_node src/mocap_offb_node.cpp)
+
+#target_link_libraries(mocap_offb_node ${catkin_LIBRARIES})
+
+#add_dependencies(mocap_offb_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
+
+#add_executable(mocap_pathFollow_node src/mocap_path_follow.cpp)
+
+#target_link_libraries(mocap_pathFollow_node ${catkin_LIBRARIES})
+
+#add_dependencies(mocap_pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
+
 ## Declare a C++ library
 # add_library(${PROJECT_NAME}
 #   src/${PROJECT_NAME}/oscillation_ctrl.cpp

config/Ctrl_param.yaml

@@ -1,5 +1,8 @@
 # Ros param when using Klausen Ctrl
+wait_time: 30
+#drone_mass: 0.5841
+drone_mass: 1.437
+pload_mass: 0.50
+use_ctrl: false
 
-wait: 40
-waypoints: {x:  0.0, y:  0.0, z:  5.0}
 

config/gazebo_config.yaml

@@ -0,0 +1,4 @@
+# Ros param when not using Klausen Ctrl
+waypoints: {x:  0.0, y:  0.0, z:  1.5}
+square_x: [0.5,1,1,1,0.5,0,0]
+square_y: [0,0,0.5,1,1,1,0.5]

config/noCtrl_param.yaml

@@ -1,4 +1,7 @@
 # Ros param when not using Klausen Ctrl
+wait_time: 40 
+drone_mass: 0.5841
+#drone_mass: 1.437
+pload_mass: 0.10
+
 
-wait: 55 
-waypoints: {x:  0.0, y:  0.0, z:  5.0}

launch/oscillation_damp.launch

@@ -5,17 +5,14 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
 <launch>
   <arg name="model" default="headless_spiri_with_tether"/>
   <arg name="test" default="none"/>
-
+	<group ns="sim">
-  <group ns="sim">
+		<rosparam file="$(find oscillation_ctrl)/config/gazebo_config.yaml" />
-  	<rosparam file="$(find oscillation_ctrl)/config/Ctrl_param.yaml" />
+	</group>
-  </group>
-
   <group ns="status">
+		<rosparam file="$(find oscillation_ctrl)/config/Ctrl_param.yaml" />
   	<param name="test_type" value="$(arg test)"/>
   </group>
 
-
-
 	<!-- LOCALIZES DRONE & DETERMINES LOAD ANGLES -->
 	<node
 		pkg="oscillation_ctrl"
@@ -40,7 +37,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
 		pkg="oscillation_ctrl"
 		type="klausen_control.py"
 		name="klausenCtrl_node"
-		output="screen"
+		launch-prefix="xterm -e"
 	/>
 	<!-- PUBLISHES DESIRED COMMANDS -->
 	<node

launch/takeoff_noCtrl.launch

@@ -2,10 +2,12 @@
 <launch>
   <arg name="model" default="headless_spiri_with_tether"/>
   <arg name='test' default="none"/>
-  <group ns="sim">
+  
-  	<rosparam file="$(find oscillation_ctrl)/config/noCtrl_param.yaml" />
+	<group ns="sim">
-  </group>
+  	<rosparam file="$(find oscillation_ctrl)/config/gazebo_config.yaml" />
+	</group>
   <group ns="status">
+		<rosparam file="$(find oscillation_ctrl)/config/noCtrl_param.yaml" />
   	<param name="test_type" value="$(arg test)"/>
   </group>
 

msg/TetheredStatus.msg

@@ -1,6 +1,6 @@
-# Reports position of drone, payload, and the roll angle between them
+# Reports position of drone, payload
 std_msgs/Header header		# Header
-std_msgs/Bool 	bool		# Boolean, True = Payload
+bool 	pload			# Boolean, True = Payload
 geometry_msgs/Pose drone_pos	# Drone pose
 geometry_msgs/Pose pload_pos	# Payload pose
 

px4_launch/spiri_with_tether.launch

@@ -45,7 +45,7 @@
         <arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
     </include>
     <!-- MAVROS -->
-    <include file="$(find mavros)/launch/px4.launch">
+    <include file="$(find oscillation_ctrl)/launch/px4.launch">
         <!-- GCS link is provided by SITL -->
         <arg name="gcs_url" value=""/>
         <arg name="fcu_url" value="$(arg fcu_url)"/>

src/LinkState.py

@@ -19,7 +19,7 @@ class Main:
 
 		# rate(s)
 		rate = 60 # rate for the publisher method, specified in Hz -- 20 Hz
-		self.dt		 = 1.0/rate
+		self.dt = 1.0/rate
 
 		rospy.sleep(5) # Sleep for 1 sec. Need to give time to Gazebo to ru
 
@@ -30,7 +30,7 @@ class Main:
 		# How long should we wait before before starting test
 		self.param_exists = False
 		while self.param_exists == False:
-			if rospy.has_param('sim/wait'):
+			if rospy.has_param('wait_time'):
 				self.wait	= rospy.get_param('sim/wait') # wait time
 				self.param_exists = True
 			elif rospy.get_time() - self.tstart >= 3.0:
@@ -143,7 +143,7 @@ class Main:
 												pload_P.link_state.pose.position.y)**2 +
 											 (drone_P.link_state.pose.position.z - 
 												pload_P.link_state.pose.position.z)**2)
-					rospy.set_param('sim/tether_length',self.tetherL)
+					rospy.set_param('status/tether_length',self.tetherL)
 
 				else:
 					self.tetherL = 0

src/gazebo_setMasses.py

@@ -0,0 +1,65 @@
+#!/usr/bin/env python2.7
+
+### Cesar Rodriguez Jul 2022
+### Sets mass of links in gazebo to desired values
+
+import rospy
+from gazebo_msgs.srv import SetLinkProperties
+
+
+class Main:
+    def __init__(self):
+        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()
+
+		# Constants
+        self.param_exists = False
+        while self.param_exists == False:
+            if rospy.has_param('drone_mass'):
+                self.drone_m	= rospy.get_param('drone_mass') # wait time
+                self.param_exists = True
+            elif rospy.has_param('sim/drone_mass'):
+                self.drone_m	= rospy.get_param('sim/drone_mass') # wait time
+                self.param_exists = True
+            elif rospy.get_time() - self.tstart >= 3.0:
+                self.drone_m = 1.0
+                rospy.loginfo('DRONE MASS NOT FOUND IN CONFIG FILE')
+                break
+
+        self.param_exists = False
+        while self.param_exists == False:
+            if rospy.has_param('pload_mass'):
+                self.pl_m	= rospy.get_param('pload_mass') # wait time
+                self.param_exists = True
+            elif rospy.has_param('sim/pload_mass'):
+                self.pl_m	= rospy.get_param('sim/pload_mass') # wait time
+                self.param_exists = True
+            elif rospy.get_time() - self.tstart >= 3.0:
+                self.pl_m = 0.0
+                rospy.loginfo('PLOAD MASS NOT FOUND IN CONFIG FILE')
+                break
+
+        self.drone_ids	  = ['spiri_with_tether::spiri::base', 'spiri::base']
+        self.pload_id	  = 'spiri_with_tether::mass::payload'
+        self.service      = '/gazebo/set_link_properties'
+
+        self.set_mass = rospy.ServiceProxy(self.service,SetLinkProperties)
+
+        for name in self.drone_ids:
+            self.set_drone_mass = self.set_mass(link_name=name,mass=self.drone_m)
+            if self.set_drone_mass.success:
+                rospy.loginfo('Set "%s" mass to: %.2f kg', name,self.drone_m)
+                break
+
+        self.set_pload_mass = self.set_mass(link_name=self.pload_id,mass=self.pl_m)
+        if self.set_pload_mass.success:
+            rospy.loginfo('Set "%s" mass to: %.2f kg', self.pload_id,self.pl_m)
+        
+
+if __name__=="__main__":
+
+    # Initiate ROS node
+    rospy.init_node('SetMassNode',anonymous=False)
+    Main() # create class object
+

src/klausen_control.py

@@ -11,21 +11,28 @@ import rospy,  tf
 import numpy as np
 import time
 import math
+import rosservice
 
 from tf.transformations   import *
 from scipy.integrate  	  import odeint 
-from oscillation_ctrl.msg import RefSignal, LoadAngles
+from oscillation_ctrl.msg import RefSignal, LoadAngles, TetheredStatus
 from oscillation_ctrl.srv import WaypointTrack
 from geometry_msgs.msg 	  import Pose, Point, TwistStamped, PoseStamped
 from sensor_msgs.msg 	  import Imu
 from mavros_msgs.msg 	  import AttitudeTarget
 
+class DesiredPoint():
+	def __init__(self,x,y,z):
+		self.x = x
+		self.y = y
+		self.z = z
+
 class Main:
 
 	def __init__(self):
 
 		# rate(s)
-		rate = 50 # rate for the publisher method, specified in Hz -- 20 Hz
+		rate = 25 # rate for the publisher method, specified in Hz -- 50 Hz
 
 		# initialize variables
 		
@@ -54,7 +61,7 @@ class Main:
 		self.empty_point = Point() # Needed to query waypoint_server
 
 		# Drone var
-		self.has_run	 = 0		# Bool to keep track of first run instance
+		self.has_run	 = 0 # Bool to keep track of first run instance
 
 		# Col1 = theta, theta dot; Col2 = phi, phidot for self.PHI
 		self.PHI 		= np.array([[0,0],[0,0]]) 
@@ -65,19 +72,13 @@ class Main:
 		# Tether var - Check if current method is used
 		# Get tether length
 		self.param_exists = False
-		while self.param_exists == False: # Need to wait until param is ready
+		self.tetherL = self.get_tether()
-			if rospy.has_param('sim/tether_length'):
+		self.tether = True if self.tetherL > 0.01 else False
-				self.tetherL	= rospy.get_param('sim/tether_length') # Tether length
-				self.param_exists = True
-				self.tether 			= True
-			elif rospy.get_time() - self.tstart >= 10.0:
-				self.tetherL = 0.0
-				rospy.loginfo('waiting for tether length')
-				break 
 
-		# Tuning gains
+		# Retrieve drone and payload masses from config file
-		self.K1	= np.identity(3)
+		[self.drone_m, self.pl_m] = self.get_masses()
-		self.K2	= np.identity(5)
+		rospy.loginfo('DRONE MASS: %.2f',self.drone_m)
+		self.tot_m 	  = self.drone_m + self.pl_m
 
 		# Values which require updates. *_p = prior
 		self.z1_p	  =	np.zeros([3,1])
@@ -90,33 +91,42 @@ class Main:
 		# 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
+	
+		# 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 # np.array([0,0,0,0.01,0.01])
+		# 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]
+		self.Ki = self.tune*self.K1
 
-		# Constants
-		self.drone_m  = 1.437
-		self.pl_m	  =	0.5
-		self.tot_m 	  = self.drone_m + self.pl_m
-
-		self.tune	  = 1 # Tuning parameter
-		self.dist	  =	np.array([0,0,0,0.01,0.01]) # Wind disturbance
-		
 		# PD Thrust Controller terms
 		# gains for thrust PD Controller
-		#self.Kp 	= 2.7 
+		#self.Kp 	= 3.0
 		#self.Kd 	= 3 
-		self.Kp 	= 3.0
+		self.Kp_thrust 	= 1.5
-		self.Kd 	= 3 
+		self.Kd_thrust 	= 1
-		self.max_a	= 14.2
+		self.max_a	= 14.2 #TODO
 		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 = 1.0	# There was found to be a constant offset 0.7
+		self.gravity = np.array([[0],[0],[9.81]])
+
+		self.thrust_offset =  0.0 #1.0	# There was found to be a constant offset 0.7
+
+		# Get scaling thrust factor, kf
+		self.kf = self.get_kf()
+
+		self.service_list = rosservice.get_service_list()
 # --------------------------------------------------------------------------------#		
 # 									SUBSCRIBERS
 # --------------------------------------------------------------------------------#
 		# Topic, msg type, and class callback method
 		rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb)
 		rospy.Subscriber('/reference/path', RefSignal, self.refsig_cb)
-		#rospy.Subscriber('/status/twoBody_status', TetheredStatus, self.dronePos_cb)
+		# rospy.Subscriber('/status/twoBody_status', TetheredStatus, self.dronePos_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)
@@ -130,6 +140,58 @@ class Main:
 		# 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
 
+# --------------------------------------------------------------------------------#		
+# 								SETUP PARAM METHODS
+# --------------------------------------------------------------------------------#
+
+	def get_tether(self):
+		""" Get tether length based on set parameters"""
+		param_exists = False
+		while param_exists == False:
+			if rospy.has_param('status/tether_length'):
+				tether_length	= rospy.get_param('status/tether_length') # Tether length
+				param_exists = True
+			elif rospy.get_time() - self.tstart >= 5.0:
+				tether_length	= 0.0
+				break
+		return tether_length
+
+	def get_masses(self):
+		""" Use parameters to retrieve drone and payload mass set in config file and sets total mass of bodies"""
+		# Constants
+		param_exists = False # reset to False to get body masses
+		while param_exists == False:
+			if rospy.has_param('status/drone_mass'):
+				drone_m	= rospy.get_param('status/drone_mass') # wait time
+				param_exists = True
+			elif rospy.get_time() - self.tstart >= 3.0:
+				drone_m = 0.5841
+				rospy.loginfo('DRONE MASS NOT FOUND IN CONFIG FILE')
+				break
+		if self.tether:
+			param_exists = False
+			while param_exists == False:
+				if rospy.has_param('status/pload_mass'):
+					pl_m	= rospy.get_param('status/pload_mass') # wait time
+					param_exists = True
+				elif rospy.get_time() - self.tstart >= 3.0:
+					pl_m = 0.0
+					rospy.loginfo('PLOAD MASS NOT FOUND IN CONFIG FILE')
+					break
+		else:
+			pl_m = 0.0
+
+		rospy.loginfo('PLOAD MASS: %.2f',pl_m)
+		return [drone_m, pl_m]
+		
+	def get_kf(self):
+		if rospy.has_param('mocap/hover_throttle'):
+			hover_throttle = rospy.get_param('mocap/hover_throttle')
+		else:
+			hover_throttle = (self.tot_m*9.81 + 9.57)/34.84 # linear scaling depending on dependent on mass
+		kf = hover_throttle/(self.tot_m*9.81)
+		return kf
+
 # --------------------------------------------------------------------------------#		
 # 								CALLBACK FUNCTIONS
 # --------------------------------------------------------------------------------#
@@ -138,20 +200,16 @@ class Main:
 	def loadAngles_cb(self,msg):
 		try:
 			self.load_angles = msg
-
 			# Populate self.PHI
 			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): 
 		try:
 			self.dr_pos = msg.pose
-			#self.dr_pos = msg.drone_pos
+			# self.dr_pos = msg.drone_pos
-
 		except ValueError:
 			pass
 
@@ -177,7 +235,8 @@ class Main:
 		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 + 9.81]]) #TODO
+			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 + 9.81]]) #TODO
 
 		except ValueError:
 			pass
@@ -190,12 +249,15 @@ class Main:
 			rospy.loginfo_once('NO TETHER DETECTED')
 
 	def waypoints_srv_cb(self):
-		rospy.wait_for_service('/status/waypoint_tracker')
+		if '/status/waypoint_tracker' in self.service_list:
-		try:
+			rospy.wait_for_service('/status/waypoint_tracker')
-			resp = self.get_xd(False,self.empty_point)
+			try:
-			self.xd = resp.xd
+				resp = self.get_xd(False,self.empty_point)
-		except ValueError:
+				self.xd = resp.xd
-			pass
+			except ValueError:
+				pass
+		else:
+			self.xd = DesiredPoint(0.0,0.0,1.5)
 
 # ---------------------------------ODE SOLVER-------------------------------------#
 	def statespace(self,y,t,Ka,Kb,Kc):
@@ -253,29 +315,29 @@ class Main:
 		# 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.error = np.array([[self.dr_pos.position.x - self.xd.x],
+		self.R = self.quaternion_rotation_matrix()
-								[self.dr_pos.position.y - self.xd.y],
+		self.error = np.array([ [self.path_pos[0] - self.dr_pos.position.x],
-								[self.dr_pos.position.z - self.xd.z - self.thrust_offset]])
+								[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.dr_vel - self.path_vel
+		self.error_vel = self.path_vel - self.R.dot(self.dr_vel)
 
-		self.R = self.quaternion_rotation_matrix()	# determine Rotation Matrix
+		# determine Rotation Matrix
 		self.R_e3 =  np.array([[self.R.T[2][0]],[self.R.T[2][1]],[self.R.T[2][2]]])
 
-
+		# test which one is better:
-		#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 = (9.81*self.tot_m*self.e3 + self.Kp_thrust*self.error + self.Kd_thrust*self.error_vel - self.tot_m*self.path_acc)*self.kf
-		#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 = thrust_vector[0]*self.R_e3[0] + thrust_vector[1]*self.R_e3[1] + thrust_vector[2]*self.R_e3[2]
-		#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
+		### OR:
-		thrust_vector = (9.81*self.tot_m - self.Kp*self.error[2] - self.Kd*self.dr_vel[2])/self.max_t
+		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[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/(math.cos(self.EulerAng[0]*self.EulerAng[1]))
-		#thrust = thrust_vector[2]
 
 		# Value needs to be between 0 - 1.0
 		self.att_targ.thrust = max(0.0,min(thrust,1.0))
 
 
 	def determine_q(self):
+		""" Determine attitude commands based on feedback and feedforward methods"""
 		# 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.load_angles.theta],[self.load_angles.phi]])
@@ -287,12 +349,6 @@ class Main:
 		s_theta = math.sin(self.load_angles.theta)
 		s_phi	= math.sin(self.load_angles.phi)
 
-		# Check for tether
-		if L <= 0.01:
-			self.tether = False
-		else:
-			self.tether = True
-
 		# Check if tether was correctly detected
 		self.tether_check()
 
@@ -311,7 +367,7 @@ class Main:
 
 		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]]
 
-		H = [[1,0,0,0,0],[0,1,0,0,0],[0,0,1,0,0]]	
+		H = [[1,0,0,0,0],[0,1,0,0,0],[0,0,1,0,0]]
 
 		D = np.diag(self.dist)	# Already in array format
 
@@ -355,6 +411,7 @@ class Main:
 
 		# 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
@@ -363,52 +420,60 @@ class Main:
 
 		B = np.dot(C_c,self.a45) + np.dot(M_c,self.a45dot)
 		
-		# Gain terms
-		Kp = np.identity(3) + np.dot(self.K2[:3,:3],self.K1) + self.tune*np.identity(3)
-		Kd = self.tot_m*self.K1 + self.K2[:3,:3]
-		Ki = self.tune*self.K1
-
 		# 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,0.5*self.dt*(self.z1 - self.z1_p))
+		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*self.path_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))
 		# Update self.z1_p for "integration"		
 		self.z1_p = self.z1
 
 		# Covert Fd into drone frame
 		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)
-		
+		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],Fd[2],0],dr_orientation_inv)) # Real part of Fd needs = 0
+		# Fd_tf = [Fd[0],Fd[1],Fd[2]]
-
 		# 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	
+
+		# rospy.loginfo('Fd before transform: %.2f, %.2f, %.2f' % Fd[0],Fd[1],Fd[2])
+		# rospy.loginfo('Fd before transform: %.2f, %.2f, %.2f', Fd[0], Fd[1], Fd[2])
+		# rospy.loginfo('Fd after transform: %.2f, %.2f, %.2f', Fd_tf[0],Fd_tf[1],Fd_tf[2])
 		
 		# Convert Euler angles to quaternions
+		# for i,val in enumerate(self.EulerAng):
+		# 	if val*-1 < 0.0:
+		# 		sign = 1
+		# 	else:
+		# 		sign = -1
+		# 	if abs(val) >=0.44:
+		# 		self.EulerAng[i] = 0.44*sign
+
 		q = quaternion_from_euler(self.EulerAng[0],self.EulerAng[1],self.EulerAng[2])
 
 		# Populate msg variable
 		# Attitude control
 		self.att_targ.header.stamp = rospy.Time.now()
-		self.att_targ.header.frame_id = 'map'
+		self.att_targ.header.frame_id = '/map'
 		self.att_targ.type_mask = 1|2|4 
 		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]
-		
+
+	def user_fback(self):
+		rospy.loginfo('thrust: %.6f' % self.att_targ.thrust)
+		rospy.loginfo('\nroll: %.2f\npitch: %.2f',self.EulerAng[0]*180/3.14,self.EulerAng[1]*180/3.14)
+
 	def publisher(self,pub_time):
 		self.determine_q()
 		self.determine_throttle()
 		self.pub_att_targ.publish(self.att_targ)
-		#rospy.loginfo('roll: %.2f\npitch: %.2f\n',self.EulerAng[0],self.EulerAng[1])
+		self.user_fback()
 
 # --------------------------------------------------------------------------------#		
-# 																			ALGORITHM
+# 							ALGORITHM ENDS 
 # --------------------------------------------------------------------------------#
-
-
 if __name__=="__main__":
 
 	# Initiate ROS node

src/offb_node.cpp

@@ -48,16 +48,15 @@ void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
 			ROS_INFO("Waiting...");
 		} else {
 			pose_read = true;
-		    	pose.pose.orientation.x = q_init.x;
+            // pose.pose.orientation.x = q_init.x;
-   			pose.pose.orientation.y = q_init.y;
+            // pose.pose.orientation.y = q_init.y;
-    			pose.pose.orientation.z = q_init.z;
+            // pose.pose.orientation.z = q_init.z;
-    			pose.pose.orientation.w = q_init.w;
+            // pose.pose.orientation.w = q_init.w;
 		}
 	}
 
 }
 
-
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "offb_node");
@@ -66,7 +65,7 @@ int main(int argc, char **argv)
     /********************** SUBSCRIBERS **********************/
     // Get current state
     ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
-            ("mavros/state", 10, state_cb);
+        ("mavros/state", 10, state_cb);
 
     // Pose subscriber
     ros::Subscriber mavPose_sub = nh.subscribe<geometry_msgs::PoseStamped>
@@ -91,7 +90,7 @@ int main(int argc, char **argv)
 
     // Publish attitude commands
     ros::Publisher att_pub = nh.advertise<geometry_msgs::PoseStamped>
-	    ("/mavros/setpoint_attitude/attitude",10);
+	("/mavros/setpoint_attitude/attitude",10);
 
     // Service Clients
     ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
@@ -99,7 +98,7 @@ int main(int argc, char **argv)
     ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
         ("mavros/set_mode");
     ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
-	    ("mavros/cmd/takeoff");
+	("mavros/cmd/takeoff");
 
     //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
     ros::Rate rate(20.0);
@@ -163,12 +162,16 @@ int main(int argc, char **argv)
                 last_request = ros::Time::now();
             }
         }
-		// Update desired waypoints
+	    // check if waypoints have changed desired waypoints
         waypoint_cl.call(wpoint_srv);
         pose.pose.position.x = wpoint_srv.response.xd.x;
         pose.pose.position.y = wpoint_srv.response.xd.y;
         pose.pose.position.z = wpoint_srv.response.xd.z;
-		
+
+        // User info
+        ROS_INFO("Current Altitude: %.2f",mavPose.pose.position.z);
+        ROS_INFO("Desired Altitude: %.2f",pose.pose.position.z);
+        ROS_INFO("---------------------------");
 		
 		local_pos_pub.publish(pose);
 

src/path_follow.cpp

@@ -4,7 +4,7 @@
 */
 
 #include <ros/ros.h>
-#include <sensor_msgs/NavSatFix.h>
+#include <std_msgs/Bool.h>
 #include <oscillation_ctrl/RefSignal.h>
 #include <oscillation_ctrl/EulerAngles.h>
 #include <oscillation_ctrl/WaypointTrack.h>
@@ -115,21 +115,6 @@ int main(int argc, char **argv)
 	//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
     ros::Rate rate_pub(25.0);
 
-	// Retrieve desired waypoints
-	oscillation_ctrl::WaypointTrack wpoint_srv;
-	wpoint_srv.request.query = false;
-	if (waypoint_cl.call(wpoint_srv))
-	{
-		ROS_INFO("Waypoints received");
-	}
-	
-	// populate desired waypoints - this is only for original hover as 
-	// a change of waypoints is handled by ref_signal.py
-    buff_pose.pose.position.x = wpoint_srv.response.xd.x;
-    buff_pose.pose.position.y = wpoint_srv.response.xd.y;
-    buff_pose.pose.position.z = wpoint_srv.response.xd.z;
-	double alt_des = buff_pose.pose.position.z; // Desired height
-
     // Desired mode is offboard
     mavros_msgs::SetMode offb_set_mode;
     offb_set_mode.request.custom_mode = "OFFBOARD";
@@ -139,7 +124,8 @@ int main(int argc, char **argv)
     arm_cmd.request.value = true;
 
     // Take off command    
-    bool takeoff = false, att_cmds = false;
+    bool takeoff = false, att_cmds = false; 
+	bool oscillation_damp = true;
 
     // Keep track of time since requests
     ros::Time tkoff_req = ros::Time::now();
@@ -149,10 +135,26 @@ int main(int argc, char **argv)
     for(int i = 100; ros::ok() && i > 0; --i){
         local_pos_pub.publish(buff_pose);
         ros::spinOnce();
-		ROS_INFO("Publishing position setpoints");
+		ROS_INFO_ONCE("Publishing position setpoints");
         rate_wait.sleep();
     }
 
+	// Retrieve desired waypoints
+	oscillation_ctrl::WaypointTrack wpoint_srv;
+	wpoint_srv.request.query = false;
+	if (waypoint_cl.call(wpoint_srv))
+	{
+		ROS_INFO("Waypoints received");
+		// populate desired waypoints - this is only for original hover as 
+		buff_pose.pose.position.x = wpoint_srv.response.xd.x;
+		buff_pose.pose.position.y = wpoint_srv.response.xd.y;
+		buff_pose.pose.position.z = wpoint_srv.response.xd.z;
+	} else {
+		ROS_INFO("NO waypoints received");
+	}
+	
+	double alt_des = wpoint_srv.response.xd.z; // Desired height	
+
     while(ros::ok()){
 		if(gps_read == true){
 			if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){
@@ -177,26 +179,44 @@ int main(int argc, char **argv)
 					takeoff = true;
 				}
 			}
+
+			// Check if we want to use oscillation controller
+			//if (ros::param::get("/use_ctrl", oscillation_damp) == true){
+			if (ros::param::has("/status/use_ctrl")){
+				ros::param::get("/status/use_ctrl", oscillation_damp);
+				if(oscillation_damp == true){
+					ROS_INFO("ATTITUDE CTRL");
+				}
+			}
 			
 			// Determine which messages to send
-			if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff){
+			// if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff && oscillation_damp){
+			// 	att_targ.header.stamp = ros::Time::now();
+			// 	att_targ_pub.publish(att_targ);
+			// 	att_cmds = true;
+			if(oscillation_damp){
 				att_targ.header.stamp = ros::Time::now();
 				att_targ_pub.publish(att_targ);
 				att_cmds = true;
-				ROS_INFO("ATTITUDE CTRL");
-				ROS_INFO("Des Altitude: %.2f", alt_des);
-				ROS_INFO("Cur Altitude: %.2f", current_altitude);	
 			} else {
+				// Check if waypoints have changed
+				if (waypoint_cl.call(wpoint_srv))
+				{
+					// populate desired waypoints - this is only for original hover as 
+					buff_pose.pose.position.x = wpoint_srv.response.xd.x;
+					buff_pose.pose.position.y = wpoint_srv.response.xd.y;
+					buff_pose.pose.position.z = wpoint_srv.response.xd.z;
+				}
 				local_pos_pub.publish(buff_pose);
 				ROS_INFO("POSITION CTRL");
-				ROS_INFO("Des Altitude: %.2f", alt_des);
+			}
-				ROS_INFO("Cur Altitude: %.2f", current_altitude);	
+			ROS_INFO("Des Altitude: %.2f", alt_des);
-			}			
+			ROS_INFO("Cur Altitude: %.2f", current_altitude);	
+			ROS_INFO("---------------------------");	
 			ros::spinOnce();
 			rate_pub.sleep();
 		}
     }
 
     return 0;
-}
+}
-

src/perform_test.py

@@ -27,8 +27,8 @@ class Main:
 
 		# Test parameters
 		# Square
-		self.xarray = [1,2,2,2,1,0,0]
+		# self.xarray = [1,2,2,2,1,0,0]
-		self.yarray = [0,0,1,2,2,2,1]
+		# self.yarray = [0,0,1,2,2,2,1]
 		self.i = 0
 		self.j = 0
 		self.buffer = 10
@@ -46,6 +46,16 @@ class Main:
 				self.test = 'none'
 				break
 		
+		if rospy.has_param('sim/square_x'):
+			self.xarray = rospy.get_param('sim/square_x')
+			self.yarray = rospy.get_param('sim/square_y')
+		elif rospy.has_param('mocap/square_x'):
+			self.xarray = rospy.get_param('mocap/square_x')
+			self.yarray = rospy.get_param('mocap/square_y')
+		else:
+			self.xarray = [1,2,2,2,1,0,0]
+			self.yarray = [0,0,1,2,2,2,1]
+
 		# get waypoints
 		self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)		
 
@@ -77,12 +87,11 @@ class Main:
 		if self.i >= len(self.xarray):
 			self.Point.x = 0
 			self.Point.y = 0
-
 		else:
-			self.Square_Point.x = self.xarray[self.i]
+			self.Point.x = self.xarray[self.i]
-			self.Square_Point.y = self.yarray[self.i]
+			self.Point.y = self.yarray[self.i]
 
-		self.Point = self.Square_Point	
+		# self.Point = self.Square_Point	
 		self.j += 1
 		self.i = self.j // self.buffer
 
@@ -91,7 +100,7 @@ class Main:
 		self.Point.x = self.step_size # STEP (meters)
 
 	def user_feedback(self):
-		rospy.loginfo("Sending [Point x] %d [Point y] %d",
+		rospy.loginfo("Sending [Point x] %.2f [Point y] %.2f",
 									 self.Point.x, self.Point.y) 
 
 	def determine_test(self):

src/ref_signalGen.py

@@ -7,30 +7,38 @@ 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.msg import  RefSignal, LoadAngles, TetheredStatus
 from oscillation_ctrl.srv import WaypointTrack
 from geometry_msgs.msg import Pose, PoseStamped, Point, TwistStamped 
 from sensor_msgs.msg import Imu
 from mavros_msgs.msg import State
 
+class DesiredPoint():
+	def __init__(self,x,y,z):
+		self.x = x
+		self.y = y
+		self.z = z
+
 class Main:
 
 	def __init__(self):
 
 		# rate(s)
-		rate = 60 # rate for the publisher method, specified in Hz -- 10 Hz
+		rate = 50 # 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
+		# self.t 				= np.linspace(0, 1.0, 10) # Time array
 
 		# Message generation/ collection
 		self.vel_data	 = TwistStamped() # This is needed to get drone vel from gps
@@ -42,24 +50,15 @@ class Main:
 		self.dr_pos 	= Pose()
 		self.dr_vel		= self.vel_data.twist.linear
 		self.dr_acc		= self.imu_data.linear_acceleration
-
+	
-		# Get tether length
+		self.tetherL = self.get_tether()		
-		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
-			elif rospy.get_time() - self.tstart >= 10.0:
-				self.tetherL	= 0.0
-				break
-		
 # --------------------------------------------------------------------------------#		
-# 																	SUBSCRIBERS																		#
+# 							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('/status/twoBody_status', TetheredStatus, self.dronePos_cb)
+		# rospy.Subscriber('/status/twoBody_status', TetheredStatus, self.dronePos_cb)
 		rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb)
 		rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb)
 
@@ -79,11 +78,11 @@ class Main:
 # --------------------------------------------------------------------------------#
 
 		# Smooth path variables 
-		self.EPS_F 		= np.zeros(9)    # Final Epsilon/ signal
+		self.EPS_F 		= np.zeros(9)   # Final Epsilon/ signal
-		self.EPS_I 		= np.zeros(9)		 # Epsilon shapefilter
+		self.EPS_I 		= np.zeros(9)	# Epsilon shapefilter
 		
 		# Constants for smooth path generation
-		self.w_tune		= 3 # 3.13 works well? #########################################################################
+		self.w_tune		= 1 # 1 also works well :) 3.13 works well? #########################################################################
 		self.epsilon	= 0.7 								# Damping ratio
 		
 		# need exception if we do not have tether:
@@ -91,7 +90,7 @@ class Main:
 			self.wn = self.w_tune
 		else:
 			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
@@ -99,12 +98,7 @@ class Main:
 		self.k1	= (self.w_tune**4)/(self.k2*self.k3*self.k4)
 		
 		# For saturation:
-		self.jmax = [3, 3, 1]
+		self.max  =	[0, 8, 1.5, 3]  #[0, 10, 1.5, 3]
-		self.amax = [5, 5, 1]
-		self.vmax = [10, 10, 1]		#[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
  
 		self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)		
 		self.empty_point = Point() # Needed to query waypoint_server
@@ -146,9 +140,11 @@ class Main:
 		# 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.sk		= self.SF_delay_x.shape[1]  # from Klausen 2017
-		self.ak 	= np.zeros(len(self.SF_delay_x[0]))
+		self.ak 	= np.zeros([self.sk])
 		self.s_gain	= 0.0 # Gain found from sigmoid	
+
+		self.service_list = rosservice.get_service_list()
 # --------------------------------------------------------------------------------#		
 # 								CALLBACK FUNCTIONS
 # --------------------------------------------------------------------------------#
@@ -156,7 +152,7 @@ class Main:
 	# Callback to get link names, states, and pload deflection angles
 	def loadAngles_cb(self,msg):
 		try:
-			self.load_angles = msg							
+			self.load_angles = msg						
 		except ValueError:
 			pass
 
@@ -164,7 +160,7 @@ class Main:
 	def dronePos_cb(self,msg): 
 		try:
 			self.dr_pos 	= msg.pose
-			#self.dr_pos 	= msg.drone_pos
+			# self.dr_pos 	= msg.drone_pos
 
 		except ValueError:
 			pass
@@ -186,34 +182,46 @@ class Main:
 			pass
 
 	def waypoints_srv_cb(self):
-		rospy.wait_for_service('/status/waypoint_tracker')
+		if '/status/waypoint_tracker' in self.service_list:
-		try:
+			rospy.wait_for_service('/status/waypoint_tracker')
-			resp = self.get_xd(False,self.empty_point)
+			try:
-			self.xd = resp.xd
+				resp = self.get_xd(False,self.empty_point)
-		except ValueError:
+				self.xd = resp.xd
-			pass
+			except ValueError:
-										
+				pass
-######################################################################
+		else:
-#TODO Will need to add a function that gets a message from  		 #
+			self.xd = DesiredPoint(0.0,0.0,2.0)
-#			a node which lets refsignal_gen.py know there has been a #
-#  		change in xd and therefore runs waypoints_srv_cb again		 #
-######################################################################
 
+	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 >= 15:
+				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
+		if abs(error) <= 0.25: error = 0.0
+
 		# Derivative of statesape array:		
 		dydt = [vel, acc, jer, 
-						self.k4*(self.k3*(self.k2*(self.k1*(xd - pos) - vel) - acc) - jer)]
+						self.k4*(self.k3*(self.k2*(self.k1*(error) - vel) - acc) - jer)]
 		return dydt
 
 	# Sigmoid
@@ -244,7 +252,7 @@ class Main:
 			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,:] = 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
@@ -285,12 +293,13 @@ class Main:
 
 	# 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
+		SHAPEFIL = 1
-		feedBack = 2
+		FEEDBACK = 2
 
 		# SOLVE ODE (get ref pos, vel, accel)
 		self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd.x,))
@@ -304,25 +313,26 @@ class Main:
 			
 		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.delay(j,SHAPEFIL) # Determine the delay (shapefilter) 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]   = self.x[-1,j] 
-				self.EPS_I[3*j+1] = self.y[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]   = 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.EPS_I[3*j+2] = self.z[1,j]  									   # No need to convolute z-dim
 
-			self.delay(1,feedBack) # Detemine feedback array
+			self.delay(1,FEEDBACK) # Detemine feedback array
 			
 		self.sigmoid()        # Determine sigmoid gain
 		EPS_D = self.fback()  # Feedback Epsilon
 
-		for i in range(9): 
+		# for i in range(9): 
 			# Populate EPS_F buffer with desired change based on feedback
-			self.EPS_F[i] = self.EPS_I[i] + self.s_gain*EPS_D[i] #+ EPS_D[i]
+			# self.EPS_F[i] = self.EPS_I[i] + self.s_gain*EPS_D[i] #+ EPS_D[i]
+		self.EPS_F = self.EPS_I + self.s_gain*EPS_D
 
 		# Populate msg with epsilon_final
 		self.ref_sig.header.stamp = rospy.Time.now()
@@ -342,15 +352,18 @@ class Main:
 		#self.ref_sig.velocity.z	= self.EPS_F[5]
 		#self.ref_sig.acceleration.z	= self.EPS_F[8]
 
-		self.x0 = [self.dr_pos.position.x, self.x[1,1], self.x[1,2], self.x[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.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.z0 = [self.dr_pos.position.z, self.z[1,1], self.z[1,2], self.z[1,3]]
+		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.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]
@@ -367,13 +380,14 @@ class Main:
 	def screen_output(self):
 
 		# Feedback to user
-		rospy.loginfo(' Var |   x   |   y   |   z   ')
+		# 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('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('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('Acc:    %.2f    %.2f    %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8])
-		rospy.loginfo('_______________________')
+		# rospy.loginfo('_______________________')
-
+		rospy.loginfo_once('Tether length: %.2f',self.tetherL)
-		#rospy.loginfo('xd = %.2f',self.xd.x)
+		# rospy.loginfo('Theta: %.2f',self.load_angles.theta)
+		# rospy.loginfo('Phi: %.2f',self.load_angles.phi)
 
 	def publisher(self,pub_tim):
 	
@@ -382,18 +396,16 @@ class Main:
 
 		# Publish reference signal
 		self.pub_path.publish(self.ref_sig)
-		#self.pub_ref.publish(self.ref_sig) # for geometric controller
 
 		# Give user feedback on published message:
-		#self.screen_output()
+		self.screen_output()
 
-		
 if __name__=="__main__":
 
 	# Initiate ROS node
 	rospy.init_node('desPath_node',anonymous=True)
 	try:
-		Main() 				# create class object
+		Main() 			# create class object
 		rospy.spin() 	# loop until shutdown signal
 
 	except rospy.ROSInterruptException:

src/wpoint_tracker.py

@@ -31,11 +31,17 @@ class Main:
 				self.xd.y = self.wpoints['y']
 				self.xd.z = self.wpoints['z']
 				self.param_exists = True
+			elif rospy.has_param('mocap/waypoints'):
+				self.wpoints	= rospy.get_param('mocap/waypoints') # get waypoints
+				self.xd.x = self.wpoints['x']
+				self.xd.y = self.wpoints['y']
+				self.xd.z = self.wpoints['z']
+				self.param_exists = True
 
-			elif rospy.get_time() - self.tstart >= 3.0:
+			elif rospy.get_time() - self.tstart >= 5.0:
 				self.xd.x = 0.0
 				self.xd.y = 0.0
-				self.xd.z = 5.0
+				self.xd.z = 1.5
 				break
 
 		# service(s)