def startTrajectory(self, trajectoryId, timescale = 1.0, reverse = False, relative = True, groupMask = 0):
if self._isGroup(groupMask):
traj = self.trajectories[trajectoryId]
# if traj is also current traj, must take pos before changing t_begin.
pos = self._vposition()
traj.t_begin = self.time()
traj.timescale = timescale
if relative:
traj.shift = firm.vzero()
if reverse:
traj_init = firm.piecewise_eval_reversed(traj, traj.t_begin)
else:
traj_init = firm.piecewise_eval(traj, traj.t_begin)
traj.shift = firm.vsub(pos, traj_init.pos)
else:
traj.shift = firm.vzero()
firm.plan_start_trajectory(self.planner, traj, reverse)
def startTrajectory(self,
trajectoryId,
timescale=1.0,
reverse=False,
relative=True,
groupMask=0):
if self._isGroup(groupMask):
self.mode = Crazyflie.MODE_HIGH_POLY
traj = self.trajectories[trajectoryId]
traj.t_begin = self.time()
traj.timescale = timescale
if relative:
traj.shift = firm.vzero()
if reverse:
traj_init = firm.piecewise_eval_reversed(
traj, traj.t_begin)
else:
traj_init = firm.piecewise_eval(traj, traj.t_begin)
traj.shift = firm.vsub(self.state.pos, traj_init.pos)
else:
traj.shift = firm.vzero()
firm.plan_start_trajectory(self.planner, traj, reverse)
def integrate(self, time, disturbanceSize):
if self.mode == Crazyflie.MODE_IDLE:
pass
elif self.mode == Crazyflie.MODE_HIGH_POLY:
self.state = firm.plan_current_goal(self.planner, self.time())
elif self.mode == Crazyflie.MODE_LOW_FULLSTATE:
self.state = self.setState
elif self.mode == Crazyflie.MODE_LOW_POSITION:
# Simple finite difference velocity approxmations.
velocity = firm.vdiv(firm.vsub(self.setState.pos, self.state.pos),
time)
yawRate = (self.setState.yaw - self.state.yaw) / time
self.state.pos = self.setState.pos
self.state.vel = velocity
self.state.acc = firm.vzero(
) # TODO: 2nd-order finite difference? Probably useless.
self.state.yaw = self.setState.yaw
self.state.omega = firm.mkvec(0.0, 0.0, yawRate)
elif self.mode == Crazyflie.MODE_LOW_VELOCITY:
# Simple Euler integration.
disturbance = firm.mkvec(*(disturbanceSize *
np.random.normal(size=3)))
velocity = firm.vadd(self.setState.vel, disturbance)
self.state.pos = firm.vadd(self.state.pos,
firm.vscl(time, velocity))
self.state.vel = velocity
self.state.acc = firm.vzero(
) # TODO: could compute with finite difference
self.state.yaw += time * self.setState.omega.z
self.state.omega = self.setState.omega
else:
raise ValueError("Unknown flight mode.")
def __init__(self, id, initialPosition, timeHelper):
# Core.
self.id = id
self.groupMask = 0
self.initialPosition = np.array(initialPosition)
self.time = lambda: timeHelper.time()
# Commander.
self.mode = Crazyflie.MODE_IDLE
self.planner = firm.planner()
firm.plan_init(self.planner)
self.trajectories = dict()
self.setState = firm.traj_eval()
# State. Public np.array-returning getters below for physics state.
self.state = firm.traj_eval()
self.state.pos = firm.mkvec(*initialPosition)
self.state.vel = firm.vzero()
self.state.acc = firm.vzero()
self.state.yaw = 0.0
self.state.omega = firm.vzero()
self.ledRGB = (0.5, 0.5, 1)
