def position(self, t=None):
"""
Args:
t (float, optional):
time [s]
Returns:
Value of self._position if t is None and if self._position is not
None. Otherwise the actual position is calculated as function of
time [m, m, m]
Note:
The calculated position is NOT stored as 'self._position'
"""
if t is None and self._position is not None:
return self._position
if self._wayPoints is None:
return self._position
if t is None or t < 0.:
t = 0.
if t >= self._wayPoints[-1].t:
P = self._wayPoints[-1]
return xyz(P.x, P.y, P.z)
# P = P' + (P"-P') * (t-t') / (t"-t')
iAhead = self.iWayPointAhead(t)
DP = self._wayPoints[iAhead] - self._wayPoints[iAhead - 1]
dt = t - self._wayPoints[iAhead - 1].t
Dt = self._wayPoints[iAhead].t - self._wayPoints[iAhead - 1].t
P = self._wayPoints[iAhead - 1] + DP * (dt / Dt)
return xyz(P.x, P.y, P.z)
def setTrajectory(self, way, rot=None, speed=None, tBegin=0, tEnd=None):
"""
Defines list of waypoints
Args:
way (array_like of xyz or xyzt):
way points in 3D space [m, m, m] or [m, m, m, s]
rot (array_like of xyz):
rotation of waypoints in 3D space [rad], size can be 0, 1, or
length of wayPoints
speed (float, optional):
constant magnitude of object velocity [m/s]
tBegin (float, optional):
start time [s]
tEnd (float, optional):
end time [s]
"""
self._wayPoints = list(way)
if len(self._wayPoints) <= 1:
return
self._wayPoints = [xyzt(point=P) for P in self._wayPoints]
way = [0.]
for i in range(1, len(self._wayPoints)):
Dl = (self._wayPoints[i] - self._wayPoints[i - 1]).magnitude()
way.append(way[i - 1] + Dl)
if rot is None or len(rot) == 0:
rot = [xyz()]
assert len(rot) == 1 or len(rot) == len(self._wayPoints)
if len(rot) == 1:
self._rotations = rot * len(self._wayPoints)
else:
self._rotations = [xyz(point=r) for r in rot]
if speed is None:
if tEnd is None or isclose(tEnd, 0.):
for P in self._wayPoints:
P.t = 0.
return
speed = way[-1] / tEnd
self._wayPoints[0].t = tBegin
for i in range(1, len(self._wayPoints)):
dt = (way[i] - way[i - 1]) / speed
self._wayPoints[i].t = self._wayPoints[i - 1].t + dt
self._position = self._wayPoints[0]
self._velocity = self.velocity(0)
self._rotation = self.rotation(0)
del way
def __init__(self, identifier='Move'):
super().__init__(identifier=identifier)
self._wayPoints = None # array of wayPoints [m, m, m, s]
self._rotations = None # rotations in 3D space [rad]
self._position = xyz() # object position in 3D space [m]
self._velocity = xyz() # velocity in 3D space [m/s]
self._rotation = xyz() # rotation in 3D space [rad]
self._iLastWayPoint = 0 # index of last passed way point
self._trajectoryHistory = None # plot data
def __init__(self, identifier: str = '?') -> None:
self.identifier = identifier
self.points = OrderedDict()
self.points['C'] = xyz(0., 0., 0.)
self.points['E'] = xyz(1., 0., 0.)
self.points['N'] = xyz(0., 1., 0.)
self.rotAxis = None
self.wayToNext = None
self.prev = None
self.next = None
def extractRotationData(self) -> Tuple[float, float, float]:
"""
Extracts rotation data from 'rotAxis' list which must contain one
string and two floats; the rotation is around the axis for which
the angle is given as string;
angle string can optionally end with 'deg' or 'rad';
in case of 'deg', the angle is converted to radians
Returns:
modified rotation axis (angle string replaced with 'None'),
bending radius [m], rotation angle [rad];
('None','None','None') is returned if rotAxis equals 'None'
Example:
rotAxis: [1.2, '90deg', 2.3] -> rotation around y-axis with
an angle of 90 degrees, rotation center is (x=1.2, z=2.3)
"""
if self.rotAxis is None or len(self.rotAxis) < 3:
return (None, None, None)
if isinstance(self.rotAxis[0], str):
i = 0
elif isinstance(self.rotAxis[1], str):
i = 1
elif isinstance(self.rotAxis[2], str):
i = 2
else:
self.write('??? rotAxis does not contain the angle string')
return (None, None, None)
j = self.rotAxis[i].lower().find('deg')
if j != -1:
phiInRad = radians(float(self.rotAxis[i][:j]))
else:
j = self.rotAxis[i].lower().find('rad')
if j != -1:
phiInRad = float(self.rotAxis[i][:j])
else:
phiInRad = float(self.rotAxis[i])
rot = deepcopy(self.rotAxis)
rot[i] = None
if i == 0:
rBend = (self.C - xyz(self.C.x, rot[1], rot[2])).magnitude()
elif i == 1:
rBend = (self.C - xyz(rot[0], self.C.y, rot[2])).magnitude()
elif i == 2:
rBend = (self.C - xyz(rot[0], rot[1], self.C.z)).magnitude()
return (rot, rBend, phiInRad)
def velocity(self, t=None):
"""
Args:
t (float, optional):
time [s]
Returns:
(xyz):
Value of self._velocity if 't' is None and self._velocity is
not None. Otherwise the actual velocity is calculated as
function of time [m/s]
Note:
The calculated velocity is NOT stored as 'self._velocity'
"""
if t is None:
if self._velocity is not None:
return self._velocity
t = 0.
if self._wayPoints is None:
return xyz()
iAhead = self.iWayPointAhead(t)
DP = self._wayPoints[iAhead] - self._wayPoints[iAhead - 1]
Dt = self._wayPoints[iAhead].t - self._wayPoints[iAhead - 1].t
return DP * (1 / Dt)
def rotation(self, t=None):
"""
Args:
t (float, optional):
time [s]
Returns:
Value of self._rotations if t is None and if self._rotations is not
None. Otherwise the actual rotation is calculated as function of
time [rad]
Note:
The calculated rotation is NOT stored as 'self._rotation'
"""
if t is None and self._rotations is not None:
return self._rotations
if self._wayPoints is None:
return xyz()
if t is None or t < 0.:
t = 0.
if t >= self._wayPoints[-1].t:
return self._rotations[-1]
# R = R' + (R"-R') * (t-t') / (t"-t')
iAhead = self.iWayPointAhead(t)
DR = self._rotations[iAhead] - self._rotations[iAhead - 1]
dt = t - self._wayPoints[iAhead - 1].t
Dt = self._wayPoints[iAhead].t - self._wayPoints[iAhead - 1].t
return self._rotations[iAhead - 1] + DR * (dt / Dt)
def set_begin(self, identifier, C, E, N):
"""
Sets characteristic points (C, E, N) of initial cross section
Args:
identifier (string):
identifier of cross-section
C, E, N (xyz):
center, east and north point defining circle area
Returns:
Reference to cross section
"""
self.head = CrossSection(identifier)
self.head.C = xyz(C[0], C[1], C[2])
self.head.E = xyz(E[0], E[1], E[2])
self.head.N = xyz(N[0], N[1], N[2])
self.tail = self.head
return self.head
def initialCondition(self):
"""
Initializes object positionm velocity and rotation
"""
super().initialCondition()
if self._wayPoints is not None:
self._position = self._wayPoints[0]
self._velocity = self.velocity(0)
self._rotation = self.rotation(0)
else:
self._position = xyz()
self._velocity = xyz()
self._rotation = xyz()
if self.silent:
self._trajectoryHistory = None
else:
self._trajectoryHistory = \
[[self._position.x], [self._position.y], [self._position.z],
[self._velocity.x], [self._velocity.y], [self._velocity.z],
[self._rotation.x], [self._rotation.y], [self._rotation.z]]
angles='xy',
scale_units='xy')
plt.xlim(0, )
plt.xlabel('y')
plt.ylabel('z')
plt.grid()
plt.show()
# Examples ####################################################################
if __name__ == '__main__':
ALL = 1
# y
way = [
xyz(.0, .0, .0), # ^
xyz(.1, .1, .0), # +1| /\
xyz(.2, .2, .0), # | / \
xyz(.3, .1, .0), # | / \ 0.8
xyz(.4, .0, .0), # 0 |/----0.2-----\----0.6-----/-->
xyz(.5, -.1, .0), # | 0.4\ / x
xyz(.6, -.2, .0), # | \ /
xyz(.7, -.1, .0), # -1| \/
xyz(.8, .0, .0)
] # | trajectory W=W(t)
rot = [
xyz(20 * np.sin(i * 3), 4 * i - 20, i * i - 30)
for i in range(len(way))
]
print(len(rot), [str(rot[i]) for i in range(9)])