cesar.alejandro
/
oscillation_ctrl
1
0
Fork 1
Code Issues Pull Requests Projects Releases Wiki Activity

Removed unused dependencies

This commit is contained in:
cesar.alejnadro 2022-04-05 16:08:28 -03:00
parent 4682d2fb52
commit a1dcc77175
6 changed files with 110 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -1,4 +1,5 @@
launch/debug.launch launch/debug.launch
launch/mocap_*
launch/cortex_bridge.launch launch/cortex_bridge.launch
src/MoCap_Localization_*.py src/MoCap_Localization_*.py
src/Mocap_*.py src/Mocap_*.py
@ -6,5 +7,6 @@ src/killswitch_client.py
src/land_client.py src/land_client.py
msg/Marker.msg msg/Marker.msg
msg/Markers.msg msg/Markers.msg
launc
*.rviz *.rviz

1
CMakeLists.txt
View File

@ -54,6 +54,7 @@ add_message_files(
tethered_status.msg tethered_status.msg
RefSignal.msg RefSignal.msg
EulerAngles.msg EulerAngles.msg
LoadAngles.msg
) )
## Generate services in the 'srv' folder ## Generate services in the 'srv' folder

2
launch/klausen_dampen.launch
View File

@ -31,11 +31,13 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
pkg="oscillation_ctrl" pkg="oscillation_ctrl"
type="wpoint_tracker.py" type="wpoint_tracker.py"
name="waypoints_server" name="waypoints_server"
launch-prefix="xterm -e"
/> />
<node <node
pkg="oscillation_ctrl" pkg="oscillation_ctrl"
type="ref_signalGen.py" type="ref_signalGen.py"
name="refSignal_node" name="refSignal_node"
launch-prefix="xterm -e"
/> />
<node <node
pkg="oscillation_ctrl" pkg="oscillation_ctrl"

79
src/LinkState.py
View File

