swangnice/rotor_py_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added yaw_ddot to all trajectories to comply w/ update_ref

Browse Source
This commit is contained in:
spencerfolk
2023-07-13 17:16:09 -04:00
parent 10cd69f110
commit befd0809e6
6 changed files with 18 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
rotorpy/trajectories/hover_traj.py
View File

@@ -35,7 +35,8 @@ class HoverTraj(object):
x_ddddot = np.zeros((3,)) x_ddddot = np.zeros((3,))
yaw = 0 yaw = 0
yaw_dot = 0 yaw_dot = 0
yaw_ddot = 0
flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot, flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
'yaw':yaw, 'yaw_dot':yaw_dot} 'yaw':yaw, 'yaw_dot':yaw_dot, 'yaw_ddot':yaw_ddot}
return flat_output return flat_output

4
rotorpy/trajectories/lissajous_traj.py
View File

@@ -66,10 +66,12 @@ class TwoDLissajous(object):
if self.yaw_bool: if self.yaw_bool:
yaw = np.pi/4*np.sin(np.pi*t) yaw = np.pi/4*np.sin(np.pi*t)
yaw_dot = np.pi*np.pi/4*np.cos(np.pi*t) yaw_dot = np.pi*np.pi/4*np.cos(np.pi*t)
yaw_ddot = np.pi*np.pi*np.pi/4*np.cos(np.pi*t)
else: else:
yaw = 0 yaw = 0
yaw_dot = 0 yaw_dot = 0
yaw_ddot = 0
flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot, flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
'yaw':yaw, 'yaw_dot':yaw_dot} 'yaw':yaw, 'yaw_dot':yaw_dot, 'yaw_ddot':yaw_ddot}
return flat_output return flat_output

7
rotorpy/trajectories/minsnap.py
View File

@@ -158,6 +158,7 @@ class MinSnap(object):
self.x_dddot_poly = np.zeros((m, 3, 5)) self.x_dddot_poly = np.zeros((m, 3, 5))
self.x_ddddot_poly = np.zeros((m, 3, 4)) self.x_ddddot_poly = np.zeros((m, 3, 4))
self.yaw_dot_poly = np.zeros((m, 1, 7)) self.yaw_dot_poly = np.zeros((m, 1, 7))
self.yaw_ddot_poly = np.zeros((m, 1, 6))
# If two or more waypoints remain, solve min snap # If two or more waypoints remain, solve min snap
if self.points.shape[0] >= 2: if self.points.shape[0] >= 2:
@@ -204,6 +205,7 @@ class MinSnap(object):
self.x_dddot_poly[i,j,:] = np.polyder(self.x_poly[i,j,:], m=3) self.x_dddot_poly[i,j,:] = np.polyder(self.x_poly[i,j,:], m=3)
self.x_ddddot_poly[i,j,:] = np.polyder(self.x_poly[i,j,:], m=4) self.x_ddddot_poly[i,j,:] = np.polyder(self.x_poly[i,j,:], m=4)
self.yaw_dot_poly[i,0,:] = np.polyder(self.yaw_poly[i,0,:], m=1) self.yaw_dot_poly[i,0,:] = np.polyder(self.yaw_poly[i,0,:], m=1)
self.yaw_ddot_poly[i,0,:] = np.polyder(self.yaw_poly[i,0,:], m=2)
else: else:
# Otherwise, there is only one waypoint so we just set everything = 0. # Otherwise, there is only one waypoint so we just set everything = 0.
@@ -236,10 +238,12 @@ class MinSnap(object):
x_ddddot = np.zeros((3,)) x_ddddot = np.zeros((3,))
yaw = 0 yaw = 0
yaw_dot = 0 yaw_dot = 0
yaw_ddot = 0
if self.null: if self.null:
# If there's only one waypoint # If there's only one waypoint
x = self.points[0,:] x = self.points[0,:]
yaw = self.yaw[0]
else: else:
# Find interval index i and time within interval t. # Find interval index i and time within interval t.
t = np.clip(t, self.t_keyframes[0], self.t_keyframes[-1]) t = np.clip(t, self.t_keyframes[0], self.t_keyframes[-1])
@@ -258,9 +262,10 @@ class MinSnap(object):
yaw = np.polyval(self.yaw_poly[i, 0, :], t) yaw = np.polyval(self.yaw_poly[i, 0, :], t)
yaw_dot = np.polyval(self.yaw_dot_poly[i,0,:], t) yaw_dot = np.polyval(self.yaw_dot_poly[i,0,:], t)
yaw_ddot = np.polyval(self.yaw_ddot_poly[i,0,:], t)
flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot, flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
'yaw':yaw, 'yaw_dot':yaw_dot} 'yaw':yaw, 'yaw_dot':yaw_dot, 'yaw_ddot':yaw_ddot}
return flat_output return flat_output
if __name__=="__main__": if __name__=="__main__":

3
rotorpy/trajectories/polynomial_traj.py
View File

@@ -86,6 +86,7 @@ class Polynomial(object):
x_ddddot = np.zeros((3,)) x_ddddot = np.zeros((3,))
yaw = 0 yaw = 0
yaw_dot = 0 yaw_dot = 0
yaw_ddot = 0
# Find interval index i and time within interval t. # Find interval index i and time within interval t.
t = np.clip(t, self.t_start[0], self.t_start[-1]+self.T[-1]) t = np.clip(t, self.t_start[0], self.t_start[-1]+self.T[-1])
@@ -103,5 +104,5 @@ class Polynomial(object):
x_ddddot[j] = np.polyval(self.x_ddddot_poly[i,j,:], t) x_ddddot[j] = np.polyval(self.x_ddddot_poly[i,j,:], t)
flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot, flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
'yaw':yaw, 'yaw_dot':yaw_dot} 'yaw':yaw, 'yaw_dot':yaw_dot, 'yaw_ddot':yaw_ddot}
return flat_output return flat_output

3
rotorpy/trajectories/speed_traj.py
View File

@@ -70,9 +70,10 @@ class ConstantSpeed(object):
yaw = 0 yaw = 0
yaw_dot = 0 yaw_dot = 0
yaw_ddot = 0
flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot, flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
'yaw':yaw, 'yaw_dot':yaw_dot} 'yaw':yaw, 'yaw_dot':yaw_dot, 'yaw_ddot':yaw_ddot}
return flat_output return flat_output
if __name__=="__main__": if __name__=="__main__":

3
rotorpy/trajectories/traj_template.py
View File

@@ -40,7 +40,8 @@ class TrajTemplate(object):
x_ddddot = np.zeros((3,)) x_ddddot = np.zeros((3,))
yaw = 0 yaw = 0
yaw_dot = 0 yaw_dot = 0
yaw_ddot = 0
flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot, flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
'yaw':yaw, 'yaw_dot':yaw_dot} 'yaw':yaw, 'yaw_dot':yaw_dot, 'yaw_ddot':yaw_ddot}
return flat_output return flat_output