RLBench/envs/_quadrocopter/quadrotor_dynamics.py
def _determine_forces(self, p_des, q_des, r_des, c_des):
        """Convert desired angular rates and thrust to rotor forces."""
        L = self.params.L
        K = self.params.K
        m = self.params.m

        a = np.array(((0, L, 0, -L),
                      (-L, 0, L, 0),
                      (K, -K, K, -K),
                      (1 / m, 1 / m, 1 / m, 1 / m)),
                     dtype=np.float64)

        # The inertial matrix
        j = np.diag((self.params.Ix, self.params.Iy, self.params.Iz))

        # The current angular velocity vector
        omega = self.state.omega

        # The rate vector (our approximation of omega_dot)
        rate_vector = np.array(
            (((1 / self.params.tau_p) * (p_des - self.state.omega[0])),
             ((1 / self.params.tau_q) * (q_des - self.state.omega[1])),
             ((1 / self.params.tau_r) * (r_des - self.state.omega[2])))).T

        b = j.dot(rate_vector) + np.cross(omega, j.dot(omega))

        # Add c_des to the bottom of the row vector
        b = np.concatenate((b, [c_des]))

        # Return the four rotor forces
        return np.linalg.solve(a, b)
Convert desired angular rates and thrust to rotor forces.
Convert desired angular rates and thrust to rotor forces.
Convert desired angular rates and thrust to rotor forces .
def _determine_forces(self, p_des, q_des, r_des, c_des):
        L = self.params.L
        K = self.params.K
        m = self.params.m
        a = np.array(((0, L, 0, -L),
                      (-L, 0, L, 0),
                      (K, -K, K, -K),
                      (1 / m, 1 / m, 1 / m, 1 / m)),
                     dtype=np.float64)
        j = np.diag((self.params.Ix, self.params.Iy, self.params.Iz))
        omega = self.state.omega
        rate_vector = np.array(
            (((1 / self.params.tau_p) * (p_des - self.state.omega[0])),
             ((1 / self.params.tau_q) * (q_des - self.state.omega[1])),
             ((1 / self.params.tau_r) * (r_des - self.state.omega[2])))).T
        b = j.dot(rate_vector) + np.cross(omega, j.dot(omega))
        b = np.concatenate((b, [c_des]))
        return np.linalg.solve(a, b)
def _determine_forces ( self , p_des , q_des , r_des , c_des ) : L = self . params . L K = self . params . K m = self . params . m a = np . array ( ( ( 0 , L , 0 , - L ) , ( - L , 0 , L , 0 ) , ( K , - K , K , - K ) , ( 1 / m , 1 / m , 1 / m , 1 / m ) ) , dtype = np . float64 ) j = np . diag ( ( self . params . Ix , self . params . Iy , self . params . Iz ) ) omega = self . state . omega rate_vector = np . array ( ( ( ( 1 / self . params . tau_p ) * ( p_des - self . state . omega [ 0 ] ) ) , ( ( 1 / self . params . tau_q ) * ( q_des - self . state . omega [ 1 ] ) ) , ( ( 1 / self . params . tau_r ) * ( r_des - self . state . omega [ 2 ] ) ) ) ) . T b = j . dot ( rate_vector ) + np . cross ( omega , j . dot ( omega ) ) b = np . concatenate ( ( b , [ c_des ] ) ) return np . linalg . solve ( a , b )
Convert desired angular rates and thrust to rotor forces.
Convert desired angular rates and thrust to rotor forces .
"""Convert desired angular rates and thrust to rotor forces.""" # The inertial matrix # The current angular velocity vector # The rate vector (our approximation of omega_dot) # Add c_des to the bottom of the row vector # Return the four rotor forces
self
p_des
q_des
r_des
c_des