@ -8,7 +8,7 @@ import rosservice
import time import time
import math import math
from tf.transformations import * from tf.transformations import *
from offboard_ex.msg import tethered_status from oscillation_ctrl.msg import tethered_status, LoadAngles
from geometry_msgs.msg import Pose from geometry_msgs.msg import Pose
from gazebo_msgs.srv import GetLinkState from gazebo_msgs.srv import GetLinkState
from std_msgs.msg import Bool from std_msgs.msg import Bool
@ -35,32 +35,31 @@ class Main:
self.wait = rospy.get_param('sim/wait') # wait time self.wait = rospy.get_param('sim/wait') # wait time
self.param_exists = True self.param_exists = True
elif rospy.get_time() - self.tstart >= 3.0: elif rospy.get_time() - self.tstart >= 3.0:
self.wait = 0.0
break break
# Will be set to true when test should start # Will be set to true when test should start
self.bool = False self.bool = False
# initialize variables
self.phi = 0.0 # Payload angle of deflection from x-axis
self.theta = 0.0 # Payload angle of deflection from y-axis
self.tetherL = 0.0 # Tether length
self.has_run = 0 # Boolean to keep track of first run instance
self.phidot = 0.0 #
self.thetadot = 0.0 #
self.phibuf = 0.0 # Need buffers to determine their rates
self.thetabuf = 0.0 #
self.pload = True # Check if payload exists
# Max dot values to prevent 'blowup'
self.phidot_max = 3.0
self.thetadot_max = 3.0
# Vehicle is spawned with yaw offset for convenience, need to deal with that # Vehicle is spawned with yaw offset for convenience, need to deal with that
self.yaw_offset = 0.0 self.yaw_offset = 0.0
# variables for message gen # variables for message gen
self.status = tethered_status() self.status = tethered_status()
self.status.drone_id = 'spiri_with_tether::spiri::base' self.drone_id = 'spiri_with_tether::spiri::base'
self.status.pload_id = 'spiri_with_tether::mass::payload' self.pload_id = 'spiri_with_tether::mass::payload'
self.loadAngles = LoadAngles()
# initialize variables
self.tetherL = 0.0 # Tether length
self.has_run = 0 # Boolean to keep track of first run instance
self.phibuf = 0.0 # Need buffers to determine their rates
self.thetabuf = 0.0 #
self.pload = True # Check if payload exists
# Max dot values to prevent 'blowup'
self.phidot_max = 3.0
self.thetadot_max = 3.0
# service(s) # service(s)
self.service1 = '/gazebo/get_link_state' self.service1 = '/gazebo/get_link_state'
@ -69,7 +68,8 @@ class Main:
rospy.wait_for_service(self.service1,timeout=10) rospy.wait_for_service(self.service1,timeout=10)
# publisher(s) # publisher(s)
self.publisher = rospy.Publisher('/status/twoBody_status', tethered_status, queue_size=1) self.twobody_pub = rospy.Publisher('/status/twoBody_status', tethered_status, queue_size=1)
self.loadAng_pub = rospy.Publisher('/status/load_angles', LoadAngles, queue_size=1)
self.pub_wd = rospy.Publisher('/status/path_follow', Bool, queue_size=1) self.pub_wd = rospy.Publisher('/status/path_follow', Bool, queue_size=1)
self.pub_timer = rospy.Timer(rospy.Duration(1.0/rate), self.link_state) self.pub_timer = rospy.Timer(rospy.Duration(1.0/rate), self.link_state)
@ -120,18 +120,18 @@ class Main:
# Establish links needed --> Spiri base and payload # Establish links needed --> Spiri base and payload
# P = Position vector # P = Position vector
drone_P = get_P(self.status.drone_id,reference) drone_P = get_P(self.drone_id,reference)
# Get orientation of drone in euler angles to determine yaw offset # Get orientation of drone in euler angles to determine yaw offset
drone_Eul = self.euler_array(drone_P.link_state.pose.orientation) drone_Eul = self.euler_array(drone_P.link_state.pose.orientation)
# Check if payload is part of simulation # Check if payload is part of simulation
if not drone_P.success: if not drone_P.success:
self.status.drone_id = 'spiri::base' self.drone_id = 'spiri::base'
drone_P = get_P(self.status.drone_id,reference) # i.e. no payload drone_P = get_P(self.drone_id,reference) # i.e. no payload
self.pload = False self.pload = False
pload_P = get_P(self.status.pload_id,reference) pload_P = get_P(self.pload_id,reference)
if not self.has_run == 1: if not self.has_run == 1:
if self.pload == True: if self.pload == True:
@ -184,30 +184,30 @@ class Main:
x_sep = pload_Px - drone_Px x_sep = pload_Px - drone_Px
if math.fabs(x_sep) >= self.tetherL or x_sep == 0: if math.fabs(x_sep) >= self.tetherL or x_sep == 0:
self.theta = 0 self.loadAngles.theta = 0
else: else:
self.theta = math.asin(x_sep/self.tetherL) self.loadAngles.theta = math.asin(x_sep/self.tetherL)
# Determine thetadot # Determine thetadot
self.thetadot = (self.theta - self.thetabuf)/self.dt self.loadAngles.thetadot = (self.loadAngles.theta - self.thetabuf)/self.dt
self.thetadot = self.cutoff(self.thetadot,self.thetadot_max) self.loadAngles.thetadot = self.cutoff(self.loadAngles.thetadot,self.thetadot_max)
self.thetabuf = self.theta self.thetabuf = self.loadAngles.theta
# Determine phi (roll) # Determine phi (roll)
y_sep = pload_Py - drone_Py y_sep = pload_Py - drone_Py
if math.fabs(y_sep) >= self.tetherL or y_sep == 0: if math.fabs(y_sep) >= self.tetherL or y_sep == 0:
self.phi = 0 self.loadAngles.phi = 0
else: else:
self.phi = math.asin(y_sep/self.tetherL) self.loadAngles.phi = math.asin(y_sep/self.tetherL)
# Determine phidot # Determine phidot
self.phidot = (self.phi - self.phibuf)/self.dt self.loadAngles.phidot = (self.loadAngles.phi - self.phibuf)/self.dt
self.phidot = self.cutoff(self.phidot,self.phidot_max) self.loadAngles.phidot = self.cutoff(self.loadAngles.phidot,self.phidot_max)
self.phibuf = self.phi # Update buffer self.phibuf = self.loadAngles.phi # Update buffer
else: # Otherwise, vars = 0 else: # Otherwise, vars = 0
x_sep = self.phi = self.phidot = self.theta = self.thetadot = 0 x_sep = self.loadAngles.phi = self.loadAngles.phidot = self.loadAngles.theta = self.thetadot = 0
# Print and save results # Print and save results
print "\n" print "\n"
@ -216,21 +216,18 @@ class Main:
print "drone pos.x: " + str(round(drone_Px,2)) print "drone pos.x: " + str(round(drone_Px,2))
print "drone pos.y: " + str(round(drone_Py,2)) print "drone pos.y: " + str(round(drone_Py,2))
print "drone pos.z: " + str(round(drone_Pz,2)) print "drone pos.z: " + str(round(drone_Pz,2))
print "phi: " + str(round(self.phi*180/3.14,3)) print "phi: " + str(round(self.loadAngles.phi*180/3.14,3))
print "theta: " + str(round(self.theta*180/3.14,3)) print "theta: " + str(round(self.loadAngles.theta*180/3.14,3))
# Populate message # Populate message
self.status.header.stamp = rospy.Time.now() self.status.header.stamp = rospy.Time.now()
self.status.drone_pos = drone_P.link_state.pose self.status.drone_pos = drone_P.link_state.pose
self.status.pload_pos = pload_P.link_state.pose self.status.pload_pos = pload_P.link_state.pose
self.status.length = self.tetherL self.loadAngles.header.stamp = rospy.Time.now()
self.status.phi = self.phi
self.status.phidot = self.phidot
self.status.theta = self.theta
self.status.thetadot = self.thetadot
# Publish message # Publish message
self.publisher.publish(self.status) self.twobody_pub.publish(self.status)
self.loadAng_pub.publish(self.loadAngles)
except rospy.ServiceException as e: except rospy.ServiceException as e:
rospy.loginfo("Get Link State call failed: {0}".format(e)) rospy.loginfo("Get Link State call failed: {0}".format(e))

