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 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 acceleration(self): if self.planner.state == firm.TRAJECTORY_STATE_IDLE: return np.array([0, 0, 0]) else: ev = firm.plan_current_goal(self.planner, self.time()) return np.array([ev.acc.x, ev.acc.y, ev.acc.z])
def yaw(self): ev = firm.plan_current_goal(self.planner, self.time()) if not firm.is_traj_eval_valid(ev): return 0.0 return ev.yaw
def yaw(self): ev = firm.plan_current_goal(self.planner, self.time()) return ev.yaw