def cmdFullState(self, pos, vel, acc, yaw, omega):
self.mode = Crazyflie.MODE_LOW_FULLSTATE
self.setState.pos = firm.mkvec(*pos)
self.setState.vel = firm.mkvec(*vel)
self.setState.acc = firm.mkvec(*acc)
self.setState.yaw = yaw
self.setState.omega = firm.mkvec(*omega)
def _vposition(self):
# TODO this should be implemented in C
if self.planner.state == firm.TRAJECTORY_STATE_IDLE:
return self.planner.lastKnownPosition
else:
ev = firm.plan_current_goal(self.planner, self.time())
self.planner.lastKnownPosition = firm.mkvec(ev.pos.x, ev.pos.y, ev.pos.z)
# not totally sure why, but if we don't do this, we don't actually return by value
return firm.mkvec(ev.pos.x, ev.pos.y, ev.pos.z)
def goTo(self, goal, yaw, duration, relative=False, groupMask=0):
if self._isGroup(groupMask):
if self.mode != Crazyflie.MODE_HIGH_POLY:
# We need to update to the latest firmware that has go_to_from.
raise ValueError(
"goTo from low-level modes not yet supported.")
self.mode = Crazyflie.MODE_HIGH_POLY
firm.plan_go_to(self.planner, relative, firm.mkvec(*goal), yaw,
duration, self.time())
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 uploadTrajectory(self, trajectoryId, pieceOffset, trajectory):
traj = firm.piecewise_traj()
traj.t_begin = 0
traj.timescale = 1.0
traj.shift = firm.mkvec(0, 0, 0)
traj.n_pieces = len(trajectory.polynomials)
traj.pieces = firm.malloc_poly4d(len(trajectory.polynomials))
for i, poly in enumerate(trajectory.polynomials):
piece = firm.pp_get_piece(traj, i)
piece.duration = poly.duration
for coef in range(0, 8):
firm.poly4d_set(piece, 0, coef, poly.px.p[coef])
firm.poly4d_set(piece, 1, coef, poly.py.p[coef])
firm.poly4d_set(piece, 2, coef, poly.pz.p[coef])
firm.poly4d_set(piece, 3, coef, poly.pyaw.p[coef])
self.trajectories[trajectoryId] = traj
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)
def arr2vec(a):
return firm.mkvec(a[0], a[1], a[2])
def cmdVelocityWorld(self, vel, yawRate):
self.mode = Crazyflie.MODE_LOW_VELOCITY
self.setState.vel = firm.mkvec(*vel)
self.setState.omega = firm.mkvec(0.0, 0.0, yawRate)
def cmdPosition(self, pos, yaw=0):
self.mode = Crazyflie.MODE_LOW_POSITION
self.setState.pos = firm.mkvec(*pos)
self.setState.yaw = yaw
