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