58
src/klausen_control.py
View File

@ -14,7 +14,7 @@ import math
from tf.transformations import * from tf.transformations import *
from scipy.integrate import odeint from scipy.integrate import odeint
from offboard_ex.msg import tethered_status, RefSignal from oscillation_ctrl.msg import tethered_status, RefSignal, LoadAngles
from controller_msgs.msg import FlatTarget from controller_msgs.msg import FlatTarget
from geometry_msgs.msg import Point, Pose from geometry_msgs.msg import Point, Pose
from geometry_msgs.msg import TwistStamped, Vector3, Vector3Stamped, PoseStamped, Quaternion from geometry_msgs.msg import TwistStamped, Vector3, Vector3Stamped, PoseStamped, Quaternion
@ -46,19 +46,14 @@ class Main:
self.path_pos = np.zeros([3,1]) self.path_pos = np.zeros([3,1])
self.path_vel = np.zeros([3,1]) self.path_vel = np.zeros([3,1])
self.path_acc = np.zeros([3,1]) self.path_acc = np.zeros([3,1])
self.pl_pos = Pose()
self.dr_pos = Pose() self.dr_pos = Pose()
self.quaternion = PoseStamped() self.quaternion = PoseStamped()
self.load_angles = LoadAngles()
self.EulerAng = [0,0,0] # Will find the euler angles, and then convert to q self.EulerAng = [0,0,0] # Will find the euler angles, and then convert to q
# Drone var # Drone var
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.has_run = 0 # Bool to keep track of first run instance
self.drone_id = ''
self.pload_id = ''
# Col1 = theta, theta dot; Col2 = phi, phidot for self.PHI # Col1 = theta, theta dot; Col2 = phi, phidot for self.PHI
self.PHI = np.array([[0,0],[0,0]]) self.PHI = np.array([[0,0],[0,0]])
self.dr_vel = np.zeros([3,1]) self.dr_vel = np.zeros([3,1])
@ -105,7 +100,7 @@ class Main:
# SUBSCRIBERS # SUBSCRIBERS
# --------------------------------------------------------------------------------# # --------------------------------------------------------------------------------#
# Topic, msg type, and class callback method # Topic, msg type, and class callback method
rospy.Subscriber('/status/twoBody_status', tethered_status, self.linkState_cb) rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb)
rospy.Subscriber('/reference/path', FlatTarget, self.refsig_cb) rospy.Subscriber('/reference/path', FlatTarget, self.refsig_cb)
rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb)
rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb)
@ -125,21 +120,22 @@ class Main:
# CALLBACK FUNCTIONS # CALLBACK FUNCTIONS
# --------------------------------------------------------------------------------# # --------------------------------------------------------------------------------#
# Callback to get link names, states, and pload deflection angles # Callback pload deflection angles and vel
def linkState_cb(self,msg): def loadAngles_cb(self,msg):
try: try:
self.drone_id = msg.drone_id self.load_angles = msg
self.pload_id = msg.pload_id
self.dr_pos = msg.drone_pos
self.pl_pos = msg.pload_pos
#self.tetherL = msg.length
self.phi = msg.phi
self.phidot = msg.phidot
self.theta = msg.theta
self.thetadot = msg.thetadot
# Populate self.PHI # Populate self.PHI
self.PHI = np.array([[self.theta,self.thetadot],[self.phi,self.phidot]]) 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): ### NEED to add mavros/local_pos.. sub
try:
self.dr_pos = msg.pose
except ValueError: except ValueError:
pass pass
@ -195,14 +191,14 @@ class Main:
def control(self): def control(self):
# Populate position vector and gamma (g). g is state space vector: [px,py,pz,theta,phi] # 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]]) 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.theta],[self.phi]]) 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]])
# State some variables for shorthand # State some variables for shorthand
L = self.tetherL L = self.tetherL
c_theta = math.cos(self.theta) c_theta = math.cos(self.load_angles.theta)
c_phi = math.cos(self.phi) c_phi = math.cos(self.load_angles.phi)
s_theta = math.sin(self.theta) s_theta = math.sin(self.load_angles.theta)
s_phi = math.sin(self.phi) s_phi = math.sin(self.load_angles.phi)
# Check for tether # Check for tether
if L <= 0.01: if L <= 0.01:
@ -224,15 +220,15 @@ class Main:
[L*self.pl_m*c_theta, L*self.pl_m*s_phi*s_theta, -L*self.pl_m*c_phi*s_theta, 0, L**2*self.pl_m]] [L*self.pl_m*c_theta, L*self.pl_m*s_phi*s_theta, -L*self.pl_m*c_phi*s_theta, 0, L**2*self.pl_m]]
C = [[0,0,0,0,-L*self.thetadot*self.pl_m*s_theta], C = [[0,0,0,0,-L*self.load_angles.thetadot*self.pl_m*s_theta],
[0,0,0,L*self.pl_m*(self.phidot*c_theta*s_phi + self.thetadot*c_phi*s_theta), L*self.pl_m*(self.phidot*c_phi*s_theta + self.thetadot*c_theta*s_phi)], [0,0,0,L*self.pl_m*(self.load_angles.phidot*c_theta*s_phi + self.load_angles.thetadot*c_phi*s_theta), L*self.pl_m*(self.load_angles.phidot*c_phi*s_theta + self.load_angles.thetadot*c_theta*s_phi)],
[0,0,0,-L*self.pl_m*(self.phidot*c_phi*c_theta - self.thetadot*s_phi*s_theta),-L*self.pl_m*(self.thetadot*c_phi*c_theta - self.phidot*s_phi*s_theta)], [0,0,0,-L*self.pl_m*(self.load_angles.phidot*c_phi*c_theta - self.load_angles.thetadot*s_phi*s_theta),-L*self.pl_m*(self.load_angles.thetadot*c_phi*c_theta - self.load_angles.phidot*s_phi*s_theta)],
[0,0,0,-0.5*(L**2)*self.pl_m*self.thetadot*math.sin(2*self.theta) + 0.5*0.01*self.thetadot*math.sin(2*self.theta), -0.5*(L**2)*self.pl_m*self.phidot*math.sin(2*self.theta)], [0,0,0,-0.5*(L**2)*self.pl_m*self.load_angles.thetadot*math.sin(2*self.load_angles.theta) + 0.5*0.01*self.load_angles.thetadot*math.sin(2*self.load_angles.theta), -0.5*(L**2)*self.pl_m*self.load_angles.phidot*math.sin(2*self.load_angles.theta)],
[0,0,0,0.5*(L**2)*self.pl_m*self.phidot*math.sin(2*self.theta),0]] [0,0,0,0.5*(L**2)*self.pl_m*self.load_angles.phidot*math.sin(2*self.load_angles.theta),0]]
G = [[0],[0],[-9.81*self.tot_m],[L*9.81*self.pl_m*c_theta*s_phi],[L*9.81*self.pl_m*c_phi*s_theta]] 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]]

