RLBench/envs/_quadrocopter/quadrotor_dynamics.py
def _inputs_to_desired_rates(self, pitch, roll, z_vel, yaw_vel):
        """Convert inputs to desired angular rates and thrust."""
        # Current roll, and yaw angles
        roll_cur, _, yaw_cur = self.state.rpy

        # r_des is simply the commanded yaw rate
        r_des = yaw_vel

        # calculate the commanded acceleration in the z direction,
        # (z_dot_des - z_dot) / tau_z
        z_ddot_des = (z_vel - self.state.vel[2]) / self.params.tau_Iz

        # And from this we may find the commanded thrust, (g + z_ddot_cmd)/R33
        c_des = (self.params.g + z_ddot_des) / self.state.R[2, 2]

        # Calculate the commanded yaw angle from:
        yaw_des = yaw_vel * self.params.tau_Iyaw + yaw_cur

        # R13_des = sin(yaw_des) * sin(roll_cmd)
        #         + cos(yaw_des) * cos(roll_cmd) * sin(pitch_cmd)
        r_13_des = (np.sin(yaw_des) * np.sin(roll) +
                    np.cos(yaw_des) * np.cos(roll) * np.sin(pitch))

        # R23_des = cos(roll_cmd) * sin(yaw_des) * sin(pitch_cmd)
        #         - cos(yaw_des) * sin(roll_cmd)
        r_23_des = (np.cos(roll) * np.sin(yaw_des) * np.sin(pitch) -
                    np.cos(yaw_des) * np.sin(roll))

        # p_des = (R21*(R13_des-R13) - R11*(R23_des-R23))/(R33*tau_rp)
        p_des = (self.state.R[1, 0] * (r_13_des - self.state.R[0, 2]) -
                 self.state.R[0, 0] * (r_23_des - self.state.R[1, 2]))
        p_des /= self.state.R[2, 2] * self.params.tau_rp

        # q_des = (R22*(R13_des-R13) - R12*(R23_des-R23))/(R33*tau_rp)
        q_des = (self.state.R[1, 1] * (r_13_des - self.state.R[0, 2]) -
                 self.state.R[0, 1] * (r_23_des - self.state.R[1, 2]))
        q_des /= self.state.R[2, 2] * self.params.tau_rp

        # Return everything!
        return p_des, q_des, r_des, c_des
Convert inputs to desired angular rates and thrust.
Convert inputs to desired angular rates and thrust.
Convert inputs to desired angular rates and thrust .
def _inputs_to_desired_rates(self, pitch, roll, z_vel, yaw_vel):
        roll_cur, _, yaw_cur = self.state.rpy
        r_des = yaw_vel
        z_ddot_des = (z_vel - self.state.vel[2]) / self.params.tau_Iz
        c_des = (self.params.g + z_ddot_des) / self.state.R[2, 2]
        yaw_des = yaw_vel * self.params.tau_Iyaw + yaw_cur
        r_13_des = (np.sin(yaw_des) * np.sin(roll) +
                    np.cos(yaw_des) * np.cos(roll) * np.sin(pitch))
        r_23_des = (np.cos(roll) * np.sin(yaw_des) * np.sin(pitch) -
                    np.cos(yaw_des) * np.sin(roll))
        p_des = (self.state.R[1, 0] * (r_13_des - self.state.R[0, 2]) -
                 self.state.R[0, 0] * (r_23_des - self.state.R[1, 2]))
        p_des /= self.state.R[2, 2] * self.params.tau_rp
        q_des = (self.state.R[1, 1] * (r_13_des - self.state.R[0, 2]) -
                 self.state.R[0, 1] * (r_23_des - self.state.R[1, 2]))
        q_des /= self.state.R[2, 2] * self.params.tau_rp
        return p_des, q_des, r_des, c_des
def _inputs_to_desired_rates ( self , pitch , roll , z_vel , yaw_vel ) : roll_cur , _ , yaw_cur = self . state . rpy r_des = yaw_vel z_ddot_des = ( z_vel - self . state . vel [ 2 ] ) / self . params . tau_Iz c_des = ( self . params . g + z_ddot_des ) / self . state . R [ 2 , 2 ] yaw_des = yaw_vel * self . params . tau_Iyaw + yaw_cur r_13_des = ( np . sin ( yaw_des ) * np . sin ( roll ) + np . cos ( yaw_des ) * np . cos ( roll ) * np . sin ( pitch ) ) r_23_des = ( np . cos ( roll ) * np . sin ( yaw_des ) * np . sin ( pitch ) - np . cos ( yaw_des ) * np . sin ( roll ) ) p_des = ( self . state . R [ 1 , 0 ] * ( r_13_des - self . state . R [ 0 , 2 ] ) - self . state . R [ 0 , 0 ] * ( r_23_des - self . state . R [ 1 , 2 ] ) ) p_des /= self . state . R [ 2 , 2 ] * self . params . tau_rp q_des = ( self . state . R [ 1 , 1 ] * ( r_13_des - self . state . R [ 0 , 2 ] ) - self . state . R [ 0 , 1 ] * ( r_23_des - self . state . R [ 1 , 2 ] ) ) q_des /= self . state . R [ 2 , 2 ] * self . params . tau_rp return p_des , q_des , r_des , c_des
Convert inputs to desired angular rates and thrust.
Convert inputs to desired angular rates and thrust .
"""Convert inputs to desired angular rates and thrust.""" # Current roll, and yaw angles # r_des is simply the commanded yaw rate # calculate the commanded acceleration in the z direction, # (z_dot_des - z_dot) / tau_z # And from this we may find the commanded thrust, (g + z_ddot_cmd)/R33 # Calculate the commanded yaw angle from: # R13_des = sin(yaw_des) * sin(roll_cmd) #         + cos(yaw_des) * cos(roll_cmd) * sin(pitch_cmd) # R23_des = cos(roll_cmd) * sin(yaw_des) * sin(pitch_cmd) #         - cos(yaw_des) * sin(roll_cmd) # p_des = (R21*(R13_des-R13) - R11*(R23_des-R23))/(R33*tau_rp) # q_des = (R22*(R13_des-R13) - R12*(R23_des-R23))/(R33*tau_rp) # Return everything!
self
pitch
roll
z_vel
yaw_vel