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