58
src/ref_signalGen.py
View File

@ -12,7 +12,7 @@ from pymavlink import mavutil
from scipy import signal from scipy import signal
from scipy.integrate import odeint from scipy.integrate import odeint
from oscillation_ctrl.msg import tethered_status, RefSignal from oscillation_ctrl.msg import tethered_status, RefSignal, LoadAngles
from oscillation_ctrl.srv import WaypointTrack, WaypointTrackRequest from oscillation_ctrl.srv import WaypointTrack, WaypointTrackRequest
from controller_msgs.msg import FlatTarget from controller_msgs.msg import FlatTarget
from geometry_msgs.msg import Pose, Vector3, PoseStamped, Point, TwistStamped from geometry_msgs.msg import Pose, Vector3, PoseStamped, Point, TwistStamped
@ -44,17 +44,9 @@ class Main:
self.vel_data = TwistStamped() # This is needed to get drone vel from gps 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.imu_data = Imu() # Needed for to get drone acc from IMU
self.ref_sig = FlatTarget() # Smooth Signal self.ref_sig = FlatTarget() # Smooth Signal
self.ref_sig.position.z = 5.0 # This does not need to be determined self.load_angles = LoadAngles()
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.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_pos = Pose()
self.dr_vel = self.vel_data.twist.linear self.dr_vel = self.vel_data.twist.linear
self.dr_acc = self.imu_data.linear_acceleration self.dr_acc = self.imu_data.linear_acceleration
@ -74,7 +66,8 @@ class Main:
# SUBSCRIBERS # # SUBSCRIBERS #
# --------------------------------------------------------------------------------# # --------------------------------------------------------------------------------#
# Topic, msg type, and class callback method # Topic, msg type, and class callback method
rospy.Subscriber('/status/twoBody_status', tethered_status, self.linkState_cb) 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/local_position/velocity_body', TwistStamped, self.droneVel_cb)
rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb)
rospy.Subscriber('/mavros/state', State, self.state_cb) rospy.Subscriber('/mavros/state', State, self.state_cb)
@ -198,17 +191,16 @@ class Main:
# mavros publishes a disconnected state message on init # mavros publishes a disconnected state message on init
# Callback to get link names, states, and pload deflection angles # Callback to get link names, states, and pload deflection angles
def linkState_cb(self,msg): def loadAngles_cb(self,msg):
try: try:
self.drone_id = msg.drone_id self.load_angles = msg
self.pload_id = msg.pload_id except ValueError:
self.dr_pos = msg.drone_pos pass
self.pl_pos = msg.pload_pos
self.phi = -msg.phi # Callback drone pose
self.phidot = -msg.phidot def dronePos_cb(self,msg): ### NEED to add mavros/local_pos.. sub
self.theta = msg.theta try:
self.thetadot = msg.thetadot self.dr_pos = msg.pose
# self.tetherL = msg.length
except ValueError: except ValueError:
pass pass
@ -298,13 +290,13 @@ class Main:
elif cmd == 2: # Feedback elif cmd == 2: # Feedback
if self.FB_idx < len(self.theta_fb): if self.FB_idx < len(self.theta_fb):
# First, fill up the delay array # First, fill up the delay array
self.theta_fb[self.FB_idx] = self.theta self.theta_fb[self.FB_idx] = self.load_angles.theta
self.theta_vel_fb[self.FB_idx] = self.thetadot self.theta_vel_fb[self.FB_idx] = self.load_angles.thetadot
self.theta_acc_fb[self.FB_idx] = self.thetadot - self.theta_vel_fb[self.FB_idx-1] self.theta_acc_fb[self.FB_idx] = self.load_angles.thetadot - self.theta_vel_fb[self.FB_idx-1]
self.phi_fb[self.FB_idx] = self.phi self.phi_fb[self.FB_idx] = self.load_angles.phi
self.phi_vel_fb[self.FB_idx] = self.phidot self.phi_vel_fb[self.FB_idx] = self.load_angles.phidot
self.phi_acc_fb[self.FB_idx] = self.phidot - self.phi_vel_fb[self.FB_idx-1] self.phi_acc_fb[self.FB_idx] = self.load_angles.phidot - self.phi_vel_fb[self.FB_idx-1]
else: else:
# once array is filled, need to shift values w/ latest value at end # once array is filled, need to shift values w/ latest value at end
@ -316,13 +308,13 @@ class Main:
self.phi_vel_fb[:] = np.roll(self.phi_vel_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.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_fb[len(self.theta_fb)-1] = self.load_angles.theta # change final value
self.theta_vel_fb[len(self.theta_fb)-1] = self.thetadot self.theta_vel_fb[len(self.theta_fb)-1] = self.load_angles.thetadot
self.theta_acc_fb[len(self.theta_fb)-1] = self.thetadot - self.theta_vel_fb[len(self.theta_fb)-1] self.theta_acc_fb[len(self.theta_fb)-1] = self.load_angles.thetadot - self.theta_vel_fb[len(self.theta_fb)-1]
self.phi_fb[len(self.phi_fb)-1] = self.phi self.phi_fb[len(self.phi_fb)-1] = self.load_angles.phi
self.phi_vel_fb[len(self.theta_fb)-1] = self.phidot self.phi_vel_fb[len(self.theta_fb)-1] = self.load_angles.phidot
self.phi_acc_fb[len(self.theta_fb)-1] = self.phidot - self.phi_vel_fb[len(self.theta_fb)-1] self.phi_acc_fb[len(self.theta_fb)-1] = self.load_angles.phidot - self.phi_vel_fb[len(self.theta_fb)-1]
else: else:
print('No delay') print('No delay')