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,))
yaw = 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,
'yaw':yaw, 'yaw_dot':yaw_dot}
'yaw':yaw, 'yaw_dot':yaw_dot, 'yaw_ddot':yaw_ddot}
return flat_output

4
rotorpy/trajectories/lissajous_traj.py
View File

@@ -66,10 +66,12 @@ class TwoDLissajous(object):
if self.yaw_bool:
yaw = np.pi/4*np.sin(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:
yaw = 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,
'yaw':yaw, 'yaw_dot':yaw_dot}
'yaw':yaw, 'yaw_dot':yaw_dot, 'yaw_ddot':yaw_ddot}
return flat_output

9
rotorpy/trajectories/minsnap.py
View File

@@ -158,6 +158,7 @@ class MinSnap(object):
self.x_dddot_poly = np.zeros((m, 3, 5))
self.x_ddddot_poly = np.zeros((m, 3, 4))
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 self.points.shape[0] >= 2:
@@ -203,7 +204,8 @@ class MinSnap(object):
self.x_ddot_poly[i,j,:] = np.polyder(self.x_poly[i,j,:], m=2)
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.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:
# Otherwise, there is only one waypoint so we just set everything = 0.
@@ -236,10 +238,12 @@ class MinSnap(object):
x_ddddot = np.zeros((3,))
yaw = 0
yaw_dot = 0
yaw_ddot = 0
if self.null:
# If there's only one waypoint
x = self.points[0,:]
yaw = self.yaw[0]
else:
# Find interval index i and time within interval t.
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_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,
'yaw':yaw, 'yaw_dot':yaw_dot}
'yaw':yaw, 'yaw_dot':yaw_dot, 'yaw_ddot':yaw_ddot}
return flat_output
if __name__=="__main__":

3
rotorpy/trajectories/polynomial_traj.py
View File

@@ -86,6 +86,7 @@ class Polynomial(object):
x_ddddot = np.zeros((3,))
yaw = 0
yaw_dot = 0
yaw_ddot = 0
# Find interval index i and time within interval t.
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)
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

3
rotorpy/trajectories/speed_traj.py
View File

@@ -70,9 +70,10 @@ class ConstantSpeed(object):
yaw = 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,
'yaw':yaw, 'yaw_dot':yaw_dot}
'yaw':yaw, 'yaw_dot':yaw_dot, 'yaw_ddot':yaw_ddot}
return flat_output
if __name__=="__main__":

3
rotorpy/trajectories/traj_template.py
View File

@@ -40,7 +40,8 @@ class TrajTemplate(object):
x_ddddot = np.zeros((3,))
yaw = 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,
'yaw':yaw, 'yaw_dot':yaw_dot}
'yaw':yaw, 'yaw_dot':yaw_dot, 'yaw_ddot':yaw_ddot}
return flat_output