Exemplo n.º 1
0
    def dynamics(self, state, control_input):
        """ returns xdot """

        [x,y,z,roll,pitch,yaw,dx,dy,dz,omegax,omegay,omegaz] = state
        [w1,w2,w3,w4] = control_input

        w1 = min(max(self.u_min[0],w1),self.u_max[0])
        w2 = min(max(self.u_min[1],w2),self.u_max[1])
        w3 = min(max(self.u_min[2],w3),self.u_max[2])
        w4 = min(max(self.u_min[3],w4),self.u_max[3])

        F1 = self.Kf*w1
        F2 = self.Kf*w2
        F3 = self.Kf*w3
        F4 = self.Kf*w4
        M1 = self.Km*w1
        M2 = self.Km*w2
        M3 = self.Km*w3
        M4 = self.Km*w4

        [ddx,ddy,ddz] = (1/self.m)*(array([0,0,-self.m*9.81]) + array(body2world([roll,pitch,yaw],[0,0,F1+F2+F3+F4])))
        [alphax,alphay,alphaz] = dot(self.invI,(array([self.L*(F4-F2),self.L*(F3-F1),(M2+M4-M1-M3)])-cross([omegax,omegay,omegaz],dot(self.I,[omegax,omegay,omegaz]))))

        [droll,dpitch,dyaw] = angularvel2rpydot([roll,pitch,yaw],[omegax,omegay,omegaz])

        return [dx,dy,dz,droll,dpitch,dyaw,ddx,ddy,ddz,alphax,alphay,alphaz]
Exemplo n.º 2
0
    def dynamics(self, state, control_input):
        """ returns xdot """

        [x, y, z, roll, pitch, yaw, dx, dy, dz, omegax, omegay, omegaz] = state
        [w1, w2, w3, w4] = control_input

        w1 = min(max(self.u_min[0], w1), self.u_max[0])
        w2 = min(max(self.u_min[1], w2), self.u_max[1])
        w3 = min(max(self.u_min[2], w3), self.u_max[2])
        w4 = min(max(self.u_min[3], w4), self.u_max[3])

        F1 = self.Kf * w1
        F2 = self.Kf * w2
        F3 = self.Kf * w3
        F4 = self.Kf * w4
        M1 = self.Km * w1
        M2 = self.Km * w2
        M3 = self.Km * w3
        M4 = self.Km * w4

        [ddx, ddy,
         ddz] = (1 / self.m) * (array([0, 0, -self.m * 9.81]) + array(
             body2world([roll, pitch, yaw], [0, 0, F1 + F2 + F3 + F4])))
        [alphax, alphay, alphaz
         ] = dot(self.invI, (array(
             [self.L * (F4 - F2), self.L * (F3 - F1), (M2 + M4 - M1 - M3)]) -
                             cross([omegax, omegay, omegaz],
                                   dot(self.I, [omegax, omegay, omegaz]))))

        [droll, dpitch, dyaw] = angularvel2rpydot([roll, pitch, yaw],
                                                  [omegax, omegay, omegaz])

        return [
            dx, dy, dz, droll, dpitch, dyaw, ddx, ddy, ddz, alphax, alphay,
            alphaz
        ]
Exemplo n.º 3
0
import transforms

if __name__=='__main__':
	rpy = [1.0, 2.0, 3.0]
	gyro = [1.5, 1.0, 1.5]
	rpydot = transforms.angularvel2rpydot(rpy, transforms.body2world(rpy, gyro))
	rpydotexpected = [-2.109520760384187, -0.721904171343705, -3.969571070914463]
	for i in range(3):
		assert (rpydotexpected[i]-rpydot[i])<1E-6
Exemplo n.º 4
0
	def get_xhat(self):
		if self._use_kalman:
			dt = time.time() - self._last_kalman_update
			self._kalman.predict(np.array(self._last_rpy + self._last_acc), dt)
			if self._valid_vicon:
				self._kalman.update(np.array(self._last_xyz))
			self._last_kalman_update = time.time()
			kalman_xhat = self._kalman.x.reshape(9).tolist()
			xhat = [kalman_xhat[0],kalman_xhat[1],kalman_xhat[2],
					self._last_rpy[0],self._last_rpy[1],self._last_rpy[2],
					kalman_xhat[3],kalman_xhat[4],kalman_xhat[5],
					self._last_gyro[0],self._last_gyro[1],self._last_gyro[2]]

			# msg = dxyz_compare_t()
			# msg.dxyzraw = self._last_dxyz
			# msg.dxyzfiltered = kalman_xhat[3:6]
			# self._xhat_lc.publish('dxyz_compare', msg.encode())

			msg = kalman_args_t()
			msg.input_rpy = self._last_rpy
			msg.input_acc = self._last_acc
			msg.input_dt = dt
			msg.valid_vicon = self._valid_vicon
			msg.meas_xyz = self._last_xyz
			msg.smooth_xyz = self._last_xyz
			msg.smooth_dxyz =self._last_dxyz
			self._xhat_lc.publish('kalman_args', msg.encode())

		else:
			xhat = [self._last_xyz[0],self._last_xyz[1],self._last_xyz[2],
					self._last_rpy[0],self._last_rpy[1],self._last_rpy[2],
					self._last_dxyz[0],self._last_dxyz[1],self._last_dxyz[2],
					self._last_gyro[0],self._last_gyro[1],self._last_gyro[2]]

		if self._delay_comp:
			control_inputs = self.get_last_inputs(self._delay)
			for ci in control_inputs:
				xhat = self._cf_model.simulate(xhat,ci[0],ci[1])

		if self._use_rpydot:
			try:
				xhat[9:12] = angularvel2rpydot(self._last_rpy, body2world(self._last_rpy, self._last_gyro))
			except ValueError:
				xhat[9:12] = [0.0, 0.0, 0.0]

		if self._publish_to_lcm:
			msg = crazyflie_state_estimate_t()
			msg.xhat = xhat
			msg.t = self.get_time()
			self._xhat_lc.publish("crazyflie_state_estimate", msg.encode())

		# uncomment the following if you want to trigger hover at a predefined state
	#	if xhat[2] <= 0.15:
	#		print('This is how low')
	#		msg = crazyflie_controller_commands_t()
	#		msg.is_running = False
	#		self._xhat_lc.publish('crazyflie_controller_commands', msg.encode())
		# 	msg = crazyflie_hover_commands_t()
		# 	msg.hover = True
		# 	self._xhat_lc.publish('crazyflie_hover_commands',msg.encode())

		return xhat
Exemplo n.º 5
0
import transforms

if __name__ == '__main__':
    rpy = [1.0, 2.0, 3.0]
    gyro = [1.5, 1.0, 1.5]
    rpydot = transforms.angularvel2rpydot(rpy,
                                          transforms.body2world(rpy, gyro))
    rpydotexpected = [
        -2.109520760384187, -0.721904171343705, -3.969571070914463
    ]
    for i in range(3):
        assert (rpydotexpected[i] - rpydot[i]) < 1E-6